1.通信协议

控制指令格式(15字节)

2.ROS端

在ROS端，首先需要接收从其他节点的控制消息，在ROS中常常使用geometry_msgs::Twist来传递控制指令，该消息格式包括两个三维向量，如下，分别是三轴线速度和三轴角速度：

geometry_msgs/Vector3 linear

geometry_msgs/Vector3 angular

我们在控制指令的消息回调函数中，将控制指令下发给STM32，部分程序如下，其中使用了C++的lambda表达式来替换回调函数

ros::Subscriber sub = nh.subscribe< geometry_msgs::Twist >("/cmd_vel", 10, [&](const geometry_msgs::Twist::ConstPtr& cmd_vel){

    uint8_t _cnt = 0;

    data_u _temp; // 声明一个联合体实例，使用它将待发送数据转换为字节数组

    uint8_t data_to_send[100]; // 待发送的字节数组    

    data_to_send[_cnt++]=0xAA;

    data_to_send[_cnt++]=0x55;

    uint8_t _start = _cnt;

    float datas[] = {(float)cmd_vel- >linear.x,

                     (float)cmd_vel- >linear.y,

                     (float)cmd_vel- >angular.z};    

    for(int i = 0; i < sizeof(datas) / sizeof(float); i++){

        // 将要发送的数据赋值给联合体的float成员

        // 相应的就能更改字节数组成员的值

        _temp.data = datas[i];

        data_to_send[_cnt++]=_temp.data8[0];

        data_to_send[_cnt++]=_temp.data8[1];

        data_to_send[_cnt++]=_temp.data8[2];

        data_to_send[_cnt++]=_temp.data8[3]; // 最高位

    }

    // 添加校验码

    char checkout = 0;

    for(int i = _start; i < _cnt; i++)

        checkout += data_to_send[i];

    data_to_send[_cnt++] = checkout;

    // 串口发送

    ser.write(data_to_send, _cnt);

});

最后，创建一个控制指令发布节点，用来发布cmd_vel话题，在xrobot功能包下新建一个scripts文件夹，添加remote_ctrl.py，内容如下：

复制

#!/usr/bin/env python

# #-*- coding: UTF-8 -*- 

import rospy

from geometry_msgs.msg import Twist

from std_msgs.msg import String

import os, sys

import  tty, termios

pub = rospy.Publisher("cmd_vel", Twist)

rospy.init_node("remote_ctrl")

rate = rospy.Rate(rospy.get_param("-hz", 20))

cmd = Twist()

cmd.linear.x = 0.0

cmd.linear.y = 0.0

cmd.angular.z = 0.0

while not rospy.is_shutdown():    

    tty.setraw(sys.stdin.fileno())  

    ch = sys.stdin.read( 1 )  

    if ch == "w":

        cmd.linear.x = 0.2

        cmd.linear.y = 0

        cmd.angular.z = 0

    elif ch == "s":

        cmd.linear.x = -0.2

        cmd.linear.y = 0

        cmd.angular.z = 0

    elif ch == "a":

        cmd.linear.x = 0

        cmd.linear.y = 0

        cmd.angular.z = 0.5

    elif ch == "d":

        cmd.linear.x = 0

        cmd.linear.y = 0

        cmd.angular.z = -0.5

    elif ch == "q":

        break

    else:

        cmd.linear.x = 0

        cmd.linear.y = 0

        cmd.angular.z = 0

    rospy.loginfo(str( cmd.linear.x) + " " + str(cmd.linear.y) + " " + str(cmd.angular.z) + "rn")

    pub.publish(cmd)

    rate.sleep()

运行robot_node和remote_ctrl节点，可以得到如下的节点图：