【ROS】移动机器人导航仿真（1）——3D建模与简单移动
前期基本按着ROS wiki 和《ROS by example vol1 indigo 》走了一遍，但是感觉对ROS还是似懂非懂的，有好多问题仍然不明白。一直在想ROS到底是怎样用在机器人上的。所以干脆从3D建模、传感器信息采集到SLAM、路径规划等把机器人的整个导航过程捋一遍。
本节主要实现移动机器人的3D建模，激光和相机的信息采集以及在gazebo仿真环境中简单地移动机器人。
机器人3D模型在ROS中通过URDF文件实现，标准化机器人描述格式（URDF）是一种描述机器人以及其部分结构、关节、自由度等的XML文件。xacro文件是一种更好的机器人建模方法，可以帮助我们压缩URDF的文件大小，并且增加文件的可读性和可维护性，并且允许复用模型。具体的关于URDF文件和xacro文件的介绍可参考《ROS by example vol2 indigo》 第四章和《Learning ROS for Robotics Programming II》第七章。我在仿真的过程中构建了一个小车模型，模型中包括机器人本体、摄像头和激光雷达。然后用gazebo搭建了一个简单的机器人工作环境。
机器人的模型文件如下：
myrobot.xacro
<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="robot">
<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 nav_sim)/urdf/robot.gazebo" />
<gazebo reference="base_footprint">
<material>Gazebo/Greenmaterial>
<turnGravityOff>falseturnGravityOff>
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 0.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 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 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 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 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.15 0"/>
<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.15 0"/>
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.15 0"/>
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.15 0"/>
joint>
<joint name="camera_joint" type="fixed">
<axis xyz="0 1 0" />
<origin xyz="0.125 0 0.125" rpy="0 0 0"/>
<parent link="base_link"/>
<child link="camera_link"/>
joint>
<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>
<joint name="hokuyo_joint" type="fixed">
<origin xyz="0.125 0.05 0.125" rpy="0 0 0"/>
<parent link="base_link"/>
<axis xyz="0 1 0" />
<child link="hokuyo_link"/>
joint>
<link name="hokuyo_link">
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.1 0.1 0.1"/>
geometry>
collision>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="package://nav_sim/meshes/hokuyo.dae"/>
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>
robot>robot.gazebo
<robot>
<gazebo reference="base_link">
<material>Gazebo/Orangematerial>
gazebo>
<gazebo reference="wheel_1">
<material>Gazebo/Blackmaterial>
gazebo>
<gazebo reference="wheel_2">
<material>Gazebo/Blackmaterial>
gazebo>
<gazebo reference="wheel_3">
<material>Gazebo/Blackmaterial>
gazebo>
<gazebo reference="wheel_4">
<material>Gazebo/Blackmaterial>
gazebo>
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
<robotNamespace>/robotrobotNamespace>
plugin>
gazebo>
<gazebo reference="link1">
<material>Gazebo/Orangematerial>
gazebo>
<gazebo reference="link2">
<mu1>0.2mu1>
<mu2>0.2mu2>
<material>Gazebo/Blackmaterial>
gazebo>
<gazebo reference="link3">
<mu1>0.2mu1>
<mu2>0.2mu2>
<material>Gazebo/Orangematerial>
gazebo>
<gazebo reference="camera_link">
<mu1>0.2mu1>
<mu2>0.2mu2>
<material>Gazebo/Redmaterial>
gazebo>
<gazebo reference="hokuyo_link">
<sensor type="ray" name="head_hokuyo_sensor">
<pose>0 0 0 0 0 0pose>
<visualize>falsevisualize>
<update_rate>40update_rate>
<ray>
<scan>
<horizontal>
<samples>720samples>
<resolution>1resolution>
<min_angle>-1.570796min_angle>
<max_angle>1.570796max_angle>
horizontal>
scan>
<range>
<min>0.10min>
<max>30.0max>
<resolution>0.01resolution>
range>
<noise>
<type>gaussiantype>
<mean>0.0mean>
<stddev>0.01stddev>
noise>
ray>
<plugin name="gazebo_ros_head_hokuyo_controller" filename="libgazebo_ros_laser.so">
<topicName>/robot/laser/scantopicName>
<frameName>hokuyo_linkframeName>
plugin>
sensor>
gazebo>
<gazebo reference="camera_link">
<sensor type="camera" name="camera1">
<update_rate>30.0update_rate>
<camera name="head">
<horizontal_fov>1.3962634horizontal_fov>
<image>
<width>800width>
<height>800height>
<format>R8G8B8format>
image>
<clip>
<near>0.02near>
<far>300far>
clip>
<noise>
<type>gaussiantype>
<mean>0.0mean>
<stddev>0.007stddev>
noise>
camera>
<plugin name="camera_controller" filename="libgazebo_ros_camera.so">
<alwaysOn>truealwaysOn>
<updateRate>0.0updateRate>
<cameraName>robot/camera1cameraName>
<imageTopicName>image_rawimageTopicName>
<cameraInfoTopicName>camera_infocameraInfoTopicName>
<frameName>camera_linkframeName>
<hackBaseline>0.07hackBaseline>
<distortionK1>0.0distortionK1>
<distortionK2>0.0distortionK2>
<distortionK3>0.0distortionK3>
<distortionT1>0.0distortionT1>
<distortionT2>0.0distortionT2>
plugin>
sensor>
gazebo>
<gazebo>
<plugin name="skid_steer_drive_controller" filename="libgazebo_ros_skid_steer_drive.so">
<alwaysOn>truealwaysOn>
<updateRate>100.0updateRate>
<robotNamespace>/robotNamespace>
<leftFrontJoint>base_to_wheel1leftFrontJoint>
<rightFrontJoint>base_to_wheel3rightFrontJoint>
<leftRearJoint>base_to_wheel2leftRearJoint>
<rightRearJoint>base_to_wheel4rightRearJoint>
<wheelSeparation>0.2wheelSeparation>
<wheelDiameter>0.1wheelDiameter>
<torque>2torque>
<commandTopic>cmd_velcommandTopic>
<odometryTopic>odomodometryTopic>
<odometryFrame>odomodometryFrame>
<robotBaseFrame>base_footprintrobotBaseFrame>
<broadcastTF>1broadcastTF>
plugin>
gazebo>
robot>launch文件如下：
<launch>
<arg name="paused" default="true"/>
<arg name="use_sim_time" default="true"/>
<arg name="gui" default="true"/>
<arg name="headless" default="false"/>
<arg name="debug" default="false"/>
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="$(find nav_sim)/urdf/simple.world"/>
<arg name="use_sim_time" value="$(arg use_sim_time)"/>
<arg name="debug" value="$(arg debug)" />
<arg name="gui" value="$(arg gui)" />
include>
<arg name="model" default="$(find nav_sim)/urdf/myrobot.xacro" />
<param name="robot_description" command="$(find xacro)/xacro.py $(arg model)" />
<node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
args="-urdf -model robot -param robot_description -z 0.05"/>
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find nav_sim)/nav.rviz"/>
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
launch>最终效果如以下几幅图所示。
第一幅图是机器人运行在gazebo环境中，采用键盘按键控制机器人实现简单的移动。启动键盘前需安装以下功能包：
$ sudo apt-get install ros-indigo-teleop-twist-keyboard
$ rosstack profile
$ rospack profile然后运行节点：
$ rosrun teleop_twist_keyboard teleop_twist_keyboard.py该节点把按键信息转化为Twist消息发送到cmd_vel主题上。
第二幅图在rviz中显示了机器人所采集到的激光雷达数据以及摄像头图像的可视化。gazebo节点分别向robot/laser/scan主题和robot/camera1/image_raw主题发布消息。这里有几个问题需要注意：
1.虚拟相机主题发布者仅更新图像信息而不是相机接口所需要的全部信息，所以在rviz中不能添加Camera来显示图像，应该采用Image来显示图像。
2.刚开始的时候在rviz中一直不能显示激光数据，原因在于launch文件中的时间和gazebo中的时间不一致，gazebo中使用的是虚拟时间，所以在launch文件中应设置use_sim_time=”true”。
第三幅显示了节点关系图。在模型刚建立的时候，小车能完整的在gazebo中显示，在rviz中显示却没有轮子，后面原因在于launch文件中没有启动joint_state_publisher节点，这种细节以后需注意！
