文章目录
- 前言
- 一、在 ROS 中自定义机器人的3D模型
- 1. 在 rviz 里查看3D模型
- 2. xacro
- 二、Gazebo
- 1. urdf 集成 gazebo
- 2. 综合应用
- 1). 运动控制及里程计
- 2). 雷达仿真
- 3). 摄像头信息仿真
- 4). kinect 深度相机仿真
- 5). 点云
前言
本文为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_1
child(2): wheel_2
child(3): wheel_3
child(4): wheel_4
- 以图形的方式来查看
urdf_to_graphiz robot1.urdf
>>> Created file Robot1.gv
Created file Robot1.pdf
evince 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.0
y: 0.0
z: 0.0
angular:
x: 0.0
y: 0.0
z: 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.0
y: 0.0
z: 0.0
angular:
x: 0.0
y: 0.0
z: 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" />
