HiepLM/AMR_T800
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

add apm

Browse Source
This commit is contained in:
HiepLM
2026-02-26 09:48:16 +07:00
parent e8d5980572
commit 148a5e2c60
158 changed files with 9014 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
Controllers/Packages/cititruck_description/urdf/include/check_xacro_order.sh Executable file
View File

@@ -0,0 +1,3 @@
rosrun xacro xacro cititruck.xacro > /tmp/old.xml
rosrun xacro xacro --inorder cititruck.xacro > /tmp/new.xml
diff /tmp/old.xml /tmp/new.xml

74
Controllers/Packages/cititruck_description/urdf/include/cititruck.gazebo Executable file
View File

@@ -0,0 +1,74 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro">
<!-- Arguments used in cititruck.gazebo -->
<xacro:arg name="controller_prefix" default=""/>
<xacro:arg name="sensor_prefix" default=""/>
<xacro:arg name="sick_name" default="safety"/>
<xacro:arg name="ls2000_name" default="nav"/>
<xacro:arg name="kinect2_name" default=""/>
<xacro:arg name="kinect1_name" default="kinect"/>
<xacro:arg name="use_gpu" default="true"/>
<!-- ros_control plugin -->
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
<robotNamespace>/cititruck</robotNamespace>
</plugin>
</gazebo>
<gazebo>
<plugin name="steer_drive_controller" filename="libgazebo_ros_steer_drive.so">
<legacyMode>false</legacyMode>
<alwaysOn>true</alwaysOn>
<updateRate>100</updateRate>
<steerJoint>base2steer_joint</steerJoint>
<driveJoint>steer2sd_wheel_joint</driveJoint>
<fixedWheelLeftJoint>base2fixed_left_wheel_joint</fixedWheelLeftJoint>
<fixedWheelRightJoint>base2fixed_right_wheel_joint</fixedWheelRightJoint>
<wheelDiameter>0.16</wheelDiameter>
<steeringFixWheelDistanceX>1.190</steeringFixWheelDistanceX>
<steeringFixWheelDistanceY>0.0</steeringFixWheelDistanceY>
<steerTorque>1000</steerTorque>
<driveTorque>1000</driveTorque>
<commandTopic>$(arg controller_prefix)cmd_vel</commandTopic>
<odometryTopic>odom</odometryTopic>
<odometryFrame>/world</odometryFrame>
<robotBaseFrame>base_footprint</robotBaseFrame>
<odomEncSteeringAngleOffset>0.01</odomEncSteeringAngleOffset>
<!--odometrySource>encoder</odometrySource-->
<publishWheelJointState>true</publishWheelJointState>
</plugin>
</gazebo>
<gazebo reference="base_link" >
<material>Gazebo/Red</material>
</gazebo>
<gazebo reference="left_fork" >
<material>Gazebo/Grey</material>
</gazebo>
<gazebo reference="right_fork" >
<material>Gazebo/Grey</material>
</gazebo>
<gazebo reference="fixed_left_wheel_link" >
<material>Gazebo/Black</material>
</gazebo>
<gazebo reference="fixed_right_wheel_link" >
<material>Gazebo/Black</material>
</gazebo>
<gazebo reference="steerdrive_wheel_link" >
<material>Gazebo/Black</material>
</gazebo>
</robot>

67
Controllers/Packages/cititruck_description/urdf/include/cititruck.gazebo.xacro Executable file
View File

@@ -0,0 +1,67 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro">
<xacro:macro name="steer_controller_plugin_gazebo" params="prefix steer_joint steerdrive_wheel_joint wheel_radius">
<gazebo>
<plugin name="steer_drive_controller" filename="libgazebo_ros_steer_drive.so">
<alwaysOn>true</alwaysOn>
<updateRate>100</updateRate>
<steerJoint>${prefix}base2steer_joint</steerJoint>
<driveJoint>${prefix}steer2sd_wheel_joint</driveJoint>
<fixedWheelLeftJoint>${prefix}fixed_left_wheel_joint</fixedWheelLeftJoint>
<fixedWheelRightJoint>${prefix}fixed_right_wheel_joint</fixedWheelRightJoint>
<wheelDiameter>1.05</wheelDiameter>
<steeringFixWheelDistanceX>1.1</steeringFixWheelDistanceX>
<steeringFixWheelDistanceY>0.0</steeringFixWheelDistanceY>
<steerTorque>1000</steerTorque>
<driveTorque>1000</driveTorque>
<commandTopic>mobile_base_controller/cmd_vel</commandTopic>
<odometryTopic>mobile_base_controller/odom</odometryTopic>
<odometryFrame>world</odometryFrame>
<robotBaseFrame>base_footprint</robotBaseFrame>
<odomEncSteeringAngleOffset>0.01</odomEncSteeringAngleOffset>
<!--odometrySource>encoder</odometrySource-->
<publishWheelTF>false</publishWheelTF>
<publishWheelJointState>true</publishWheelJointState>
</plugin>
</gazebo>
</xacro:macro>
<xacro:macro name="set_wheel_friction" params="link friction">
<gazebo reference="${link}">
<mu1 value="${friction}"/>
<mu2 value="${friction}"/>
<kp value="10000000.0"/>
<kd value="1.0"/>
<minDepth>0.01</minDepth>
</gazebo>
</xacro:macro>
<xacro:macro name="set_all_wheel_frictions" params="prefix">
<xacro:set_wheel_friction link="${prefix}steer_link" friction="200"/>
<xacro:set_wheel_friction link="${prefix}drive_wheel_link" friction="200"/>
<xacro:set_wheel_friction link="${prefix}fixed_wheel_left_link" friction="1"/>
<xacro:set_wheel_friction link="${prefix}fixed_wheel_right_link" friction="1"/>
</xacro:macro>
<xacro:macro name="p3d_base_controller" params="prefix">
<gazebo>
<plugin name="p3d_base_controller" filename="libgazebo_ros_p3d.so">
<alwaysOn>true</alwaysOn>
<updateRate>50.0</updateRate>
<bodyName>${prefix}base_footprint</bodyName>
<topicName>base_pose_ground_truth</topicName>
<gaussianNoise>0.01</gaussianNoise>
<frameName>map</frameName>
<xyzOffsets>0 0 0</xyzOffsets>
<rpyOffsets>0 0 0</rpyOffsets>
</plugin>
</gazebo>
</xacro:macro>
</robot>

30
Controllers/Packages/cititruck_description/urdf/include/cititruck.transmission.xacro Executable file
View File

@@ -0,0 +1,30 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro">
<xacro:macro name="cititruck_wheel_transmission" params="prefix locationprefix">
<transmission name="${prefix}${locationprefix}_wheel_trans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${prefix}${locationprefix}_wheel_joint">
<hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
</joint>
<actuator name="${prefix}${locationprefix}_wheel_motor">
<hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
</xacro:macro>
<xacro:macro name="cititruck_steer_transmission" params="prefix locationprefix">
<transmission name="${prefix}${locationprefix}_wheel_trans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${prefix}${locationprefix}_joint">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
</joint>
<actuator name="${prefix}${locationprefix}_motor">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
</xacro:macro>
</robot>

251
Controllers/Packages/cititruck_description/urdf/include/cititruck_v1.urdf.xacro Executable file
View File

@@ -0,0 +1,251 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro">
<xacro:arg name="robot_type" default="demo" />
<xacro:property name="robot_type" value="$(arg robot_type)" /> <!-- necessary because args cannot be accessed inside ${} expressions -->
<xacro:include filename="$(find cititruck_description)/urdf/include/common_properties.urdf.xacro" />
<xacro:include filename="$(find cititruck_description)/urdf/include/cititruck.gazebo.xacro" />
<xacro:include filename="$(find cititruck_description)/urdf/include/cititruck.transmission.xacro" />
<xacro:include filename="$(find cititruck_description)/urdf/include/imu.gazebo.urdf.xacro" />
<xacro:include filename="$(find cititruck_description)/urdf/include/sick_s300.urdf.xacro" />
<xacro:property name="deg_to_rad" value="0.017453293" />
<!-- The inertia for the MiR platform is intentionally chosen to be smaller than
the bounding box and also shifted a bit to the back, because most of the mass
is in the lower center back (because of the batteries). -->
<xacro:macro name="actuated_wheel" params="prefix locationprefix locationright">
<joint name="fixed_${locationprefix}_wheel_joint" type="continuous">
<parent link="${prefix}base_link"/>
<child link="${prefix}fixed_wheel_${locationprefix}_link"/>
<origin xyz="${locationright}" rpy="0 0 0" />
<axis xyz="0 1 0" />
</joint>
<link name="${prefix}fixed_wheel_${locationprefix}_link">
<inertial>
<mass value="1"/>
<inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
<origin/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="1.57 0 0" />
<geometry>
<cylinder radius="0.05" length="0.08" />
</geometry>
<xacro:insert_block name="material_black" />
</visual>
<collision>
<origin xyz="0 0 0" rpy="${pi/2} 0 0" />
<geometry>
<cylinder radius="0.04" length="0.08" />
</geometry>
</collision>
</link>
<gazebo reference="${prefix}fixed_wheel_${locationprefix}_link">
<material>Gazebo/FlatBlack</material>
</gazebo>
</xacro:macro>
<xacro:macro name="fork" params="locationprefix locationright">
<xacro:box_inertial mass="5" x="1" y="0" z="0">
<origin xyz="0 0 0" rpy="0 0 0" />
</xacro:box_inertial>
<visual>
<origin xyz="${locationright}" rpy="0 0 0" />
<geometry>
<box size="1.35 0.15 0.04" />
</geometry>
<xacro:insert_block name="material_grey" />
</visual>
</xacro:macro>
<!-- robot -->
<xacro:macro name="cititruck" params="prefix">
<link name="${prefix}base_footprint" />
<joint name="${prefix}base_joint" type="fixed">
<parent link="${prefix}base_footprint" />
<child link="${prefix}base_link" />
<origin xyz="0 0 0" rpy="0 0 0" />
</joint>
<!-- Base Link -->
<link name="${prefix}base_link">
<xacro:box_inertial mass="30" x="1" y="0" z="0">
<origin xyz="0 0 0" rpy="0 0 0" />
</xacro:box_inertial>
<visual>
<origin xyz="${1.3268 - 0.21 + 0.4/2} 0 ${0.8/2 + 0.3337 }" rpy="0 0 0" />
<geometry>
<box size="0.40 0.65 0.80" />
</geometry>
<xacro:insert_block name="material_white" />
</visual>
<visual>
<origin xyz="${1.3268 - 0.21 + 0.4/2} 0 ${0.3 - 0.01}" rpy="0 0 0" />
<geometry>
<box size="0.40 0.65 0.02" />
</geometry>
<xacro:insert_block name="material_grey" />
</visual>
<visual>
<origin xyz="${1.3268 - 0.21 + 0.02/2} 0 ${0.3 - (0.3 - 0.01)/2}" rpy="0 0 0" />
<geometry>
<box size="0.02 0.65 ${0.3 - 0.01}" />
</geometry>
<xacro:insert_block name="material_grey" />
</visual>
<collision>
<origin xyz="${1.3268 - 0.21 + 0.4/2} 0 ${0.8/2 + 0.3337 }" rpy="0 0 0" />
<geometry>
<box size="0.40 0.70 0.80" />
</geometry>
</collision>
<xacro:fork locationprefix="left" locationright="${1.12 - 1.35/2} 0.205 ${0.05/2 + 0.3 - 0.29}"/>
<xacro:fork locationprefix="right" locationright="${1.12 - 1.35/2} -0.205 ${0.05/2 + 0.3 - 0.29}"/>
</link>
<gazebo reference="${prefix}base_link">
<material>Gazebo/White</material>
</gazebo>
<!-- <xacro:fork prefix="${prefix}" locationprefix="left" parent="base_link" locationright="${0.9 - 1.15/2} 0.205 ${0.05/2 + 0.3 - 0.29}"/> -->
<!-- <xacro:fork prefix="${prefix}" locationprefix="right" parent="base_link" locationright="${0.9 - 1.15/2} -0.205 ${0.05/2 + 0.3 - 0.29}"/> -->
<joint name="${prefix}base2steer_joint" type="revolute">
<parent link="${prefix}base_link"/>
<child link="${prefix}steer_link"/>
<limit effort="10000.0" lower="-1.8" upper="1.8" velocity="2500"/>
<origin xyz="1.3268 0 0.125" rpy="0 0 0" />
<axis xyz="0 0 1" />
</joint>
<xacro:cititruck_steer_transmission prefix="${prefix}" locationprefix="base2steer"/>
<link name="${prefix}steer_link">
<inertial>
<mass value="45"/>
<inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001" />
<origin/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="1.57 0 0" />
<geometry>
<cylinder radius="0.09" length="0.1" />
</geometry>
<xacro:insert_block name="material_dark_grey" />
</visual>
<collision>
<origin xyz="0 0 0" rpy="1.57 0 0" />
<geometry>
<cylinder radius="0.09" length="0.1" />
</geometry>
</collision>
</link>
<gazebo reference="${prefix}steer_link" >
<material>Gazebo/FlatBlack</material>
</gazebo>
<link name="${prefix}steer_link_2" />
<joint name="${prefix}steer2steer_2_joint" type="fixed">
<parent link="${prefix}steer_link" />
<child link="${prefix}steer_link_2" />
<origin xyz="0 0 1.02" rpy="0 0 0" />
</joint>
<joint name="${prefix}steer2sd_wheel_joint" type="continuous">
<parent link="${prefix}steer_link"/>
<child link="${prefix}drive_wheel_link"/>
<limit effort="10000.0" velocity="100"/>
<origin xyz="0 0 0" rpy="0 0 0" />
<axis xyz="0 1 0" />
</joint>
<xacro:cititruck_wheel_transmission prefix="${prefix}" locationprefix="steer2sd"/>
<link name="${prefix}drive_wheel_link">
<inertial>
<mass value="10"/>
<inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
<origin/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="1.57 0 0" />
<geometry>
<cylinder radius="0.125" length="0.08" />
</geometry>
<xacro:insert_block name="material_yellow" />
</visual>
<collision>
<origin xyz="0 0 0" rpy="1.57 0 0" />
<geometry>
<cylinder radius="0.125" length="0.08" />
</geometry>
</collision>
</link>
<gazebo reference="${prefix}drive_wheel_link" >
<material>Gazebo/FlatBlack</material>
</gazebo>
<xacro:actuated_wheel prefix="${prefix}" locationprefix="left" locationright="0.0 0.205 ${0.05}"/>
<xacro:actuated_wheel prefix="${prefix}" locationprefix="right" locationright="0.0 -0.205 ${0.05}" />
<!-- IMU -->
<joint name="${prefix}base_link_to_imu_joint" type="fixed">
<parent link="${prefix}base_link" />
<child link="${prefix}imu_link" />
<origin xyz="${1.3268 -0.15} 0.0 0.3" rpy="0 0 0" /> <!-- same as real MiR -->
</joint>
<link name="${prefix}imu_link" />
<xacro:imu_gazebo link="${prefix}imu_link" imu_topic="imu_data" update_rate="50.0" />
<!-- Create an alias for imu_link. This is necessary because the real MiR's
TF has imu_link, but the imu_data topic is published in the imu_frame
frame. -->
<joint name="${prefix}imu_link_to_imu_frame_joint" type="fixed">
<parent link="${prefix}imu_link" />
<child link="${prefix}imu_frame" />
<origin xyz="0 0 0" rpy="0 0 0" />
</joint>
<link name="${prefix}imu_frame" />
<!-- Laser scanners -->
<joint name="${prefix}base_link_to_left_laser_joint" type="fixed">
<parent link="${prefix}base_link" />
<child link="${prefix}left_laser_link" />
<origin xyz="1.5447 0.2953 0.1743" rpy="0.0 0.0 ${0.25 * pi}" />
</joint>
<xacro:sick_s300 prefix="${prefix}" link="left_laser_link" topic="l_scan" />
<joint name="${prefix}base_link_to_right_laser_joint" type="fixed">
<parent link="${prefix}base_link" />
<child link="${prefix}right_laser_link" />
<origin xyz="1.5447 -0.2953 0.1743" rpy="0.0 0.0 ${-0.25 * pi}" />
</joint>
<xacro:sick_s300 prefix="${prefix}" link="right_laser_link" topic="r_scan" />
<!-- set the gazebo friction parameters for the wheels -->
<xacro:set_all_wheel_frictions prefix="${prefix}"/>
<xacro:p3d_base_controller prefix="${prefix}" />
</xacro:macro>
</robot>

15
Controllers/Packages/cititruck_description/urdf/include/common.gazebo.xacro Executable file
View File

@@ -0,0 +1,15 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro">
<xacro:macro name="controller_plugin_gazebo" params="robot_namespace">
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
<xacro:unless value="${robot_namespace == ''}">
<robotNamespace>${robot_namespace}</robotNamespace>
</xacro:unless>
<controlPeriod>0.001</controlPeriod>
<legacyModeNS>false</legacyModeNS>
</plugin>
</gazebo>
</xacro:macro>
</robot>

84
Controllers/Packages/cititruck_description/urdf/include/common_properties.urdf.xacro Executable file
View File

@@ -0,0 +1,84 @@
<?xml version="1.0" ?>
<!--
Various useful properties such as constants and materials
-->
<robot name="xacro_properties" xmlns:xacro="http://ros.org/wiki/xacro">
<!-- #################### RViz materials #################### -->
<xacro:property name="material_white">
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</xacro:property>
<xacro:property name="material_yellow">
<material name="yellow">
<color rgba="${255/255} ${226/255} ${0/255} 1"/>
</material>
</xacro:property>
<xacro:property name="material_almost_white">
<material name="almost_white">
<color rgba="0.9 0.9 0.9 1" />
</material>
</xacro:property>
<xacro:property name="material_grey">
<material name="grey">
<color rgba="0.5 0.5 0.5 1"/>
</material>
</xacro:property>
<xacro:property name="material_light_grey">
<material name="light_grey">
<color rgba="0.6 0.6 0.6 1"/>
</material>
</xacro:property>
<xacro:property name="material_dark_grey">
<material name="dark_grey">
<color rgba="0.3 0.3 0.3 1"/>
</material>
</xacro:property>
<xacro:property name="material_black">
<material name="black">
<color rgba="0.1 0.1 0.1 1"/>
</material>
</xacro:property>
<xacro:property name="material_aluminum">
<material name="aluminum">
<color rgba="0.8 0.8 0.8 1" />
</material>
</xacro:property>
<xacro:property name="material_silver">
<material name="silver">
<color rgba="0.79 0.82 0.93 1" />
</material>
</xacro:property>
<!-- #################### inertials with origin #################### -->
<!-- see https://secure.wikimedia.org/wikipedia/en/wiki/List_of_moment_of_inertia_tensors -->
<xacro:macro name="sphere_inertial" params="radius mass *origin">
<inertial>
<mass value="${mass}" />
<xacro:insert_block name="origin" />
<inertia ixx="${0.4 * mass * radius * radius}" ixy="0.0" ixz="0.0"
iyy="${0.4 * mass * radius * radius}" iyz="0.0"
izz="${0.4 * mass * radius * radius}" />
</inertial>
</xacro:macro>
<xacro:macro name="cylinder_inertial" params="radius length mass *origin">
<inertial>
<mass value="${mass}" />
<xacro:insert_block name="origin" />
<inertia ixx="${0.0833333 * mass * (3 * radius * radius + length * length)}" ixy="0.0" ixz="0.0"
iyy="${0.0833333 * mass * (3 * radius * radius + length * length)}" iyz="0.0"
izz="${0.5 * mass * radius * radius}" />
</inertial>
</xacro:macro>
<xacro:macro name="box_inertial" params="x y z mass *origin">
<inertial>
<mass value="${mass}" />
<xacro:insert_block name="origin" />
<inertia ixx="${0.0833333 * mass * (y*y + z*z)}" ixy="0.0" ixz="0.0"
iyy="${0.0833333 * mass * (x*x + z*z)}" iyz="0.0"
izz="${0.0833333 * mass * (x*x + y*y)}" />
</inertial>
</xacro:macro>
</robot>

31
Controllers/Packages/cititruck_description/urdf/include/imu.gazebo.urdf.xacro Executable file
View File

@@ -0,0 +1,31 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro">
<!-- If tf_prefix is given, use "frame tf_prefix/imu_frame", else "imu_frame" -->
<xacro:arg name="tf_prefix" default="" />
<xacro:property name="tf_prefix" value="$(arg tf_prefix)" />
<xacro:if value="${tf_prefix == ''}">
<xacro:property name="imu_frame" value="imu_frame" />
</xacro:if>
<xacro:unless value="${tf_prefix == ''}">
<xacro:property name="imu_frame" value="$(arg tf_prefix)/imu_frame" />
</xacro:unless>
<xacro:macro name="imu_gazebo" params="link imu_topic update_rate">
<gazebo>
<plugin name="imu_plugin" filename="libhector_gazebo_ros_imu.so">
<updateRate>${update_rate}</updateRate>
<bodyName>${link}</bodyName>
<frameId>${imu_frame}</frameId> <!-- from real MiR -->
<topicName>${imu_topic}</topicName>
<accelDrift>0.0 0.0 0.0</accelDrift>
<accelGaussianNoise>${sqrt(5e-05)} ${sqrt(0.0001)} ${sqrt(0.00013)}</accelGaussianNoise> <!-- real MiR linear_acceleration_covariance: [5e-05, 0.0, 0.0, 0.0, 0.0001, 0.0, 0.0, 0.0, 0.00013] -->
<rateDrift>0.0 0.0 0.0</rateDrift>
<rateGaussianNoise>${sqrt(8e-06)} ${sqrt(8e-06)} ${sqrt(3e-07)}</rateGaussianNoise> <!-- real MiR angular_velocity_covariance: [8e-06, 0.0, 0.0, 0.0, 8e-06, 0.0, 0.0, 0.0, 3e-07] -->
<yawDrift>0.0</yawDrift>
<yawGaussianNoise>${sqrt(0.1)}</yawGaussianNoise> <!-- real MiR orientation_covariance: [0.1, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.1] -->
</plugin>
</gazebo>
</xacro:macro>
</robot>

40
Controllers/Packages/cititruck_description/urdf/include/materials.xacro Executable file
View File

@@ -0,0 +1,40 @@
<?xml version="1.0"?>
<robot>
<material name="black">
<color rgba="0.0 0.0 0.0 1.0"/>
</material>
<material name="blue">
<color rgba="0.0 0.0 0.8 1.0"/>
</material>
<material name="green">
<color rgba="0.0 0.8 0.0 1.0"/>
</material>
<material name="grey">
<color rgba="0.2 0.2 0.2 1.0"/>
</material>
<material name="grey2">
<color rgba="0.4 0.4 0.4 1.0"/>
</material>
<material name="orange">
<color rgba="${255/255} ${108/255} ${10/255} 1.0"/>
</material>
<material name="brown">
<color rgba="${222/255} ${207/255} ${195/255} 1.0"/>
</material>
<material name="red">
<color rgba="0.8 0.0 0.0 1.0"/>
</material>
<material name="white">
<color rgba="1.0 1.0 1.0 1.0"/>
</material>
</robot>

185
Controllers/Packages/cititruck_description/urdf/include/sensor_calibration_assembly.xacro Executable file
View File

@@ -0,0 +1,185 @@
<?xml version="1.0"?>
<!--
# (c) 2018 EU H2020 Project ILIAD, iliad-project.eu
# The following code in this file is licensed under the MIT license:
#
# Permission is hereby granted, free of charge, to any person obtaining a copy
# of this software and associated documentation files (the "Software"), to deal
# in the Software without restriction, including without limitation the rights
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
# copies of the Software, and to permit persons to whom the Software is
# furnished to do so, subject to the following conditions:
# The above copyright notice and this permission notice shall be included in all
# copies or substantial portions of the Software.
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
# SOFTWARE.
-->
<!-- This xacro contains a macro for spawning a sensor calibration assembly inside a robot's URDF file.
The idea is that on the final robot, the assembly just consists of two fixed joints, which have the origin and rpy values set properly
to reflect the sensor's extrinsic calibration values.
Instead, during the calibration phase, these two fixed joints are replaced by a chain of nine prismatic+revolute joints, such that
each dimension can be tweaked individually using the sliders in the joint state publisher GUI.
Once calibrated, calibration values should be saved via save_calibration.py.
-->
<robot name="sensor_calibration_assembly_macro" xmlns:xacro="http://ros.org/wiki/xacro">
<!-- Command-line arguments -->
<xacro:arg name="use_fixed_joints" default="true"/> <!-- Set to false for calibration -->
<xacro:arg name="calib_file" default=""/> <!-- Same file name should be passed to joint state publisher. Should be set up like this:
zeros:
velodyne_calib_pre_x: 0.1
velodyne_calib_pre_y: -0.2
...
velodyne_calib_roll: 0.10
velodyne_calib_pitch: -0.03
..
velodyne_calib_post_x: 0.05
-->
<!-- Helper macro -->
<xacro:macro name="simple_link" params="name">
<link name="${name}">
<inertia ixx="3.64583333333e-07" ixy="0.0" ixz="0.0" iyy="3.64583333333e-07" iyz="0.0" izz="3.125e-07"/>
</link>
</xacro:macro>
<!-- Invoke this macro to create a sensor calibration assembly (XYZ offset + rotation + XYZ post-offset). -->
<xacro:macro name="sensor_calibration_assembly" params="sensor_id parent_link create_final_link:=true final_link_suffix:='link'">
<!-- Lighter version with a single fixed pre-calibrated joint per sensor (for use in Gazebo and on the actual robot) -->
<xacro:if value="$(arg use_fixed_joints)">
<!-- Load resulting transforms exported by save_calibration.py, and use them for the fixed offset and rpy -->
<xacro:property name="all_sensor_tfs" value="${load_yaml('$(arg calib_file)').get('resulting_transforms', dict())}"/>
<xacro:property name="sensor_tf" value="${all_sensor_tfs.get(sensor_id, [0.0]*6)}"/>
<xacro:property name="xyz" value="${' '.join(str(x) for x in sensor_tf[0:3] )}"/>
<xacro:property name="rpy" value="${' '.join(str(x) for x in sensor_tf[3:6] )}"/>
<!-- Resulting transform -->
<joint name="${sensor_id}_resulting_transform_joint" type="fixed">
<origin xyz="${xyz}" rpy="${rpy}"/>
<parent link="${parent_link}"/>
<child link="${sensor_id}_${final_link_suffix}"/>
</joint>
</xacro:if>
<!-- Version with movable joints (only for calib use with joint state publisher GUI) -->
<xacro:unless value="$(arg use_fixed_joints)">
<!-- Dynamic joint limits -->
<xacro:property name="max_trans" value="1.0"/>
<xacro:property name="max_rot" value="3.14159"/>
<!-- Zero values will be loaded by joint state publisher! Joints should be 1D because only 1D values are supported in the GUI -->
<!-- Initial dummy link (zero transform) that starts the chain -->
<!-- Do not remove, required for automatic saving! -->
<simple_link name="${sensor_id}_calib_start_link"/>
<joint name="${sensor_id}_calib_start" type="fixed">
<parent link="${parent_link}"/>
<child link="${sensor_id}_calib_start_link"/>
</joint>
<!-- Pre-translation -->
<simple_link name="${sensor_id}_calib_pre_x_link"/>
<joint name="${sensor_id}_calib_pre_x" type="prismatic">
<axis xyz="1 0 0"/>
<limit lower="-${max_trans}" upper="+${max_trans}" effort="1" velocity="1"/>
<parent link="${sensor_id}_calib_start_link"/>
<child link="${sensor_id}_calib_pre_x_link"/>
</joint>
<simple_link name="${sensor_id}_calib_pre_y_link"/>
<joint name="${sensor_id}_calib_pre_y" type="prismatic">
<axis xyz="0 1 0"/>
<limit lower="-${max_trans}" upper="+${max_trans}" effort="1" velocity="1"/>
<parent link="${sensor_id}_calib_pre_x_link"/>
<child link="${sensor_id}_calib_pre_y_link"/>
</joint>
<simple_link name="${sensor_id}_calib_pre_z_link"/>
<joint name="${sensor_id}_calib_pre_z" type="prismatic">
<axis xyz="0 0 1"/>
<limit lower="-${max_trans}" upper="+${max_trans}" effort="1" velocity="1"/>
<parent link="${sensor_id}_calib_pre_y_link"/>
<child link="${sensor_id}_calib_pre_z_link"/>
</joint>
<!-- Rotation -->
<simple_link name="${sensor_id}_calib_roll_link"/>
<joint name="${sensor_id}_calib_roll" type="revolute">
<axis xyz="1 0 0"/>
<limit lower="-${max_rot}" upper="+${max_rot}" effort="1" velocity="1"/>
<parent link="${sensor_id}_calib_pre_z_link"/>
<child link="${sensor_id}_calib_roll_link"/>
</joint>
<simple_link name="${sensor_id}_calib_pitch_link"/>
<joint name="${sensor_id}_calib_pitch" type="revolute">
<axis xyz="0 1 0"/>
<limit lower="-${max_rot}" upper="+${max_rot}" effort="1" velocity="1"/>
<parent link="${sensor_id}_calib_roll_link"/>
<child link="${sensor_id}_calib_pitch_link"/>
</joint>
<simple_link name="${sensor_id}_calib_yaw_link"/>
<joint name="${sensor_id}_calib_yaw" type="revolute">
<axis xyz="0 0 1"/>
<limit lower="-${max_rot}" upper="+${max_rot}" effort="1" velocity="1"/>
<parent link="${sensor_id}_calib_pitch_link"/>
<child link="${sensor_id}_calib_yaw_link"/>
</joint>
<!-- Post-translation (after rotation) -->
<simple_link name="${sensor_id}_calib_post_x_link"/>
<joint name="${sensor_id}_calib_post_x" type="prismatic">
<axis xyz="1 0 0"/>
<limit lower="-${max_trans}" upper="+${max_trans}" effort="1" velocity="1"/>
<parent link="${sensor_id}_calib_yaw_link"/>
<child link="${sensor_id}_calib_post_x_link"/>
</joint>
<simple_link name="${sensor_id}_calib_post_y_link"/>
<joint name="${sensor_id}_calib_post_y" type="prismatic">
<axis xyz="0 1 0"/>
<limit lower="-${max_trans}" upper="+${max_trans}" effort="1" velocity="1"/>
<parent link="${sensor_id}_calib_post_x_link"/>
<child link="${sensor_id}_calib_post_y_link"/>
</joint>
<simple_link name="${sensor_id}_calib_post_z_link"/>
<joint name="${sensor_id}_calib_post_z" type="prismatic">
<axis xyz="0 0 1"/>
<limit lower="-${max_trans}" upper="+${max_trans}" effort="1" velocity="1"/>
<parent link="${sensor_id}_calib_post_y_link"/>
<child link="${sensor_id}_calib_post_z_link"/>
</joint>
<!-- Final dummy link (zero transform) that ends the chain -->
<!-- Do not remove, required for automatic saving! -->
<joint name="${sensor_id}_calib_end_link" type="fixed">
<parent link="${sensor_id}_calib_post_z_link"/>
<child link="${sensor_id}_${final_link_suffix}"/>
</joint>
<!-- End of version with movable joints for calib -->
</xacro:unless>
<!-- Final link -->
<xacro:if value="${create_final_link}">
<simple_link name="${sensor_id}_${final_link_suffix}"/>
</xacro:if>
</xacro:macro>
</robot>

72
Controllers/Packages/cititruck_description/urdf/include/sick_s300.urdf.xacro Executable file
View File

@@ -0,0 +1,72 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro">
<xacro:include filename="$(find cititruck_description)/urdf/include/common_properties.urdf.xacro" />
<xacro:property name="laser_x" value="0.156" />
<xacro:property name="laser_y" value="0.155" />
<xacro:property name="laser_z" value="0.185" />
<xacro:property name="laser_mass" value="1.2" />
<xacro:macro name="sick_s300" params="link topic prefix">
<link name="${prefix}${link}">
<visual>
<origin xyz="0.0 0.0 0.0" rpy="${pi} 0 0" />
<geometry>
<mesh filename="package://cititruck_description/meshes/visual/sick_lms-100.stl" />
</geometry>
<!-- <xacro:insert_block name="material_yellow" /> -->
<xacro:insert_block name="material_black" />
</visual>
<collision>
<origin xyz="0.0 0.0 0.0" rpy="${pi} 0 0" />
<geometry>
<mesh filename="package://cititruck_description/meshes/visual/sick_lms-100.stl" />
</geometry>
</collision>
<xacro:box_inertial x="${laser_x}" y="${laser_y}" z="${laser_z}" mass="${laser_mass}">
<origin xyz="0 0 0" />
</xacro:box_inertial>
</link>
<gazebo reference="${prefix}${link}">
<!-- <material value="Gazebo/Yellow" /> -->
<material value="Gazebo/FlatBlack" />
<sensor type="ray" name="${prefix}${link}">
<pose>0 0 0 0 0 0</pose>
<visualize>false</visualize>
<update_rate>12.5</update_rate>
<ray>
<scan>
<horizontal>
<samples>541</samples>
<resolution>1</resolution> <!-- has to be 1; actual resolution will be computed from number of samples + min_angle/max_angle -->
<!-- <min_angle>-2.35619449615</min_angle>
<max_angle>2.35619449615</max_angle> -->
<min_angle>-2.094395102</min_angle>
<max_angle>2.181661565</max_angle>
</horizontal>
</scan>
<range>
<min>0.05</min>
<max>29.0</max>
<resolution>0.01</resolution>
</range>
<noise>
<type>gaussian</type>
<!-- Noise parameters based on published spec for S300 achieving
"+-29mm" accuracy at range < 3m (~0.01 of the range) at
1 sigma. -->
<mean>0.0</mean>
<stddev>0.01</stddev>
</noise>
</ray>
<plugin name="gazebo_ros_${link}_controller" filename="libgazebo_ros_laser.so">
<frameName>${prefix}${link}</frameName>
<topicName>${topic}</topicName>
</plugin>
</sensor>
</gazebo>
</xacro:macro>
</robot>