前言
gazebo 中能仿真真实世界,包括很多物理属性,比如惯性,碰撞等。对于没有真实机器人和场地条件的情况下,作用十分强大。
gazebo 世界中的机器人
先上个成品图:
在gazebo世界中,要完成仿真,首先要有机器人模型。
先贴上机器人模型的代码:
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro"
xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
name="robot1_xacro">
<xacro:property name="length_wheel" value="0.05" />
<xacro:property name="radius_wheel" value="0.05" />
<xacro:property name="camera_link" value="0.05" />
<xacro:macro name="default_inertial" params="mass">
<inertial>
<mass value="${mass}" />
<inertia ixx="1.0" ixy="0.0" ixz="0.0"
iyy="1.0" iyz="0.0"
izz="1.0" />
</inertial>
</xacro:macro>
<!-- <link name="base_footprint">
<visual>
<geometry>
<box size="0.001 0.001 0.001"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0"/>
</visual>
<xacro:default_inertial mass="0.0001"/>
</link> -->
<xacro:include filename="$(find robot1_description)/urdf/robot_depthCam_laser.gazebo" />
<gazebo reference="base_footprint">
<material>Gazebo/Green</material>
<turnGravityOff>false</turnGravityOff>
</gazebo>
<!-- <joint name="base_footprint_joint" type="fixed">
<origin xyz="0 0 0" />
<parent link="base_footprint" />
<child link="base_link" />
</joint> -->
<link name="base_link">
<visual>
<geometry>
<box size="0.2 .3 1"/>
</geometry>
<origin rpy="0 0 1.54" xyz="0 0 0.05"/>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<box size="0.2 .3 1"/>
</geometry>
</collision>
<xacro:default_inertial mass="10"/>
</link>
<link name="wheel_1">
<visual>
<geometry>
<cylinder length="${length_wheel}" radius="${radius_wheel}"/>
</geometry>
<!-- <origin rpy="0 1.5 0" xyz="0.1 0.1 0"/> -->
<origin rpy="0 0 0" xyz="0 0 0"/>
<material name="black">
<color rgba="0 0 0 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length="${length_wheel}" radius="${radius_wheel}"/>
</geometry>
</collision>
<xacro:default_inertial mass="1"/>
</link>
<link name="wheel_2">
<visual>
<geometry>
<cylinder length="${length_wheel}" radius="${radius_wheel}"/>
</geometry>
<!-- <origin rpy="0 1.5 0" xyz="-0.1 0.1 0"/> -->
<origin rpy="0 0 0" xyz="0 0 0"/>
<material name="black"/>
</visual>
<collision>
<geometry>
<cylinder length="${length_wheel}" radius="${radius_wheel}"/>
</geometry>
</collision>
<xacro:default_inertial mass="1"/>
</link>
<link name="wheel_3">
<visual>
<geometry>
<cylinder length="${length_wheel}" radius="${radius_wheel}"/>
</geometry>
<!-- <origin rpy="0 1.5 0" xyz="0.1 -0.1 0"/> -->
<origin rpy="0 0 0" xyz="0 0 0"/>
<material name="black"/>
</visual>
<collision>
<geometry>
<cylinder length="${length_wheel}" radius="${radius_wheel}"/>
</geometry>
</collision>
<xacro:default_inertial mass="1"/>
</link>
<link name="wheel_4">
<visual>
<geometry>
<cylinder length="${length_wheel}" radius="${radius_wheel}"/>
</geometry>
<!-- <origin rpy="0 1.5 0" xyz="-0.1 -0.1 0"/> -->
<origin rpy="0 0 0" xyz="0 0 0" />
<material name="black"/>
</visual>
<collision>
<geometry>
<cylinder length="${length_wheel}" radius="${radius_wheel}"/>
</geometry>
</collision>
<xacro:default_inertial mass="1"/>
</link>
<joint name="base_to_wheel1" type="continuous">
<parent link="base_link"/>
<child link="wheel_1"/>
<origin rpy="-1.5707 0 0" xyz="0.1 0.125 -0.45"/>
<axis xyz="0 0 1" />
</joint>
<joint name="base_to_wheel2" type="continuous">
<axis xyz="0 0 1" />
<anchor xyz="0 0 0" />
<limit effort="100" velocity="100" />
<parent link="base_link"/>
<child link="wheel_2"/>
<origin rpy="-1.5707 0 0" xyz="-0.1 0.125 -0.45"/>
</joint>
<joint name="base_to_wheel3" type="continuous">
<parent link="base_link"/>
<axis xyz="0 0 1" />
<child link="wheel_3"/>
<origin rpy="-1.5707 0 0" xyz="0.1 -0.125 -0.45"/>
</joint>
<joint name="base_to_wheel4" type="continuous">
<parent link="base_link"/>
<axis xyz="0 0 1" />
<child link="wheel_4"/>
<origin rpy="-1.5707 0 0" xyz="-0.1 -0.125 -0.45"/>
</joint>
<!-- Camera joint -->
<joint name="camera_joint" type="fixed">
<axis xyz="0 1 0" />
<origin xyz="0.125 0 0.575" rpy="0 0 0"/>
<parent link="base_link"/>
<child link="camera_link"/>
</joint>
<!-- Camera -->
<link name="camera_link">
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.05 0.05 0.05"/>
</geometry>
</collision>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.05 0.05 0.05"/>
</geometry>
<material name="red">
<color rgba="1 0 0 1"/>
</material>
</visual>
<inertial>
<mass value="1e-5" />
<origin xyz="0 0 0" rpy="0 0 0"/>
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
</inertial>
</link>
<!-- Hokuyo joint -->
<joint name="hokuyo_joint" type="fixed">
<origin xyz="0.125 0.075 0.575" rpy="0 0 0"/>
<parent link="base_link"/>
<axis xyz="0 1 0" />
<child link="hokuyo_link"/>
</joint>
<!-- Hokuyo Laser -->
<link name="hokuyo_link">
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.05 0.05 0.05"/>
</geometry>
</collision>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.05 0.05 0.05"/>
</geometry>
</visual>
<inertial>
<mass value="1e-5" />
<origin xyz="0 0 0" rpy="0 0 0"/>
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
</inertial>
</link>
<!-- Drive controller -->
<gazebo>
<plugin name="skid_steer_drive_controller" filename="libgazebo_ros_skid_steer_drive.so">
<updateRate>100.0</updateRate>
<robotNamespace>/</robotNamespace>
<leftFrontJoint>base_to_wheel1</leftFrontJoint>
<rightFrontJoint>base_to_wheel3</rightFrontJoint>
<leftRearJoint>base_to_wheel2</leftRearJoint>
<rightRearJoint>base_to_wheel4</rightRearJoint>
<wheelSeparation>4</wheelSeparation>
<wheelDiameter>0.1</wheelDiameter>
<robotBaseFrame>base_link</robotBaseFrame>
<torque>1</torque>
<topicName>cmd_vel</topicName>
<broadcastTF>0</broadcastTF>
</plugin>
</gazebo>
</robot>
这是一个带有传感器Hokuyo雷达,相机的机器人模型文件:robot_depthCam_laser.xacro,
<!-- Camera -->
<link name="camera_link">
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.05 0.05 0.05"/>
</geometry>
</collision>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.05 0.05 0.05"/>
</geometry>
<material name="red">
<color rgba="1 0 0 1"/>
</material>
</visual>
<inertial>
<mass value="1e-5" />
<origin xyz="0 0 0" rpy="0 0 0"/>
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
</inertial>
</link>
这是相机模块的节点,基本语法在这里就不多说了,很多资料有教程。
<!-- Hokuyo Laser -->
<link name="hokuyo_link">
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.05 0.05 0.05"/>
</geometry>
</collision>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.05 0.05 0.05"/>
</geometry>
</visual>
<inertial>
<mass value="1e-5" />
<origin xyz="0 0 0" rpy="0 0 0"/>
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
</inertial>
</link>
这是Hokuyo雷达的模块。
<!-- Drive controller -->
<gazebo>
<plugin name="skid_steer_drive_controller" filename="libgazebo_ros_skid_steer_drive.so">
<updateRate>100.0</updateRate>
<robotNamespace>/</robotNamespace>
<leftFrontJoint>base_to_wheel1</leftFrontJoint>
<rightFrontJoint>base_to_wheel3</rightFrontJoint>
<leftRearJoint>base_to_wheel2</leftRearJoint>
<rightRearJoint>base_to_wheel4</rightRearJoint>
<wheelSeparation>4</wheelSeparation>
<wheelDiameter>0.1</wheelDiameter>
<robotBaseFrame>base_link</robotBaseFrame>
<torque>1</torque>
<topicName>cmd_vel</topicName>
<broadcastTF>0</broadcastTF>
</plugin>
</gazebo>
这一个节点有点不同,这个是一个gazebo插件,是用于使机器人在gazebo世界中运动的模块,正常来说,如果不需要在gazebo中进行仿真运动,在一般的xacro文件或者urdf文件中,定义机器人是不需要这种模块的,去掉这个模块也可以在rviz中显示出机器人模型。gazebo插件有很多种,这里要补充说明的是,上文中已经添加了雷达,相机等模块,但是也只是一个安放在机器人身上的一个模型,在gazebo中是不会起到仿真作用,因此必须要在另外一个文件(或者在同一个文件)中进行插件定义。
定义gazebo插件:gazebo文件
在robot_depthCam_laser.gazebo文件中:
<?xml version="1.0"?>
<robot>
<!-- materials -->
<gazebo reference="base_link">
<material>Gazebo/Orange</material>
</gazebo>
<gazebo reference="wheel_1">
<material>Gazebo/Black</material>
</gazebo>
<gazebo reference="wheel_2">
<material>Gazebo/Black</material>
</gazebo>
<gazebo reference="wheel_3">
<material>Gazebo/Black</material>
</gazebo>
<gazebo reference="wheel_4">
<material>Gazebo/Black</material>
</gazebo>
<!-- ros_control plugin -->
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
<robotNamespace>/robot</robotNamespace>
</plugin>
</gazebo>
<!--
<gazebo reference="link1">
<material>Gazebo/Orange</material>
</gazebo>
<gazebo reference="link2">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/Black</material>
</gazebo>
<gazebo reference="link3">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/Orange</material>
</gazebo>-->
<!-- camera_link -->
<gazebo reference="camera_link">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/Red</material>
</gazebo>
<!-- hokuyo -->
<gazebo reference="hokuyo_link">
<sensor type="ray" name="head_hokuyo_sensor">
<pose>0 0 0 0 0 0</pose>
<visualize>false</visualize>
<update_rate>40</update_rate>
<ray>
<scan>
<horizontal>
<samples>720</samples>
<resolution>1</resolution>
<min_angle>-1.570796</min_angle>
<max_angle>1.570796</max_angle>
</horizontal>
</scan>
<range>
<min>0.10</min>
<max>30.0</max>
<resolution>0.01</resolution>
</range>
<noise>
<type>gaussian</type>
<!-- Noise parameters based on published spec for Hokuyo laser
achieving "+-30mm" accuracy at range < 10m. A mean of 0.0m and
stddev of 0.01m will put 99.7% of samples within 0.03m of the true
reading. -->
<mean>0.0</mean>
<stddev>0.01</stddev>
</noise>
</ray>
<plugin name="gazebo_ros_head_hokuyo_controller" filename="libgazebo_ros_laser.so">
<topicName>/robot/laser/scan</topicName>
<frameName>hokuyo_link</frameName>
</plugin>
</sensor>
</gazebo>
<gazebo reference="camera_link">
<sensor type="depth" name="camera">
<always_on>true</always_on>
<update_rate>20.0</update_rate>
<camera>
<horizontal_fov>${1.3962634}</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></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>camera_link</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>
这个文件主要定义很多机器人在gazebo世界中各种表现,和属性。这里主要看一下如何定义传感器等插件的定义:
<!-- hokuyo -->
<gazebo reference="hokuyo_link">
<sensor type="ray" name="head_hokuyo_sensor">
<pose>0 0 0 0 0 0</pose>
<visualize>false</visualize>
<update_rate>40</update_rate>
<ray>
<scan>
<horizontal>
<samples>720</samples>
<resolution>1</resolution>
<min_angle>-1.570796</min_angle>
<max_angle>1.570796</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_ros_head_hokuyo_controller" filename="libgazebo_ros_laser.so">
<topicName>/robot/laser/scan</topicName>
<frameName>hokuyo_link</frameName>
</plugin>
</sensor>
</gazebo>
这个是定义雷达模块的,一般属性定义成默认值即可使用。
<gazebo reference="camera_link">
<sensor type="depth" name="camera">
<always_on>true</always_on>
<update_rate>20.0</update_rate>
<camera>
<horizontal_fov>${1.3962634}</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></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>camera_link</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>
这个是定义了深度相机模块的属性。
值得注意的是:
这里很重要,因为这里的reference作用是直接与xacro文件中的模块名字进行对应,从而将相机属性赋予xacro中camera_link模块。
这里在与gazebo世界中进行仿真算法的时候十分重要,在这里定义的topic名称必须与算法中所要求的topic名称要一致。
启动gazebo世界和机器人模型
现在已经完成了机器人模型的创建了,下一步就是把机器人在gazebo世纪中启动。在ros系统中,打开多个节点一般需要编写launch文件:
<?xml version="1.0"?>
<launch>
<arg name="world" />
<arg name="model"/>
<include file="$(find gazebo_ros)/launch/$(arg world)">
</include>
<!-- Load the URDF into the ROS Parameter Server -->
<param name="robot_description" command="$(find xacro)/xacro.py --inorder $(arg model)"/>
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
<!-- joint state publisher -->
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher"/>
<!-- Run a python script to the send a service call to gazebo_ros to spawn a URDF robot -->
<node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
args="-urdf -model robot1 -param robot_description -z 0.05"/>
<!-- tf transform -->
<arg name="pi/2" value="1.5707963267948966" />
<arg name="optical_rotate" value="0 0 0 -$(arg pi/2) 0 -$(arg pi/2)" />
<node pkg="tf" type="static_transform_publisher" name="camera_base_link"
args="$(arg optical_rotate) base_link camera_link 100" />
</launch>
launch文件的语法在这里也不多说,主要说以下使用这个launch 文件所需要的参数:
在启动的时候需要传入两个参数,一个是所用的gazebo环境的启动launch文件,一般会放在./opt/ros/melodic/share/gazebo_ros/launch中,包括自定义的gazebo世界的launch,最好也要放在同处地方。另一个参数则是所用的模型文件。这里传入参数后会用到一个命令,完整命令是这样的:
rosrun xacro xacro.py model.xacro > model_processed.urdf
这句命令很简单,主要是用作将xacro文件导出成urdf文件。在launch文件中则是这样使用:
这里实际上是将xacro文件转化为urdf后,将其放进param参数服务器中,便于其他节点的调用,也就是launch文件中那个urdf_spawner节点的调用。
在这里需要启用一个节点是tf转换,原因是在rtabmap算法中,世界坐标系是x向前,y向左,z向上,但是在相机坐标系中,三轴的方向是不同的,因此需要转换。
启动gazebo世界和机器人模型:
roslaunch robot1_gazebo gazebo_wg.launch model:="`rospack find robot1_description`/urdf/robot_depthCam_laser.xacro" world:="willowgarage_world.launch"
启动后的结果:
机器人已经成功地在gazebo世界中运行了,接下来就是运行rtabmap算法进行建图了。