最终效果展示

这里的效果还是可以继续改进的，比如落点的位置可以进行调整，这样可以实现始终跟随在屁股背后，我已经实现可以在我的主页中找到，这里实现的仅仅是最简单的能跟上。后续想改进的话可以继续调整机器人的结构，参数等等，因为我发现有时候连直线都走不好，这应该和结构参数设置有关系，另一方面还可以跟换路径规划的算法以实现最短路径什么的。
改善后的在本篇博客：https://blog.csdn.net/m0_71523511/article/details/135675446

一、总体launch文件

<launch><!-- 启动Gazebo世界,这里直接用别人建好的仿真环境 --><include file="$(find gazebo_ros)/launch/empty_world.launch"><arg name="world_name" value="$(find urdf02_gazebo)/worlds/box_house.world" /></include><!-- 打开已经保存的地图，由雷达探测出来的地图文件 --><include file="$(find nav_demo)/launch/nav03_readmap.launch" /><!-- 机器人2命名空间，这个机器人需要在rviz中：通过键盘控制移动、不断发布自己相对于map坐标系的位置关系。所以关于雷达相关的参数这里没有因为如果加上雷达相关参数就会有问题，一般来说一个场景下只要有一个机器人能建图就好。这里之所以还使用gmapping来打开，是因为换成别的方式之后机器人无法显示。这里面robot_state_publisher、joint_state_publisher都是必须的，但是注意要控制一下频率，不然就会有很多警告，因为启动gazebo的代码中（urdf02_gazebo/urdf/gazebo/move.xacro）也会创建一个发布关节信息的节点(/joint_states)，会导致在同一时间内多个节点(/robot1/joint_state
s和/robot2/joint_states和/joint_states)同时发布相同数据 --><group ns="robot2"><param name="robot_description" command="$(find xacro)/xacro $(find urdf02_gazebo)/urdf2/car2.urdf.xacro" /><node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" /><node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher" ><param name="publish_frequency" value="10.0" /> </node><param name="use_sim_time" value="true"/> <!-- 选择使用仿真 --><!-- 机器人2的里程计坐标系：robot2/odom、基坐标系：robot2/base_footprint --><node pkg="gmapping" type="slam_gmapping" name="slam_gmapping2" output="screen"><param name="base_frame" value="robot2/base_footprint"/><param name="map_frame" value="map"/><param name="odom_frame" value="robot2/odom"/></node></group><!-- 机器人1的命名空间，机器人1需要实现导航功能所以需要包含amcl包，AMCL(adaptive Monte Carlo Localization) 是用于2D移动机器人的概率定位系统，它实现了自适应（或KLD采样）蒙特卡洛定位方法，可以根据已有地图使用粒子滤波器推算机器人位置。由于机器人1是需要路径规划的，所以要包含move_base，move_base 功能包提供了基于动作(action)的路径规划实现，move_base 可以根据给定的目标点，控制机器人底盘运动至目标位置，并且在运动过程中会连续反馈机器人自身的姿态与目标点的状态信息。move_base主要由全局路径规划与本地路径规划组成。move_base中包含的那些文件就是机器人的参数配置，该功能包面向的是各种类型支持ROS的机器人，不同类型机器人可能大小尺寸不同，传感器不同，速度不同，应用场景不同....最后可能会导致不同的路径规划结果。 --><group ns="robot1"><param name="robot_description" command="$(find xacro)/xacro $(find urdf02_gazebo)/urdf/car.urdf.xacro" /><node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" /><node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher" ><param name="publish_frequency" value="10.0" /> </node><include file="$(find nav_demo)/launch/nav04_amcl_t.launch" />    <!--  --><node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true"><!-- 该文件是move_base 在全局路径规划与本地路径规划时调用的通用参数，包括:机器人的尺寸、距离障碍物的安全距离、传感器信息等。 --><rosparam file="$(find nav_demo)/param/costmap_common_params_t.yaml" command="load" ns="global_costmap" /><rosparam file="$(find nav_demo)/param/costmap_common_params_t.yaml" command="load" ns="local_costmap" /><!-- 该文件用于局部代价地图参数设置 --><rosparam file="$(find nav_demo)/param/local_costmap_params_t.yaml" command="load" /><!-- 该文件用于全局代价地图参数设置 --><rosparam file="$(find nav_demo)/param/global_costmap_params_t.yaml" command="load" /><!-- 基本的局部规划器参数配置，这个配置文件设定了机器人的最大和最小速度限制值，也设定了加速度的阈值。 --><rosparam file="$(find nav_demo)/param/base_local_planner_params_t.yaml" command="load" /></node><!-- 在rviz中点击2D nav goal时就会将位置信息发布到这个话题中，这里将它remap一下好看些，在rviz中，我将2D nav goal订阅的话题给改了以免造成影响，后面机器人2就会将自己相对于map坐标系的位置发布到这个话题中，这样来控制机器人1向机器人2的位置运动 --><remap from="/move_base_simple/goal" to="/robot1/move_base_simple/goal"/><param name="use_sim_time" value="true"/><node pkg="gmapping" type="slam_gmapping" name="slam_gmapping1" output="screen"><remap from="scan" to="scan"/><param name="base_frame" value="robot1/base_footprint"/><param name="map_frame" value="map"/><param name="odom_frame" value="robot1/odom"/><param name="map_topic" value="map"/><param name="map_update_interval" value="5.0"/><param name="maxUrange" value="16.0"/><param name="sigma" value="0.05"/><param name="kernelSize" value="1"/><param name="lstep" value="0.05"/><param name="astep" value="0.05"/><param name="iterations" value="5"/><param name="lsigma" value="0.075"/><param name="ogain" value="3.0"/><param name="lskip" value="0"/><param name="srr" value="0.1"/><param name="srt" value="0.2"/><param name="str" value="0.1"/><param name="stt" value="0.2"/><param name="linearUpdate" value="1.0"/><param name="angularUpdate" value="0.5"/><param name="temporalUpdate" value="3.0"/><param name="resampleThreshold" value="0.5"/><param name="particles" value="30"/><param name="xmin" value="-50.0"/><param name="ymin" value="-50.0"/><param name="xmax" value="50.0"/><param name="ymax" value="50.0"/><param name="delta" value="0.05"/><param name="llsamplerange" value="0.01"/><param name="llsamplestep" value="0.01"/><param name="lasamplerange" value="0.005"/><param name="lasamplestep" value="0.005"/></node><!-- 其他robot1的节点，如gmapping等 --></group><!-- 使用gazebo_ros包中的spawn_model工具来在Gazebo仿真环境中生成机器人模型的节点。 --><node pkg="gazebo_ros" type="spawn_model" name="spawn_model_2" args="-urdf -model car2 -param robot2/robot_description" /><node pkg="gazebo_ros" type="spawn_model" name="spawn_model_1" args="-urdf -model car1 -param robot1/robot_description" /><!-- 启动RViz --><node pkg="rviz" type="rviz" name="rviz" args = "-d $(find nav_demo)/config/test.rviz" />
</launch>

1、打开已有地图

上述launch文件中包含了其它许多文件，放在下面：
nav_demo/launch/nav03_readmap.launch，打开已保存地图（创建和保存地图可以直接看文档教程7.2.1，7.2.2）：

<launch><!-- 设置地图的配置文件 --><arg name="map" default="nav.yaml" /><!-- 运行地图服务器，并且加载设置的地图--><node name="map_server" pkg="map_server" type="map_server" args="$(find nav_demo)/map/$(arg map)"/>
</launch>

2、组合小车的各个部分

urdf02_gazebo/urdf/car.urdf.xacro，组合小车的各个部分（这里只给出机器人1的，即进行跟随的小车）：

<!-- 组合小车底盘与摄像头与雷达 -->
<robot name="my_car_camera" xmlns:xacro="http://wiki.ros.org/xacro"><xacro:include filename="head.xacro" /><xacro:include filename="demo05_car_base.urdf.xacro" /><xacro:include filename="demo06_car_camera.urdf.xacro" /><xacro:include filename="demo07_car_laser.urdf.xacro" /><xacro:include filename="gazebo/move.xacro" /><xacro:include filename="gazebo/laser.xacro" /><xacro:include filename="gazebo/camera.xacro" /><xacro:include filename="gazebo/kinect.xacro" />
</robot>

2.1惯性矩阵设置

head.xacro（这下面这几个全都在urdf02_gazebo/gazebo目录下）：

<robot name="base" xmlns:xacro="http://wiki.ros.org/xacro"><!-- Macro for inertia matrix --><xacro:macro name="sphere_inertial_matrix" params="m r"><inertial><mass value="${m}" /><inertia ixx="${2*m*r*r/5}" ixy="0" ixz="0"iyy="${2*m*r*r/5}" iyz="0" izz="${2*m*r*r/5}" /></inertial></xacro:macro><xacro:macro name="cylinder_inertial_matrix" params="m r h"><inertial><mass value="${m}" /><inertia ixx="${m*(3*r*r+h*h)/12}" ixy = "0" ixz = "0"iyy="${m*(3*r*r+h*h)/12}" iyz = "0"izz="${m*r*r/2}" /> </inertial></xacro:macro><xacro:macro name="Box_inertial_matrix" params="m l w h"><inertial><mass value="${m}" /><inertia ixx="${m*(h*h + l*l)/12}" ixy = "0" ixz = "0"iyy="${m*(w*w + l*l)/12}" iyz= "0"izz="${m*(w*w + h*h)/12}" /></inertial></xacro:macro>
</robot>

2.2小车底盘

demo05_car_base.urdf.xacro：

<!--使用 xacro 优化 URDF 版的小车底盘实现：实现思路:1.将一些常量、变量封装为 xacro:property比如:PI 值、小车底盘半径、离地间距、车轮半径、宽度 ....2.使用 宏 封装驱动轮以及支撑轮实现，调用相关宏生成驱动轮与支撑轮-->
<!-- 根标签，必须声明 xmlns:xacro -->
<robot name="my_base" xmlns:xacro="http://www.ros.org/wiki/xacro"><!-- 封装变量、常量 --><!-- PI 值设置精度需要高一些，否则后续车轮翻转量计算时，可能会出现肉眼不能察觉的车轮倾斜，从而导致模型抖动 --><xacro:property name="PI" value="3.1415926"/><!-- 宏:黑色设置 --><material name="black"><color rgba="0.0 0.0 0.0 1.0" /></material><!-- 底盘属性 --><xacro:property name="base_footprint_radius" value="0.001" /> <!-- base_footprint 半径  --><xacro:property name="base_link_radius" value="0.1" /> <!-- base_link 半径 --><xacro:property name="base_link_length" value="0.08" /> <!-- base_link 长 --><xacro:property name="earth_space" value="0.015" /> <!-- 离地间距 --><xacro:property name="base_link_m" value="0.5" /> <!-- 质量  --><!-- 底盘 --><link name="robot1/base_footprint"><visual><geometry><sphere radius="${base_footprint_radius}" /></geometry></visual></link><link name="base_link"><visual><geometry><cylinder radius="${base_link_radius}" length="${base_link_length}" /></geometry><origin xyz="0 0 0" rpy="0 0 0" /><material name="yellow"><color rgba="0.5 0.3 0.0 0.5" /></material></visual><collision><geometry><cylinder radius="${base_link_radius}" length="${base_link_length}" /></geometry><origin xyz="0 0 0" rpy="0 0 0" /></collision><xacro:cylinder_inertial_matrix m="${base_link_m}" r="${base_link_radius}" h="${base_link_length}" /></link><joint name="base_link2base_footprint" type="fixed"><parent link="robot1/base_footprint" /><child link="base_link" /><origin xyz="0 0 ${earth_space + base_link_length / 2 }" /></joint><gazebo reference="base_link"><material>Gazebo/Yellow</material></gazebo><!-- 驱动轮 --><!-- 驱动轮属性 --><xacro:property name="wheel_radius" value="0.0325" /><!-- 半径 --><xacro:property name="wheel_length" value="0.015" /><!-- 宽度 --><xacro:property name="wheel_m" value="0.05" /> <!-- 质量  --><!-- 驱动轮宏实现 --><xacro:macro name="add_wheels" params="name flag"><link name="${name}_wheel"><visual><geometry><cylinder radius="${wheel_radius}" length="${wheel_length}" /></geometry><origin xyz="0.0 0.0 0.0" rpy="${PI / 2} 0.0 0.0" /><material name="black" /></visual><collision><geometry><cylinder radius="${wheel_radius}" length="${wheel_length}" /></geometry><origin xyz="0.0 0.0 0.0" rpy="${PI / 2} 0.0 0.0" /></collision><xacro:cylinder_inertial_matrix m="${wheel_m}" r="${wheel_radius}" h="${wheel_length}" /></link><joint name="${name}_wheel2base_link" type="continuous"><parent link="base_link" /><child link="${name}_wheel" /><origin xyz="0 ${flag * base_link_radius} ${-(earth_space + base_link_length / 2 - wheel_radius) }" /><axis xyz="0 1 0" /></joint><gazebo reference="${name}_wheel"><material>Gazebo/Red</material></gazebo></xacro:macro><xacro:add_wheels name="left" flag="1" /><xacro:add_wheels name="right" flag="-1" /><!-- 支撑轮 --><!-- 支撑轮属性 --><xacro:property name="support_wheel_radius" value="0.0075" /> <!-- 支撑轮半径 --><xacro:property name="support_wheel_m" value="0.03" /> <!-- 质量  --><!-- 支撑轮宏 --><xacro:macro name="add_support_wheel" params="name flag" ><link name="${name}_wheel"><visual><geometry><sphere radius="${support_wheel_radius}" /></geometry><origin xyz="0 0 0" rpy="0 0 0" /><material name="black" /></visual><collision><geometry><sphere radius="${support_wheel_radius}" /></geometry><origin xyz="0 0 0" rpy="0 0 0" /></collision><xacro:sphere_inertial_matrix m="${support_wheel_m}" r="${support_wheel_radius}" /></link><joint name="${name}_wheel2base_link" type="continuous"><parent link="base_link" /><child link="${name}_wheel" /><origin xyz="${flag * (base_link_radius - support_wheel_radius)} 0 ${-(base_link_length / 2 + earth_space / 2)}" /><axis xyz="1 1 1" /></joint><gazebo reference="${name}_wheel"><material>Gazebo/Red</material></gazebo></xacro:macro><xacro:add_support_wheel name="front" flag="1" /><xacro:add_support_wheel name="back" flag="-1" />
</robot>

2.3摄像头

demo06_car_camera.urdf.xacro：

<!-- 摄像头相关的 xacro 文件 -->
<robot name="my_camera" xmlns:xacro="http://wiki.ros.org/xacro"><!-- 摄像头属性 --><xacro:property name="camera_length" value="0.01" /> <!-- 摄像头长度(x) --><xacro:property name="camera_width" value="0.025" /> <!-- 摄像头宽度(y) --><xacro:property name="camera_height" value="0.025" /> <!-- 摄像头高度(z) --><xacro:property name="camera_x" value="0.08" /> <!-- 摄像头安装的x坐标 --><xacro:property name="camera_y" value="0.0" /> <!-- 摄像头安装的y坐标 --><xacro:property name="camera_z" value="${base_link_length / 2 + camera_height / 2}" /> <!-- 摄像头安装的z坐标:底盘高度 / 2 + 摄像头高度 / 2  --><xacro:property name="camera_m" value="0.01" /> <!-- 摄像头质量 --><!-- 摄像头关节以及link --><link name="camera"><visual><geometry><box size="${camera_length} ${camera_width} ${camera_height}" /></geometry><origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" /><material name="black" /></visual><collision><geometry><box size="${camera_length} ${camera_width} ${camera_height}" /></geometry><origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" /></collision><xacro:Box_inertial_matrix m="${camera_m}" l="${camera_length}" w="${camera_width}" h="${camera_height}" /></link><joint name="camera2base_link" type="fixed"><parent link="base_link" /><child link="camera" /><origin xyz="${camera_x} ${camera_y} ${camera_z}" /></joint><gazebo reference="camera"><material>Gazebo/Blue</material></gazebo>
</robot>

2.4雷达

demo07_car_laser.urdf.xacro：

<!--小车底盘添加雷达
-->
<robot name="my_laser" xmlns:xacro="http://wiki.ros.org/xacro"><!-- 雷达支架 --><xacro:property name="support_length" value="0.15" /> <!-- 支架长度 --><xacro:property name="support_radius" value="0.01" /> <!-- 支架半径 --><xacro:property name="support_x" value="0.0" /> <!-- 支架安装的x坐标 --><xacro:property name="support_y" value="0.0" /> <!-- 支架安装的y坐标 --><xacro:property name="support_z" value="${base_link_length / 2 + support_length / 2}" /> <!-- 支架安装的z坐标:底盘高度 / 2 + 支架高度 / 2  --><xacro:property name="support_m" value="0.02" /> <!-- 支架质量 --><link name="support"><visual><geometry><cylinder radius="${support_radius}" length="${support_length}" /></geometry><origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" /><material name="red"><color rgba="0.8 0.2 0.0 0.8" /></material></visual><collision><geometry><cylinder radius="${support_radius}" length="${support_length}" /></geometry><origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" /></collision><xacro:cylinder_inertial_matrix m="${support_m}" r="${support_radius}" h="${support_length}" /></link><joint name="support2base_link" type="fixed"><parent link="base_link" /><child link="support" /><origin xyz="${support_x} ${support_y} ${support_z}" /></joint><gazebo reference="support"><material>Gazebo/White</material></gazebo><!-- 雷达属性 --><xacro:property name="laser_length" value="0.05" /> <!-- 雷达长度 --><xacro:property name="laser_radius" value="0.03" /> <!-- 雷达半径 --><xacro:property name="laser_x" value="0.0" /> <!-- 雷达安装的x坐标 --><xacro:property name="laser_y" value="0.0" /> <!-- 雷达安装的y坐标 --><xacro:property name="laser_z" value="${support_length / 2 + laser_length / 2}" /> <!-- 雷达安装的z坐标:支架高度 / 2 + 雷达高度 / 2  --><xacro:property name="laser_m" value="0.1" /> <!-- 雷达质量 --><!-- 雷达关节以及link --><link name="laser"><visual><geometry><cylinder radius="${laser_radius}" length="${laser_length}" /></geometry><origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" /><material name="black" /></visual><collision><geometry><cylinder radius="${laser_radius}" length="${laser_length}" /></geometry><origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" /></collision><xacro:cylinder_inertial_matrix m="${laser_m}" r="${laser_radius}" h="${laser_length}" /></link><joint name="laser2support" type="fixed"><parent link="support" /><child link="laser" /><origin xyz="${laser_x} ${laser_y} ${laser_z}" /></joint><gazebo reference="laser"><material>Gazebo/Black</material></gazebo>
</robot>
2.5、gazebo/move.xacro：
<robot name="my_car_move" xmlns:xacro="http://wiki.

2.5为机器人模型添加传动装置以及控制器

gazebo/move.xacro：

<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="left_wheel2base_link" /><xacro:joint_trans joint_name="right_wheel2base_link" /><!-- 控制器 --><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>10.0</updateRate><legacyMode>true</legacyMode><leftJoint>left_wheel2base_link</leftJoint> <!-- 左轮 --><rightJoint>right_wheel2base_link</rightJoint> <!-- 右轮 --><wheelSeparation>${base_link_radius * 2}</wheelSeparation> <!-- 车轮间距 --><wheelDiameter>${wheel_radius * 2}</wheelDiameter> <!-- 车轮直径 --><broadcastTF>1</broadcastTF><wheelTorque>30</wheelTorque><wheelAcceleration>1.8</wheelAcceleration><commandTopic>robot1/cmd_vel</commandTopic> <!-- 运动控制话题 --><odometryFrame>robot1/odom</odometryFrame> <odometryTopic>robot1/odom</odometryTopic> <!-- 里程计话题 --><robotBaseFrame>robot1/base_footprint</robotBaseFrame> <!-- 根坐标系 --></plugin></gazebo>
</robot>

2.6为机器人模型添加雷达配置

gazebo/laser.xacro：

<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>/robot1/scan</topicName><frameName>laser</frameName></plugin></sensor></gazebo></robot>

2.7为机器人模型添加摄像头配置

gazebo/camera.xacro：

<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><!-- 被引用的link -->
</robot>

2.8为机器人模型添加kinect摄像头配置

gazebo/kinect.xacro：

<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_depth</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>

3、定位系统（amcl）

所谓定位就是推算机器人自身在全局地图中的位置，当然，SLAM中也包含定位算法实现，不过SLAM的定位是用于构建全局地图的，是属于导航开始之前的阶段，而当前定位是用于导航中，导航中，机器人需要按照设定的路线运动，通过定位可以判断机器人的实际轨迹是否符合预期。在ROS的导航功能包集navigation中提供了 amcl 功能包，用于实现导航中的机器人定位。AMCL(adaptive Monte Carlo Localization) 是用于2D移动机器人的概率定位系统，它实现了自适应（或KLD采样）蒙特卡洛定位方法，可以根据已有地图使用粒子滤波器推算机器人位置。只有机器人1需要使用定位系统。
nav_demo/launch/nav04_amcl_t.launch：

<launch><node pkg="amcl" type="amcl" name="amcl1" output="screen"><param name="odom_model_type" value="diff"/><param name="odom_alpha5" value="0.1"/><param name="gui_publish_rate" value="10.0"/><param name="laser_max_beams" value="30"/><param name="min_particles" value="500"/><param name="max_particles" value="5000"/><param name="kld_err" value="0.05"/><param name="kld_z" value="0.99"/><param name="odom_alpha1" value="0.2"/><param name="odom_alpha2" value="0.2"/><param name="odom_alpha3" value="0.8"/><param name="odom_alpha4" value="0.2"/><param name="laser_z_hit" value="0.5"/><param name="laser_z_short" value="0.05"/><param name="laser_z_max" value="0.05"/><param name="laser_z_rand" value="0.5"/><param name="laser_sigma_hit" value="0.2"/><param name="laser_lambda_short" value="0.1"/><param name="laser_model_type" value="likelihood_field"/><param name="laser_likelihood_max_dist" value="2.0"/><param name="update_min_d" value="0.2"/><param name="update_min_a" value="0.5"/><param name="odom_frame_id" value="robot1/odom"/><param name="base_frame_id" value="robot1/base_footprint"/><param name="resample_interval" value="1"/><param name="transform_tolerance" value="0.1"/><param name="recovery_alpha_slow" value="0.0"/><param name="recovery_alpha_fast" value="0.0"/></node>
</launch>

4、路径规划（move_base）

路径规划是导航中的核心功能之一，在ROS的导航功能包集navigation中提供了 move_base 功能包，用于实现此功能。move_base 功能包提供了基于动作(action)的路径规划实现，move_base 可以根据给定的目标点，控制机器人底盘运动至目标位置，并且在运动过程中会连续反馈机器人自身的姿态与目标点的状态信息。只有机器人1需要用到路径规划。

4.1全局路径规划与本地路径规划调用的通用参数

nav_demo/param/costmap_common_params_t.yaml：

#机器人几何参，如果机器人是圆形，设置 robot_radius,如果是其他形状设置 footprint
robot_radius: 0.12 #圆形
# footprint: [[-0.12, -0.12], [-0.12, 0.12], [0.12, 0.12], [0.12, -0.12]] #其他形状obstacle_range: 3.0 # 用于障碍物探测，比如: 值为 3.0，意味着检测到距离小于 3 米的障碍物时，就会引入代价地图
raytrace_range: 3.5 # 用于清除障碍物，比如：值为 3.5，意味着清除代价地图中 3.5 米以外的障碍物#膨胀半径，扩展在碰撞区域以外的代价区域，使得机器人规划路径避开障碍物
inflation_radius: 0.2
#代价比例系数，越大则代价值越小
cost_scaling_factor: 3.0#地图类型
map_type: costmap
#导航包所需要的传感器
observation_sources: robot1/scan
#对传感器的坐标系和数据进行配置。这个也会用于代价地图添加和清除障碍物。例如，你可以用激光雷达传感器用于在代价地图添加障碍物，再添加kinect用于导航和清除障碍物。
scan: {sensor_frame: robot1/laser, data_type: LaserScan, topic: robot1/scan, marking: true, clearing: true}

4.2全局代价地图参数设置

nav_demo/param/global_costmap_params_t.yaml:

global_costmap:global_frame: map #地图坐标系robot_base_frame: robot1/base_footprint #机器人坐标系# 以此实现坐标变换update_frequency: 1.0 #代价地图更新频率publish_frequency: 1.0 #代价地图的发布频率transform_tolerance: 0.5 #等待坐标变换发布信息的超时时间static_map: true # 是否使用一个地图或者地图服务器来初始化全局代价地图，如果不使用静态地图，这个参数为false.

4.3局部代价地图参数设置

nav_demo/param/local_costmap_params_t.yaml：

local_costmap:global_frame: robot1/odom #里程计坐标系robot_base_frame: robot1/base_footprint #机器人坐标系update_frequency: 10.0 #代价地图更新频率publish_frequency: 10.0 #代价地图的发布频率transform_tolerance: 0.5 #等待坐标变换发布信息的超时时间static_map: false  #不需要静态地图，可以提升导航效果rolling_window: true #是否使用动态窗口，默认为false，在静态的全局地图中，地图不会变化width: 3 # 局部地图宽度 单位是 mheight: 3 # 局部地图高度 单位是 mresolution: 0.05 # 局部地图分辨率 单位是 m，一般与静态地图分辨率保持一致

4.4基本的局部规划器参数配置

nav_demo/param/base_local_planner_params_t.yaml：

TrajectoryPlannerROS:# Robot Configuration Parametersmax_vel_x: 0.5 # X 方向最大速度min_vel_x: 0.1 # X 方向最小速速max_vel_theta:  1.0 # min_vel_theta: -1.0min_in_place_vel_theta: 1.0acc_lim_x: 1.0 # X 加速限制acc_lim_y: 0.0 # Y 加速限制acc_lim_theta: 0.6 # 角速度加速限制# Goal Tolerance Parameters，目标公差xy_goal_tolerance: 0.10yaw_goal_tolerance: 0.05# Differential-drive robot configuration
# 是否是全向移动机器人holonomic_robot: false# Forward Simulation Parameters，前进模拟参数sim_time: 1.5vx_samples: 18vtheta_samples: 20sim_granularity: 0.05

二、目标跟随节点（坐标变换）

监听机器人2base_link2与map的坐标变换，并将其发布至/robot1/move_base_simple/goal，即可控制机器人1运动（这里监听map与base_link2的原因是/robot2/base_footprint坐标系会随着机器人运动而变换位置，经过观察发现base_link2不会乱跳，这里没找到原因，应该是某些地方没配置好或者冲突了）：

#include"ros/ros.h"
#include"tf2_ros/transform_listener.h"
#include"tf2_ros/buffer.h"
#include"nav_msgs/Odometry.h"
#include"geometry_msgs/PoseStamped.h"
#include"tf2_geometry_msgs/tf2_geometry_msgs.h"
#include"geometry_msgs/TransformStamped.h"
#include"geometry_msgs/Twist.h"int main(int argc, char *argv[])
{ros::init(argc,argv,"test03_control_turtle2");ros::NodeHandle nh;tf2_ros::Buffer buffer;tf2_ros::TransformListener sub(buffer);ros::Publisher pub = nh.advertise<geometry_msgs::PoseStamped>("/robot1/move_base_simple/goal",100);ros::Rate rate(0.5);while (ros::ok()){try{geometry_msgs::TransformStamped son1toson2 = buffer.lookupTransform("map","base_link2",ros::Time(0));ROS_INFO("son1xiangduiyuson2 : pianyiliang(%.2f,%.2f)",son1toson2.transform.translation.x,son1toson2.transform.translation.y);geometry_msgs::PoseStamped odometry;odometry.header.frame_id = "robot1/odom";odometry.pose.position.x = (son1toson2.transform.translation.x-0.5);odometry.pose.position.y = (son1toson2.transform.translation.y);odometry.pose.position.z = 0;odometry.pose.orientation.w = 1.0;ROS_INFO("msg = (%.2f,%.2f)",odometry.pose.position.x,odometry.pose.position.y);pub.publish(odometry);}catch(const std::exception& e){ROS_ERROR("Exception: %s", e.what());}rate.sleep();ros::spinOnce();}return 0;
}

三、键盘控制机器人2运动节点

机器人2控制节点，也可以使用ros自带的（rosrun teleop_twist_keyboard teleop_twist_keyboard.py，它发布到/cmd_vel话题）。

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <signal.h>
#include <stdio.h>
#include <fcntl.h>
#include <sys/select.h>
#ifndef _WIN32
# include <termios.h>
# include <unistd.h>
#else
# include <windows.h>
#endif#define KEYCODE_RIGHT 0x43
#define KEYCODE_LEFT 0x44
#define KEYCODE_UP 0x41
#define KEYCODE_DOWN 0x42
#define KEYCODE_Q 0x71class KeyboardReader
{
public:KeyboardReader()
#ifndef _WIN32: kfd(0)
#endif{
#ifndef _WIN32// get the console in raw modetcgetattr(kfd, &cooked);struct termios raw;memcpy(&raw, &cooked, sizeof(struct termios));raw.c_lflag &=~ (ICANON | ECHO);// Setting a new line, then end of fileraw.c_cc[VEOL] = 1;raw.c_cc[VEOF] = 2;tcsetattr(kfd, TCSANOW, &raw);
#endif}void readOne(char * c){
#ifndef _WIN32// 使用select来实现非阻塞读取fd_set readfds;struct timeval tv;FD_ZERO(&readfds);FD_SET(kfd, &readfds);// 设置超时为0，这使得select立即返回tv.tv_sec = 1;tv.tv_usec = 0;// 检查是否有输入int retval = select(kfd + 1, &readfds, NULL, NULL, &tv);if (retval == -1){perror("select()");return;}else if (retval){// 数据可读int rc = read(kfd, c, 1);if (rc < 0){throw std::runtime_error("read failed");}}else{// 没有数据*c = 0; // 无字符}
#elsefor(;;){HANDLE handle = GetStdHandle(STD_INPUT_HANDLE);INPUT_RECORD buffer;DWORD events;PeekConsoleInput(handle, &buffer, 1, &events);if(events > 0){ReadConsoleInput(handle, &buffer, 1, &events);if (buffer.Event.KeyEvent.wVirtualKeyCode == VK_LEFT){*c = KEYCODE_LEFT;return;}else if (buffer.Event.KeyEvent.wVirtualKeyCode == VK_UP){*c = KEYCODE_UP;return;}else if (buffer.Event.KeyEvent.wVirtualKeyCode == VK_RIGHT){*c = KEYCODE_RIGHT;return;}else if (buffer.Event.KeyEvent.wVirtualKeyCode == VK_DOWN){*c = KEYCODE_DOWN;return;}else if (buffer.Event.KeyEvent.wVirtualKeyCode == 0x42){*c = KEYCODE_B;return;}else if (buffer.Event.KeyEvent.wVirtualKeyCode == 0x43){*c = KEYCODE_C;return;}else if (buffer.Event.KeyEvent.wVirtualKeyCode == 0x44){*c = KEYCODE_D;return;}else if (buffer.Event.KeyEvent.wVirtualKeyCode == 0x45){*c = KEYCODE_E;return;}else if (buffer.Event.KeyEvent.wVirtualKeyCode == 0x46){*c = KEYCODE_F;return;}else if (buffer.Event.KeyEvent.wVirtualKeyCode == 0x47){*c = KEYCODE_G;return;}else if (buffer.Event.KeyEvent.wVirtualKeyCode == 0x51){*c = KEYCODE_Q;return;}else if (buffer.Event.KeyEvent.wVirtualKeyCode == 0x52){*c = KEYCODE_R;return;}else if (buffer.Event.KeyEvent.wVirtualKeyCode == 0x54){*c = KEYCODE_T;return;}else if (buffer.Event.KeyEvent.wVirtualKeyCode == 0x56){*c = KEYCODE_V;return;}}}
#endif}void shutdown(){
#ifndef _WIN32tcsetattr(kfd, TCSANOW, &cooked);
#endif}
private:
#ifndef _WIN32int kfd;struct termios cooked;
#endif
};KeyboardReader input;class TeleopTurtle
{
public:TeleopTurtle();void keyLoop();void getKeyPress(char& c);private:ros::NodeHandle nh_;double linear_, angular_, l_scale_, a_scale_;ros::Publisher twist_pub_;};TeleopTurtle::TeleopTurtle():linear_(0),angular_(0),l_scale_(2.0),a_scale_(2.0)
{nh_.param("scale_angular", a_scale_, a_scale_);nh_.param("scale_linear", l_scale_, l_scale_);twist_pub_ = nh_.advertise<geometry_msgs::Twist>("/cmd_vel22", 1);
}void quit(int sig)
{(void)sig;input.shutdown();ros::shutdown();exit(0);
}int main(int argc, char** argv)
{ros::init(argc, argv, "keyboard");// ros::Rate rate(10);  // 设置频率为1HzTeleopTurtle teleop_turtle;signal(SIGINT,quit);teleop_turtle.keyLoop();// rate.sleep(); // 等待以保持1Hz频率return(0);
}void TeleopTurtle::getKeyPress(char& c)
{try{input.readOne(&c);}catch (const std::runtime_error &){perror("read():");return;}linear_=angular_=0;ROS_DEBUG("value: 0x%02X\n", c);
}void TeleopTurtle::keyLoop()
{char c;bool dirty=false;puts("Reading from keyboard");puts("----------------------------------------------");puts(" ");puts("Use up and down keys to move the left  turtle.");puts("Use w  and s    keys to move the right turtle.");puts(" ");puts("----------------------------------------------");while (ros::ok()){// get the next event from the keyboard  getKeyPress(c);linear_ = angular_ = 0;switch(c){case KEYCODE_UP:linear_ = 0.3;dirty = true;break;case KEYCODE_DOWN:linear_ = -0.3;dirty = true;break;case KEYCODE_LEFT:angular_ = 1.0;dirty = true;break;case KEYCODE_RIGHT:angular_ = -1.0;dirty = true;break;default:angular_ = 0;linear_ = 0;break;}geometry_msgs::Twist twist2;twist2.angular.z = a_scale_*angular_;twist2.linear.x = l_scale_*linear_;twist_pub_.publish(twist2);}
}