（2）操作步骤

① 打开参考例程文件夹（src\Arduino_Programing\Control_FuLaiLun_Car），下载程序Control_FuLaiLun_Car.ino：

/*------------------------------------------------------------------------------------版权说明：Copyright 2023 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.Distributed under MIT license.See file LICENSE for detail or copy athttps://opensource.org/licenses/MITby 机器谱 2023-03-16 https://www.robotway.com/------------------------------*//***************************************************************************************************实验功能：驱动全向三轮底盘实现前进、后退、左转、右转、左平移、右平移实验接线：步进电机细分：16细分步进电机方向引脚：dir: x: 44, y: 32, z: 42, a: 30步进电机步进引脚：stp: x: 45, y: 33, z: 43, a: 31实验现象：上电后，小车前进3秒，停止5秒；后退3秒，停止5秒；左平移3秒，停止5秒；右平移3秒，停止5秒；左转3秒，停5秒，右转3秒，停5秒，小车停止。***************************************************************************************************///DUE控制版需要启用USE_USBON或USE_NATIVE_USB,UNO不需要//#define USE_USBCON          //PAOGRAMMING PORT#define USE_NATIVE_USB      //NATIVE USB PORT#include <ros.h>#include <ros/time.h>#include <geometry_msgs/Vector3.h>ros::NodeHandle   nh;#define Action_implement_delay 3000 //单个动作执行所用时间#define Action_wait_for_delay 5000   //动作与动作之间的间隔时间#define Set_Speed_Test 0.04void XYRun(double vx, double vy, double w);void messageCb( const geometry_msgs::Vector3& vel_cmd)   {XYSetVel(vel_cmd.x, vel_cmd.y, vel_cmd.z);}ros::Subscriber<geometry_msgs::Vector3> vel_cmd_sub("vel_cmd", &messageCb );geometry_msgs::Vector3 pose_message;ros::Publisher pose_feedback_pub("pose_feedback",&pose_message);geometry_msgs::Vector3 vel_message;ros::Publisher vel_feedback_pub("vel_feedback",&vel_message);const int kMessagePubRate = 5;unsigned long message_pub_time = 0;const int kReadMotorDeltaT = 50;unsigned long position_read_time = 0;float current_x = 0,current_y = 0,current_a = 0;float current_vx = 0,current_vy = 0,current_va = 0;void setup(){delay(1000);Serial.begin(57600);initMotor();//   Stepper_move_test();nh.initNode();nh.subscribe(vel_cmd_sub);nh.advertise(pose_feedback_pub);nh.advertise(vel_feedback_pub);XYSetVel(0.0,0.0,0.0);position_read_time = millis();message_pub_time = millis();}void loop(){XYRun();  if(millis()>position_read_time){XYRead();position_read_time += kReadMotorDeltaT;}if(millis()>message_pub_time){pose_message.x = current_x;pose_message.y = current_y;pose_message.z = current_a;vel_message.x = current_vx;vel_message.y = current_vy;vel_message.z = current_va;pose_feedback_pub.publish(&pose_message);vel_feedback_pub.publish(&vel_message);message_pub_time+=1000.0/kMessagePubRate;}nh.spinOnce();  }

② 启动雷达、键盘、rosserial程序包。打开终端,输入roslaunch robot_navigation_control robot.launch命令（见下图），等待程序的运行启动界面

成功启动后，可以在终端看到底盘小车的当前speed（线速度）为0.04，turn（角速度）为0.08。【注意：speed指的是线速度的大小（包含方向），turn指的是角速度的大小（包含方向）】

启动后的界面（见下图）:

③ 尝试按下键盘命令，控制底盘小车的运动。【注意：一、请先保证终端是激活状态，见上图；二、按键盘时，稍微有点间隔，这样能给底盘小车转动的时间；三、一定要打开扩展板上的电源开关，否则会发现小车位置在rviz中位置虽然变化，但车不会运动】

假如现在希望先增加底盘小车的最大速度，再降低底盘小车的最大速度，则分别按下键盘上q、z命令，便可以观察到终端的结果（见下图）。

更多命令请自行尝试。

3.2 底盘小车同步定位与建图-SLAM

（1）实现思路 

利用Gmapping算法对底盘小车所在的未知环境进行建立地图、同步定位、最后保存此地图。此地图可供后续底盘小车导航使用。

Gmapping算法是目前基于激光雷达和里程计方案里面比较可靠和成熟的一个算法， 它基于粒子滤波，采用RBPF的方法，效果稳定。本次三轮全向底盘小车slam建图采用的是gmapping_slam包。下面介绍Gmapping SLAM计算图。Gmapping的作用是根据激光雷达和里程计（Odometry）的信息，对环境地图进行构建，并且对自身状态进行估计。因此它得输入应当包括激光雷达和里程计的数据，而输出应当有自身位置和地图。下面我们从计算图（消息的流向）的角度来看看gmapping算法在实际运行中的结构：

位于中心的是我们运行的slam_gmapping节点，这个节点负责整个gmapping SLAM的工作。它的输入需要有两个：

输入

/tf以及/tf_static：坐标变换，类型为第一代的tf/tfMessage或第二代的 tf2_msgs/TFMessage 其中一定得提供的有两个tf，一个是base_frame 与 laser_frame 之间的tf，即机器人底盘和激光雷达之间的变换；一个是base_frame 与 odom_frame之间的tf，即底盘和里程计原点之间的坐标变换。odom_frame可以理解为里程计原点所在的坐标系。

/scan：激光雷达数据，类型为sensor_msgs/LaserScan

/scan很好理解，Gmapping SLAM所必须的激光雷达数据，而/tf 是一个比较容易忽 视的细节。尽管/tf 这个Topic听起来很简单，但它维护了整个ROS三维世界里的转换关系，而 slam_gmapping要从中读取的数据是base_frame 与 laser_frame 之间的tf，只有这样才能够把周围障碍物变换到机器人坐标系下，更重要的是base_frame 与 odom_frame之间的tf，这个tf 反映了里程计（电机的光电码盘、视觉里程计、IMU）的监测数据，也就是机器人里程计测得走了多少距离，它会把这段变换发布到odom_frame 和 laser_frame 92之间。因此slam_gmapping 会从/tf 中获得机器人里程计的数据。

输出

/tf：主要是输出 map_frame 和 odom_frame 之间的变换

/slam_gmapping/entropy：std_msgs/Float64 类型，反映了机器人位姿估计的分 散程度

/map：slam_gmapping 建立的地图

/map_metadata：地图的相关信息

输出的/tf 里又一个很重要的信息，就是map_frame和odom_frame之间的变换，这 其实就是对机器人的定位。通过连通map_frame和odom_frame，这样map_frame与 base_frame 甚至与 laser_frame 都连通了。这样便实现了机器人在地图上的定位。

（2）操作步骤

① 先搭建一个实际场景，便于建立地图。【注意：为了方便识别，搭建的实际测试地图最好是不规则的区域。如下图测试的场地，在长方形区域的一边是不规则的，这样便于识别建立的仿真地图】

② 启动雷达、键盘、rosserial；打开终端并输入命令：roslaunch robot_navigation_control robot_car.launch （见下图）

③ 启动构建地图服务。按（ctrl+shift+T）打开第二个终端并输入：roslaunch four_macnum_slam four_macnum_slam.launch（见下图）

界面启动后，在rviz中选择“map”，可以在可视化界面看到有地图出现。下面根据实际环境进行建图：

【注意：一、请先保证键盘控制的终端是激活状态（请用鼠标点击一下此终端）；二、按键盘时，稍微有点间隔，这样能给底盘小车转动的时间】

按下键盘相关指令（键盘指令请参考3.1）来控制底盘小车在场地内运行，直至在Rviz内可以看到构建地图的轮廓，如下图所示（注意：地图环境会根据实际场景建立）。

④ 为了后续在已知环境中运行底盘小车，需把此次建立的地图进行保存。需要先进入保存地图的文件夹，然后给地图命名，保存即可。例如我们把此次建立的地图命名为your_map_name。【提示：your_map_name为自定义的名称，您也可以按自己的需求进行命名】

下面是本实验中的具体操作

重新打开新终端（ctrl+shift+t）并输入：cd fourmacnum_car_ws/src/four_macnum_navigation/maps

之后输入：rosrun map_server map_saver -f ./your_map_name

返回下图红色框的信息，代表成功保存地图。

查看生成的地图文件：

上面我们用map_server来保存地图。这里我们来简单了解和查看一下刚才保存的地图文件（见下图）。其中：map_server是一个和地图相关的功能包，它可以将已知地图发布出来，供导航和其他功能使用，也可以保存SLAM建立的地图。

地图文件，通常为pgm格式；

地图的描述文件，通常为yaml格式

打开your_map_name.pgm，见下图：

your_map_name.yaml ，各个参数见下图。

其中占据的概率 occ = (255-color_avg)/255.0，color_avg为RGB三个通道的平均值。

3.3 底盘小车导航-Navigation

（1）实现思路

在上述3.2搭建好的实际场景中，实现三轮全向底盘小车的导航效果。

• 具体思路：加载上述3.2建好的地图，设置底盘小车的初始位置和方向，设定目标位置及方向，底盘小车就可以实现导航效果。
• Navigation与底盘关系：ROS的二维导航功能包，简单来说，就是根据输入的里程计等传感器的信息流和机器人的全局位置，通过导航算法，计算得出安全可靠的机器人速度控制指令。但是如何在特定的机器人上实现导航功能包的功能，却是一件较为复杂的工程，作为导航功能包使用的必要先决条件，机器人必须运行ROS，发布tf变换树，并发布使用ROS消息类型的传感器数据。同时为了让机器人更好的完成导航任务，开发者还要根据机器人的外形尺寸和性能， 配置导航功能包的一些参数。

在ROS中进行导航需要使用到的三个包是：

① move_base：根据参照的消息进行路径规划，使移动机器人到达指定的位置；

② gmapping：根据激光数据（或者深度数据模拟的激光数据）建立地图；

③ amcl：根据已经有的地图进行定位。

基于上图，我们来看看底盘简单对应关系。

（2）操作步骤

① 将创建的地图应用到导航程序中。

这里我们需要修改four_macnum_navigation.launch文件，把上述3.2的your_map_name.yaml 添加到此launch文件中。步骤如下：

打开终端并输入：cd fourmacnum_car_ws/src/four_macnum_navigation/launch

之后再输入：gedit four_macnum_navigation.launch

② 启动雷达、键盘、rosserial。

在该终端继续输入：roslaunch robot_navigation_control robot_car.launch（见下图）

③ 按下（ctrl+shift+T）打开第二个终端并输入：roslaunch four_macnum_navigation four_macnum_navigation.launch（见下图）

启动界面后，可以看到许多红色的箭头，代表底盘小车的姿态估计还不准确。

④ 开始调整底盘小车的姿态。

第一步：请先旋转地图，确保地图和实际场景的方向基本一致。

第二步：使用2D Pose Estimate 标定底盘小车位于地图中的初始位置及车头指向（见下图）。

第三步：鼠标点击有roslaunch robot_navigation_control robot_car.launch的终端后，尝试按下键盘命令控制底盘小车运动（键盘命令请参考3.1），最好尽可能多的消除地图中的箭头【这一步实际是确定底盘小车在地图中的实际位置以及车头指向】。下图是多次按下键盘命令控制底盘小车运动后，箭头逐渐减少的效果。

下图是最后调整完的底盘小车姿态（即底盘小车在地图中的实际位置以及车头指向）。

尝试给定三轮全向底盘小车目标位置。使用2D Nav Goal，这里的目标位置包含了目标点及车头指向，在可视化界面点击且转动箭头防线，机器人会移动到指定点的位置。

4. 资料下载

三轮全向底盘-SLAM导航

①slam导航-例程源代码

②slam导航-样机3D文件

资料内容详见：三轮全向底盘-SLAM导航