自制ROS移动小车——ROS编程基础知识(C++)

        ROS的上位机端基础，需要明白通讯的几种方式，如何订阅话题？查看订阅了什么话题？如何编写话题？话题如何发布？文章初次编写时间为2022年暑假，如今已经2024年11月，已完成大部分学业内容，通过这个系列来记录制作简单ROS小车的过程。使用的为ROS1 - Noetic，后续跟进ROS2。参考文章与视频如下：

ROS最基础的编程与导航内容参考赵老师：

首页 - Autolabor开源ROS机器人底盘 - 官方网站

如何快速搭建ROS环境？参考小鱼老师：

小鱼的一键安装系列 | 鱼香ROS (fishros.org.cn)

上下位机如何串通？参考小虎老师：

小虎哥哥爱学习-CSDN博客

---

前言：

        大题内容我想分成三个阶段，第一阶段，理解与明白ROS的通信与操作机制；第二阶段，搭建ROS仿真车，以前我一直觉得这块没什么用，其实错误了，仿真车和实车很多地方都类似，而且单独弄仿真也可以对算法部分有一些理解，后续也想学一些ROS + PX4 + 无人机的内容，部署RL，希望大佬指点；第三阶段是上下位机通信，这一部分也需要拆开，上半部分位下位机，使用STM32F1，这个系列是我们入门最常使用的系列，我想从标准库在KEIL5下实现，与HAL库在IDE下实现两个版本，也回顾一下32的内容，下半部分ROS端，如何从仿真到实车。

---

一. ROS通信机制——话题通信

        例如在生活中，我们在家里有一个放零食的箱子，而操作服务器就是这个箱子，无论是使用者还是开发者都可以共同使用他。

        话题通信 是ROS中使用频率最高的一种通信模式，话题通信是基于发布订阅模式的，也即:一个节点发布消息，另一个节点订阅该消息。这一部分也是最重要的内容：

                                                 雷达------->导航（计算）-------->底盘

        在传递消息和传递运动控制指令时，就使用了话题通信，我以小车控制为例子：你要对小车进行控制，你就需要发布一个 base_controller 的话题，将话题发布到odom，这样你的导航才知道你控制的里程计信息是多少，同时你也要订阅 /cmd_vel这个话题，这个话题会给你发送运动命令，例如键盘控制节点，你启动了它，订阅了这个话题，你就能知道他需要控制车怎么走，从这个话题中获取小车需要运动的命令，通过串口控制小车进行移动。

        实现模型是比较复杂的，这个模型你需要注意到三个角色：

• ROS Master (管理者)                         Talker (发布者)                          Listener (订阅者)

1.1 实现流程如下：

       1. Talker(发布者) 启动后，会通过RPC在 ROS Master 中注册自身信息，其中包含所发布消息的话题名称。ROS Master 会将节点的注册信息加入到注册表中。

       2. Listene(订阅者)启动后，也会通过RPC在 ROS Master 中注册自身信息，包含需要订阅消息的话题名。ROS Master 会将节点的注册信息加入到注册表中

        3.ROS Master 会根据注册表中的信息匹配Talker 和 Listener，并通过 RPC 向 Listener 发送 Talker 的 RPC 地址信息。

        4.Listener 根据接收到的 RPC 地址，通过 RPC 向 Talker 发送连接请求，传输订阅的话题名称、消息类型以及通信协议(TCP/UDP)。

        5.Talker 接收到 Listener 的请求后，也是通过 RPC 向 Listener 确认连接信息，并发送自身的 TCP 地址信息。

        6.Listener 根据步骤5返回的消息使用 TCP 与 Talker 建立网络连接。

1.2 代码实现（发布方）：

    //1.初始化 ROS 节点:命名(唯一)
    ros::init(argc, argv, "odom_caculate"); // ROS节点初始化
    ros::NodeHandle n;   //封装一些常用功能


    //2.实例化 发布者 对象
    //泛型: 发布的消息类型
    //参数1: 要发布到的话题
    //参数2: 队列中最大保存的消息数，超出此阀值时，先进的先销毁(时间早的先销毁)
    // 创建odom发布器
    ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);
    // 创建左右轮速度发布器
    ros::Publisher wheel_left_speed_pub = n.advertise<std_msgs::Float32>("wheel_left_speed", 50);
    ros::Publisher wheel_right_speed_pub = n.advertise<std_msgs::Float32>("wheel_right_speed", 50);

// 主循环
    while(ros::ok())
    {
        // 3.组织被发布的数据，并编写逻辑发布数据
        nav_msgs::Odometry odom;
        geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(myOdomCaculateData.oriention);

        odom.header.stamp = ros::Time::now();
        odom.header.frame_id = "odom";
        odom.pose.pose.position.x = myOdomCaculateData.position_x;
        odom.pose.pose.position.y = myOdomCaculateData.position_y;
        odom.pose.pose.orientation = odom_quat;

        odom.child_frame_id = "base_footprint";
        odom.twist.twist.linear.x = myOdomCaculateData.velocity_linear;
        odom.twist.twist.angular.z = myOdomCaculateData.velocity_angular;

        geometry_msgs::TransformStamped odom_trans;
        odom_trans.header.stamp = ros::Time::now();
        odom_trans.header.frame_id = "odom";
        odom_trans.child_frame_id = "base_footprint";
        odom_trans.transform.translation.x = myOdomCaculateData.position_x;
        odom_trans.transform.translation.y = myOdomCaculateData.position_y;
        odom_trans.transform.rotation = odom_quat;

        // 定义左右轮速度消息
        std_msgs::Float32 wheel_left_speed_msg;
        wheel_left_speed_msg.data = (myBaseSensorData.delta_encode_left) * (myOdomCaculateData.speed_ratio) / myOdomCaculateData.encode_sampling_time;
        std_msgs::Float32 wheel_right_speed_msg;
        wheel_right_speed_msg.data = (myBaseSensorData.delta_encode_right) * (myOdomCaculateData.speed_ratio) / myOdomCaculateData.encode_sampling_time;

        // 发布odom、左右轮速度和tf
        odom_pub.publish(odom);
        wheel_left_speed_pub.publish(wheel_left_speed_msg);
        wheel_right_speed_pub.publish(wheel_right_speed_msg);
        odom_broadcaster.sendTransform(odom_trans);

        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0;
}
    

1.3 订阅者

// ROS /cmd_vel消息回调函数，控制电机速度
void cmd_callback(const geometry_msgs::Twist &cmd_vel)
{
    // 线速度和角速度限幅
    float linear_temp = cmd_vel.linear.x;
    float angular_temp = cmd_vel.angular.z;

    if(linear_temp > myOdomCaculateData.cmd_vel_linear_max)
        linear_temp = myOdomCaculateData.cmd_vel_linear_max;
    else if(linear_temp < -myOdomCaculateData.cmd_vel_linear_max)
        linear_temp = -myOdomCaculateData.cmd_vel_linear_max;
    if(angular_temp > myOdomCaculateData.cmd_vel_angular_max)
        angular_temp = myOdomCaculateData.cmd_vel_angular_max;
    else if(angular_temp < -myOdomCaculateData.cmd_vel_angular_max)
        angular_temp = -myOdomCaculateData.cmd_vel_angular_max;
    // 计算左右轮速度
    float v_left = linear_temp - angular_temp * myOdomCaculateData.wheel_distance * 0.5;
    float v_right = linear_temp + angular_temp * myOdomCaculateData.wheel_distance * 0.5;
    int encode_left = v_left / myOdomCaculateData.speed_ratio;
    int encode_right = v_right / myOdomCaculateData.speed_ratio;

    // 填充发送缓冲区
    myComDev.writebuff[2]=(encode_left>0)?0x01:0x00;
    myComDev.writebuff[3]=(abs(encode_left)>>16)&0xff;
    myComDev.writebuff[4]=(abs(encode_left)>>8)&0xff;
    myComDev.writebuff[5]=(abs(encode_left))&0xff;
    myComDev.writebuff[6]=(encode_right>0)?0x01:0x00;
    myComDev.writebuff[7]=(abs(encode_right)>>16)&0xff;
    myComDev.writebuff[8]=(abs(encode_right)>>8)&0xff;
    myComDev.writebuff[9]=(abs(encode_right))&0xff;
    // 校验和
    myComDev.writebuff[10]=0;
    for(int i=0;i<10;i++)
        myComDev.writebuff[10]+=myComDev.writebuff[i];

    myComDev.send_update_flag=1; // 设置发送标志
}   
int main(int argc, char  *argv[])
{
   //1.初始化 ROS 节点:命名(唯一)
     ros::init(argc, argv, "odom_caculate"); // ROS节点初始化
     ros::NodeHandle n;

   //2.订阅/cmd_vel主题
     ros::Subscriber cmd_sub = n.subscribe("/cmd_vel", 1000, cmd_callback);

    ros::spin();//循环读取接收的数据，并调用回调函数处理

    return 0;
}
     

 1.4 自定义话题

        在 ROS 通信协议中，数据载体是一个较为重要组成部分，ROS 中通过 std_msgs 封装了一些原生的数据类型,比如:String、Int32、Int64、Char、Bool、Empty.... 但是，这些数据一般只包含一个 data 字段，结构的单一意味着功能上的局限性，当传输一些复杂的数据，比如: 激光雷达的信息... std_msgs 由于描述性较差而显得力不从心，这种场景下可以使用自定义的消息类型。

        ROS中还有一种特殊类型：Header，标头包含时间戳和ROS中常用的坐标帧信息。会经常看到msg文件的第一行具有 Header。

        定义后使用的方法和msgs一样。

—————————————————————————————————————

二. ROS通信机制——服务通信

        服务通信也是ROS中一种极其常用的通信模式，服务通信是基于请求响应模式的，是一种应答机制。也即: 一个节点A向另一个节点B发送请求，B接收处理请求并产生响应结果返回给A。

• ROS master(管理者)                   Server(服务端）                    Client(客户端)

        ROS Master 负责保管 Server 和 Client 注册的信息，并匹配话题相同的 Server 与 Client ，帮助 Server 与 Client 建立连接，连接建立后，Client 发送请求信息，Server 返回响应信息。

定义srv 文件内的可用数据类型与 msg 文件一致，且定义 srv 实现流程与自定义 msg相似，具体内容如下：

/*
    需求: 
        编写两个节点实现服务通信，客户端节点需要提交两个整数到服务器
        服务器需要解析客户端提交的数据，相加后，将结果响应回客户端，
        客户端再解析

    服务器实现:
        1.包含头文件
        2.初始化 ROS 节点
        3.创建 ROS 句柄
        4.创建 服务 对象
        5.回调函数处理请求并产生响应
        6.由于请求有多个，需要调用 ros::spin()

*/
#include "ros/ros.h"
#include "demo03_server_client/AddInts.h"

// bool 返回值由于标志是否处理成功
bool doReq(demo03_server_client::AddInts::Request& req,
          demo03_server_client::AddInts::Response& resp){
    int num1 = req.num1;
    int num2 = req.num2;

    ROS_INFO("服务器接收到的请求数据为:num1 = %d, num2 = %d",num1, num2);

    //逻辑处理
    if (num1 < 0 || num2 < 0)
    {
        ROS_ERROR("提交的数据异常:数据不可以为负数");
        return false;
    }

    //如果没有异常，那么相加并将结果赋值给 resp
    resp.sum = num1 + num2;
    return true;


}

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    // 2.初始化 ROS 节点
    ros::init(argc,argv,"AddInts_Server");
    // 3.创建 ROS 句柄
    ros::NodeHandle nh;
    // 4.创建 服务 对象
    ros::ServiceServer server = nh.advertiseService("AddInts",doReq);
    ROS_INFO("服务已经启动....");
    //     5.回调函数处理请求并产生响应
    //     6.由于请求有多个，需要调用 ros::spin()
    ros::spin();
    return 0;
}

---

三. ROS通信机制——参数服务器

参数服务器实现是最为简单的，该模型如下图所示,该模型中涉及到三个角色:

• ROS Master (管理者)
• Talker           (参数设置者)
• Listener        (参数调用者)

        ROS Master 作为一个公共容器保存参数，Talker 可以向容器中设置参数，Listener 可以获取参数。

        在 C++ 中实现参数服务器数据的增删改查，可以通过两套 API 实现，这里只说明一套，简单来说，这个就是设置命令，你可以使用rqt工具来更换实时参数值。

3.1.1 参数服务器

/*
    参数服务器操作之新增与修改(二者API一样)_C++实现:
    在 roscpp 中提供了两套 API 实现参数操作
    ros::NodeHandle
        setParam("键",值)
    ros::param
        set("键","值")

    示例:分别设置整形、浮点、字符串、bool、列表、字典等类型参数
        修改(相同的键，不同的值)

*/
#include "ros/ros.h"

int main(int argc, char *argv[])
{
    ros::init(argc,argv,"set_update_param");

    std::vector<std::string> stus;
    stus.push_back("zhangsan");
    stus.push_back("李四");
    stus.push_back("王五");
    stus.push_back("孙大脑袋");

    std::map<std::string,std::string> friends;
    friends["guo"] = "huang";
    friends["yuang"] = "xiao";

    //NodeHandle--------------------------------------------------------
    ros::NodeHandle nh;
    nh.setParam("nh_int",10); //整型
    nh.setParam("nh_double",3.14); //浮点型
    nh.setParam("nh_bool",true); //bool
    nh.setParam("nh_string","hello NodeHandle"); //字符串
    nh.setParam("nh_vector",stus); // vector
    nh.setParam("nh_map",friends); // map

    //修改演示(相同的键，不同的值)
    nh.setParam("nh_int",10000);

    //param--------------------------------------------------------
    ros::param::set("param_int",20);
    ros::param::set("param_double",3.14);
    ros::param::set("param_string","Hello Param");
    ros::param::set("param_bool",false);
    ros::param::set("param_vector",stus);
    ros::param::set("param_map",friends);

    //修改演示(相同的键，不同的值)
    ros::param::set("param_int",20000);

    return 0;
}

四、常用的ROS命令

4.1 rostopic

        包含rostopic命令行工具，用于显示有关ROS 主题的调试信息，包括发布者，订阅者，发布频率和ROS消息

rostopic bw     显示主题使用的带宽
rostopic delay  显示带有 header 的主题延迟
rostopic echo   打印消息到屏幕
rostopic find   根据类型查找主题
rostopic hz     显示主题的发布频率
rostopic info   显示主题相关信息
rostopic list   显示所有活动状态下的主题
rostopic pub    将数据发布到主题
rostopic type   打印主题类型

4.1.1 rostopic list(-v)

        直接调用即可，控制台将打印当前运行状态下的主题名称

4.1.2 rostopic pub

        可以直接调用命令向订阅者发布消息，相当于直接sub，在文档中给出了一个例子，小乌龟案例的 订阅者 发布一条运动信息

rostopic pub /turtle1/cmd_vel geometry_msgs/Twist
 "linear:
  x: 1.0
  y: 0.0
  z: 0.0
angular:
  x: 0.0
  y: 0.0
  z: 2.0"
//只发布一次运动信息

rostopic pub -r 10 /turtle1/cmd_vel geometry_msgs/Twist
 "linear:
  x: 1.0
  y: 0.0
  z: 0.0
angular:
  x: 0.0
  y: 0.0
  z: 2.0"
// 以 10HZ 的频率循环发送运动信息

4.1.3 rostpic echo

        获取指定话题当前发布的消息

-----------------------------------------------

五、ROS通信机制实操

        通过ROS内置的turtlesim案例，结合已经介绍ROS命令获取节点、话题、话题消息、服务、服务消息与参数的信息，最终再以编码的方式实现乌龟运动的控制、乌龟位姿的订阅、乌龟生成与乌龟窗体背景颜色的修改。最主要的内容，就是如何发布获取命令，并控制小乌龟进行运动。

5.1 话题发布

        乌龟运动控制实现，关键节点有两个，一个是乌龟运动显示节点turtlesim_node,另一个控制节点，二者是订阅发布模式实现通信，乌龟运动显示节点直接调用即可，运动控制节点是使用的，turtle_teleop_key通过键盘 控制，现在需要自定义控制节点。

        控制节点实现时，需要了解控制节点与显示节点通信使用的话题与命令，可以使用ros命令结合计算图来获取。

5.1.1 话题获取

        和小车类似，我们需要获取到话题，这里可以通过rqt工具来获得，或通过rostopic列出话题

rqt_graph       rostopic list

5.1.2 消息获取

        获取消息类型:geometry_msgs/Twist

rostopic echo /turtle1/cmd_vel

 geometry的获取内容为线速度和角速度，分别为x,y,z对应的速度 m/s 与 rad/s

5.2 实现发布节点

        创建功能包需要依赖的功能包: roscpp  std_msgs geometry_msgs，实现内容如下

#include "ros/ros.h"
#include "geometry_msgs/Twist.h"

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    // 2.初始化 ROS 节点
    ros::init(argc,argv,"control");
    ros::NodeHandle nh;
    // 3.创建发布者对象
    ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel",1000);
    // 4.循环发布运动控制消息
    //4-1.组织消息
    geometry_msgs::Twist msg;
    msg.linear.x = 1.0;
    msg.linear.y = 0.0;
    msg.linear.z = 0.0;

    msg.angular.x = 0.0;
    msg.angular.y = 0.0;
    msg.angular.z = 2.0;

    //4-2.设置发送频率
    ros::Rate r(10);
    //4-3.循环发送
    while (ros::ok())
    {
        pub.publish(msg);

        ros::spinOnce();
    }


    return 0;
}

        上面的代码和我们第一章的代码很相似，利用geometry::Twist的命令格式发送消息，在下面对xyz三轴进行控制，这样就可以把设定的路径发送给 /turtle/cmd_vel。

5.3 话题订阅

        已知turtlesim中的乌龟显示节点，会发布当前乌龟的位姿(窗体中乌龟的坐标以及朝向)，要求控制乌龟运动，并时时打印当前乌龟的位姿。换回到我们的小车控制，键盘控制节点发布后，我们也需要订阅到控制命令。

        创建功能包需要依赖的功能包: roscpp std_msgs turtlesim

/*  
    订阅小乌龟的位姿: 时时获取小乌龟在窗体中的坐标并打印
    准备工作:
        1.获取话题名称 /turtle1/pose
        2.获取消息类型 turtlesim/Pose
        3.运行前启动 turtlesim_node 与 turtle_teleop_key 节点

    实现流程:
        1.包含头文件
        2.初始化 ROS 节点
        3.创建 ROS 句柄
        4.创建订阅者对象
        5.回调函数处理订阅的数据
        6.spin
*/

#include "ros/ros.h"
#include "turtlesim/Pose.h"

void doPose(const turtlesim::Pose::ConstPtr& p){
    ROS_INFO("乌龟位姿信息:x=%.2f,y=%.2f,theta=%.2f,lv=%.2f,av=%.2f",
        p->x,p->y,p->theta,p->linear_velocity,p->angular_velocity
    );
}

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    // 2.初始化 ROS 节点
    ros::init(argc,argv,"sub_pose");
    // 3.创建 ROS 句柄
    ros::NodeHandle nh;
    // 4.创建订阅者对象
    ros::Subscriber sub = nh.subscribe<turtlesim::Pose>("/turtle1/pose",1000,doPose);
    // 5.回调函数处理订阅的数据
    // 6.spin
    ros::spin();
    return 0;
}

        主要的内容还是订阅小乌龟的位姿