11 月 25 日 ROS 学习笔记——3D 建模与仿真

---

前言

本文为11 月 25 日 ROS 学习笔记——3D 建模与仿真，分为两个章节：

• 在 ROS 中自定义机器人的3D模型，
• Gazebo

---

一、在 ROS 中自定义机器人的3D模型

<robot name="Robot1"><link name="base_link"><visual><geometry><box size="0.2 .3 .1" /></geometry><origin rpy="0 0 0" xyz="0 0 0.05" /><material name="white"><color rgba="1 1 1 1" /></material></visual></link><link name="wheel_1"><visual><geometry><cylinder length="0.05" radius="0.05" /></geometry><origin rpy="0 1.5 0" xyz="0.1 0.1 0" /><material name="black"><color rgba="0 0 0 1" /></material></visual></link><link name="wheel_2"><visual><geometry><cylinder length="0.05" radius="0.05" /></geometry><origin rpy="0 1.5 0" xyz="-0.1 0.1 0" /><material name="black" /></visual></link><link name="wheel_3"><visual><geometry><cylinder length="0.05" radius="0.05" /></geometry><origin rpy="0 1.5 0" xyz="0.1 -0.1 0" /><material name="black" /></visual></link><link name="wheel_4"><visual><geometry><cylinder length="0.05" radius="0.05" /></geometry><origin rpy="0 1.5 0" xyz="-0.1 -0.1 0" /><material name="black" /></visual></link><joint name="base_to_wheel1" type="fixed"><parent link="base_link" /><child link="wheel_1" /><origin xyz="0 0 0" /></joint><joint name="base_to_wheel2" type="fixed"><parent link="base_link" /><child link="wheel_2" /><origin xyz="0 0 0" /></joint><joint name="base_to_wheel3" type="fixed"><parent link="base_link" /><child link="wheel_3" /><origin xyz="0 0 0" /></joint><joint name="base_to_wheel4" type="fixed"><parent link="base_link" /><child link="wheel_4" /><origin xyz="0 0 0" /></joint>
</robot>

• 检查书写的语法是否正确和配置是否有误

check_urdf robot1.urdf 
>>> robot name is: Robot1---------- Successfully Parsed XML ---------------root Link: base_link has 4 child(ren)child(1):  wheel_1child(2):  wheel_2child(3):  wheel_3child(4):  wheel_4

• 以图形的方式来查看

urdf_to_graphiz robot1.urdf 
>>> Created file Robot1.gvCreated file Robot1.pdfevince Robot1.pdf

1. 在 rviz 里查看3D模型

• 创建 .launch 文件

<launch><arg name="model" /><arg name="gui" default="False" /><param name="robot_description" textfile="$(arg model)" /><param name="use_gui" value="$(arg gui)" /><node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" /><node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
</launch>

• 启动命令

roslaunch robot1_description display.launch model:="$(rospack find robot1_description)/urdf/robot1.urdf"

• 添加一些组件

</joint><link name="arm_base">
<visual>
<geometry>
<box size="0.1 .1 .1"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0.1"/>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual><collision>
<geometry>
<box size="0.1 .1 .1"/>
</geometry>
</collision><inertial>
<mass value="1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link><joint name="base_to_arm_base" type="continuous">
<parent link="base_link"/>
<child link="arm_base"/>
<axis xyz="0 0 1"/>
<origin xyz="0 0 0"/>
</joint><link name="arm_1">
<visual>
<geometry>
<box size="0.05 .05 0.5"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0.25"/>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<box size="0.05 .05 0.5"/>
</geometry>
</collision>
<inertial>
<mass value="1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link><joint name="arm_1_to_arm_base" type="revolute">
<parent link="arm_base"/>
<child link="arm_1"/>
<axis xyz="1 0 0"/>
<origin xyz="0 0 0.15"/>
<limit effort ="1000.0" lower="-1.0" upper="1.0" velocity="0.5"/>
</joint>
<link name="arm_2">
<visual>
<geometry>
<box size="0.05 0.05 0.5"/>
</geometry>
<origin rpy="0 0 0" xyz="0.06 0 0.15"/>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<box size="0.05 .05 0.5"/>
</geometry>
</collision>
<inertial>
<mass value="1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="arm_2_to_arm_1" type="revolute">
<parent link="arm_1"/>
<child link="arm_2"/>
<axis xyz="1 0 0"/>
<origin xyz="0.0 0 0.45"/>
<limit effort ="1000.0" lower="-2.5" upper="2.5" velocity="0.5"/>
</joint>
<joint name="left_gripper_joint" type="revolute">
<axis xyz="0 0 1"/>
<limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
<origin rpy="0 -1.57 0" xyz="0.06 0 0.4"/>
<parent link="arm_2"/>
<child link="left_gripper"/>
</joint>
<link name="left_gripper">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.dae"/>
</geometry>
</visual>
<collision>
<geometry>
<box size="0.1 .1 .1"/>
</geometry>
</collision>
<inertial>
<mass value="1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="left_tip_joint" type="fixed">
<parent link="left_gripper"/>
<child link="left_tip"/>
</joint>
<link name="left_tip">
<visual>
<origin rpy="0.0 0 0" xyz="0.09137 0.00495 0"/>
<geometry>
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"/>
</geometry>
</visual>
<collision>
<geometry>
<box size="0.1 .1 .1"/>
</geometry>
</collision>
<inertial>
<mass value="1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="right_gripper_joint" type="revolute">
<axis xyz="0 0 -1"/>
<limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
<origin rpy="0 -1.57 0" xyz="0.06 0 0.4"/>
<parent link="arm_2"/>
<child link="right_gripper"/>
</joint>
<link name="right_gripper">
<visual>
<origin rpy="-3.1415 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.dae"/>
</geometry>
</visual>
<collision>
<geometry>
<box size="0.1 .1 .1"/>
</geometry>
</collision>
<inertial>
<mass value="1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="right_tip_joint" type="fixed">
<parent link="right_gripper"/>
<child link="right_tip"/>
</joint>
<link name="right_tip">
<visual>
<origin rpy="-3.1415 0 0" xyz="0.09137 0.00495 0"/>
<geometry>
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"/>
</geometry>
</visual>
<collision>
<geometry>
<box size="0.1 .1 .1"/>
</geometry>
</collision>
<inertial>
<mass value="1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>

• 旋转关节

<joint name="arm_1_to_arm_base" type="revolute"><parent link="arm_base"/><child link="arm_1"/><axis xyz="1 0 0"/><origin xyz="0 0 0.15"/><limit effort ="1000.0" lower="-1.0" upper="1.0" velocity="0.5"/>
</joint>

2. xacro

Xacro 可帮助我们压缩 URDF 文件的大小， 增加文件的可读性和可维护性。它还允许我们创建模型并复用这些模型以创建相同的结构，如更多的手臂和腿.

• 使用 xacro 声明常量，避免在很多行重复定义同一个 数值

<xacro:property name="length_wheel" value="0.05" />
<xacro:property name="radius_wheel" value="0.05" />

• 使用常量

${name_of_variable}:
<cylinder length="${length_wheel}" radius="${radius_wheel}" />

• 将 .xacro 转换为 .urdf

rosrun xacro xacro demo01_helloworld.urdf.xacro
>>> <robot name="mycar"><link name="left_wheel"><visual><geometry><cylinder length="0.0015" radius="0.0325"/></geometry><origin rpy="1.57079635 0 0" xyz="0 0 0"/><material name="wheel_color"><color rgba="0 0 0 0.3"/></material></visual></link><!-- 3-2.joint --><joint name="left2link" type="continuous"><parent link="base_link"/><child link="left_wheel"/><!-- x 无偏移y 车体半径z z= 车体高度 / 2 + 离地间距 - 车轮半径--><origin rpy="0 0 0" xyz="0 0.1 -0.0225"/><axis xyz="0 1 0"/></joint><link name="right_wheel"><visual><geometry><cylinder length="0.0015" radius="0.0325"/></geometry><origin rpy="1.57079635 0 0" xyz="0 0 0"/><material name="wheel_color"><color rgba="0 0 0 0.3"/></material></visual></link><!-- 3-2.joint --><joint name="right2link" type="continuous"><parent link="base_link"/><child link="right_wheel"/><!-- x 无偏移y 车体半径z z= 车体高度 / 2 + 离地间距 - 车轮半径--><origin rpy="0 0 0" xyz="0 -0.1 -0.0225"/><axis xyz="0 1 0"/></joint></robot>

rosrun xacro xacro demo01_helloworld.urdf.xacro > demo01_helloworld.urdf

• 属性与运算

<xacro:property name="PI" value="3.1415927" />
<xacro:property name="radius" value="0.03" /><!-- 属性调用 -->
<myUsePropertyxxx name="${PI}" />
<myUsePropertyxxx name="${radius}" />rosrun xacro xacro demo02_field.urdf.xacro
>>> <robot name="mycar"><!-- 属性调用 --><myUsePropertyxxx name="3.1415927"/><myUsePropertyxxx name="0.03"/><!-- 数学运算 --></robot><!-- 数学运算 -->
<myUsePropertyyy result="${PI / 2}" />rosrun xacro xacro demo02_field.urdf.xacro
>>> <robot name="mycar"><!-- 属性调用 --><myUsePropertyxxx name="3.1415927"/><myUsePropertyxxx name="0.03"/><!-- 数学运算 --><myUsePropertyyy result="1.57079635"/></robot>

• 宏

<!-- 宏定义 -->
<xacro:macro name="getSum" params="num1 num2"><result value="${num1 + num2}" />
</xacro:macro><!-- 宏调用 -->
<xacro:getSum num1="1" num2="5" />>>> rosrun xacro xacro demo03_macro.urdf.xacro
<robot name="mycar"><result value="6"/>
</robot>

• 文件包含

<xacro:include filename="demo02_field.urdf.xacro" />
<xacro:include filename="demo03_macro.urdf.xacro" />rosrun xacro xacro demo04_sum.urdf.xacro
>>> <robot name="mycar"><!-- 属性调用 --><myUsePropertyxxx name="3.1415927"/><myUsePropertyxxx name="0.03"/><!-- 数学运算 --><myUsePropertyyy result="1.57079635"/><result value="6"/></robot>

• xacro 集成 rviz

<param name="robot_description" command="$(find xacro)/xacro $(find urdf01_rviz)/urdf/xacro/demo05_car_base.urdf.xacro" />

• 控制移动机器人做圆周运动

<launch><!-- 载入 urdf 至参数服务器 --><!-- <param name="robot_description" textfile="$(find urdf01_rviz)/urdf/xacro/demo05_car_base.urdf" /> --><param name="robot_description" command="$(find xacro)/xacro $(find urdf01_rviz)/urdf/xacro/car.urdf.xacro" /><!-- 启动 rviz --><node pkg="rviz" type="rviz" name="rviz" args="-d $(find urdf01_rviz)/config/show_mycar.rviz" /><!-- 添加关节状态发布节点 --><node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher" /><!-- 机器人状态发布节点 --><node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" /><!-- 集成 arbotix 运动控制节点 --><node pkg="arbotix_python" type="arbotix_driver" name="driver" output="screen"><rosparam command="load" file="$(find urdf01_rviz)/config/control.yaml" /><param name="sim" value="true" /></node>
</launch>

rostopic pub -r 10 /cmd_vel geometry_msgs/Twist "linear:x: 1.0y: 0.0z: 0.0
angular:x: 0.0y: 0.0z: 1.0"

---

二、Gazebo

1. urdf 集成 gazebo

• 创建机器人模型

<robot name="mycar"><link name="base_link"><visual><geometry><box size="0.5 0.2 0.1" /></geometry><origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" /><material name="yellow"><color rgba="0.5 0.3 0.0 1" /></material></visual><collision><geometry><box size="0.5 0.2 0.1" /></geometry><origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" /></collision><inertial><origin xyz="0 0 0" /><mass value="6" /><inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1" /></inertial></link><gazebo reference="base_link"><material>Gazebo/Red</material></gazebo></robot>

• 创建 .launch 文件

<launch><!-- 载入 urdf 至参数服务器 --><param name="robot_description" textfile="$(find urdf02_gazebo)/urdf/demo01_helloworld.urdf" /><!-- 启动 Gazebo --><include file="$(find gazebo_ros)/launch/empty_world.launch" /><!-- 添加机器人模型 --><node pkg="gazebo_ros" type="spawn_model" name="spawn_model" args="-urdf -model mycar -param robot_description" />
</launch>

• 仿真环境集成

<launch><!-- 载入 urdf 至参数服务器 --><param name="robot_description" command="$(find xacro)/xacro $(find urdf02_gazebo)/urdf/car.urdf.xacro" /><!-- 启动 Gazebo --><include file="$(find gazebo_ros)/launch/empty_world.launch"><arg name="world_name" value="${find urdf02_gazebo}/worlds/box_house.world" /></include><!-- 添加机器人模型 --><node pkg="gazebo_ros" type="spawn_model" name="spawn_model" args="-urdf -model mycar -param robot_description" />
</launch>

2. 综合应用

1). 运动控制及里程计

• 在 gazebo 中运动控制

<robot name="my_car_move" xmlns:xacro="http://wiki.ros.org/xacro"><!-- 传动实现:用于连接控制器与关节 --><xacro:macro name="joint_trans" params="joint_name"><!-- Transmission is important to link the joints and the controller --><transmission name="${joint_name}_trans"><type>transmission_interface/SimpleTransmission</type><joint name="${joint_name}"><hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface></joint><actuator name="${joint_name}_motor"><hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface><mechanicalReduction>1</mechanicalReduction></actuator></transmission></xacro:macro><!-- 每一个驱动轮都需要配置传动装置 --><xacro:joint_trans joint_name="base_l_wheel_joint" /><xacro:joint_trans joint_name="base_r_wheel_joint" /><!-- 控制器 --><gazebo><plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so"><rosDebugLevel>Debug</rosDebugLevel><publishWheelTF>true</publishWheelTF><robotNamespace>/</robotNamespace><publishTf>1</publishTf><publishWheelJointState>true</publishWheelJointState><alwaysOn>true</alwaysOn><updateRate>100.0</updateRate><legacyMode>true</legacyMode><leftJoint>base_l_wheel_joint</leftJoint> <!-- 左轮 --><rightJoint>base_r_wheel_joint</rightJoint> <!-- 右轮 --><wheelSeparation>${base_radius * 2}</wheelSeparation> <!-- 车轮间距 --><wheelDiameter>${wheel_radius * 2}</wheelDiameter> <!-- 车轮直径 --><broadcastTF>1</broadcastTF><wheelTorque>30</wheelTorque><wheelAcceleration>1.8</wheelAcceleration><commandTopic>cmd_vel</commandTopic> <!-- 运动控制话题 --><odometryFrame>odom</odometryFrame> <odometryTopic>odom</odometryTopic> <!-- 里程计话题 --><robotBaseFrame>base_footprint</robotBaseFrame> <!-- 根坐标系 --></plugin></gazebo></robot>

rostopic pub -r 10 /cmd_vel geometry_msgs/Twist "linear:x: 1.0y: 0.0z: 0.0
angular:x: 0.0y: 0.0z: 1.0"

• 在 rviz 中查看里程计消息

<launch><!-- 启动 rviz --><node pkg="rviz" type="rviz" name="rviz" args="-d $(find urdf01_rviz)/config/show_mycar.rviz" /><!-- 添加关节状态发布节点 --><!-- <node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher" /> --><!-- 机器人状态发布节点 --><node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" /></launch>

2). 雷达仿真

<robot name="my_sensors" xmlns:xacro="http://wiki.ros.org/xacro"><!-- 雷达 --><gazebo reference="laser"><sensor type="ray" name="rplidar"><pose>0 0 0 0 0 0</pose><visualize>true</visualize><update_rate>5.5</update_rate><ray><scan><horizontal><samples>360</samples><resolution>1</resolution><min_angle>-3</min_angle><max_angle>3</max_angle></horizontal></scan><range><min>0.10</min><max>30.0</max><resolution>0.01</resolution></range><noise><type>gaussian</type><mean>0.0</mean><stddev>0.01</stddev></noise></ray><plugin name="gazebo_rplidar" filename="libgazebo_ros_laser.so"><topicName>/scan</topicName><frameName>laser</frameName></plugin></sensor></gazebo></robot>

3). 摄像头信息仿真

<robot name="my_sensors" xmlns:xacro="http://wiki.ros.org/xacro"><!-- 被引用的link --><gazebo reference="camera"><!-- 类型设置为 camara --><sensor type="camera" name="camera_node"><update_rate>30.0</update_rate> <!-- 更新频率 --><!-- 摄像头基本信息设置 --><camera name="head"><horizontal_fov>1.3962634</horizontal_fov><image><width>1280</width><height>720</height><format>R8G8B8</format></image><clip><near>0.02</near><far>300</far></clip><noise><type>gaussian</type><mean>0.0</mean><stddev>0.007</stddev></noise></camera><!-- 核心插件 --><plugin name="gazebo_camera" filename="libgazebo_ros_camera.so"><alwaysOn>true</alwaysOn><updateRate>0.0</updateRate><cameraName>/camera</cameraName><imageTopicName>image_raw</imageTopicName><cameraInfoTopicName>camera_info</cameraInfoTopicName><frameName>camera</frameName><hackBaseline>0.07</hackBaseline><distortionK1>0.0</distortionK1><distortionK2>0.0</distortionK2><distortionK3>0.0</distortionK3><distortionT1>0.0</distortionT1><distortionT2>0.0</distortionT2></plugin></sensor></gazebo></robot>

4). kinect 深度相机仿真

<robot name="my_sensors" xmlns:xacro="http://wiki.ros.org/xacro"><gazebo reference="support">  <sensor type="depth" name="camera"><always_on>true</always_on><update_rate>20.0</update_rate><camera><horizontal_fov>${60.0*PI/180.0}</horizontal_fov><image><format>R8G8B8</format><width>640</width><height>480</height></image><clip><near>0.05</near><far>8.0</far></clip></camera><plugin name="kinect_camera_controller" filename="libgazebo_ros_openni_kinect.so"><cameraName>camera</cameraName><alwaysOn>true</alwaysOn><updateRate>10</updateRate><imageTopicName>rgb/image_raw</imageTopicName><depthImageTopicName>depth/image_raw</depthImageTopicName><pointCloudTopicName>depth/points</pointCloudTopicName><cameraInfoTopicName>rgb/camera_info</cameraInfoTopicName><depthImageCameraInfoTopicName>depth/camera_info</depthImageCameraInfoTopicName><frameName>support</frameName><baseline>0.1</baseline><distortion_k1>0.0</distortion_k1><distortion_k2>0.0</distortion_k2><distortion_k3>0.0</distortion_k3><distortion_t1>0.0</distortion_t1><distortion_t2>0.0</distortion_t2><pointCloudCutoff>0.4</pointCloudCutoff></plugin></sensor></gazebo></robot>

5). 点云

• 修改配置文件的 FrameName 标签并添加坐标变换关系

<frameName>support_depth</frameName><!-- 点云坐标系到 kinect 连杆坐标系的变换 -->
<node pkg="tf2_ros" name="static_transform_publisher" type="static_transform_publisher" args="0 0 0 -1.57 0 -1.57 /support /support_depth" />

---