机器人自主导航控制指令的下发与接收

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节点，可以得到如下的节点图：