1.通信協(xié)議

控制指令格式(15字節(jié))

2.ROS端

在ROS端，首先需要接收從其他節(jié)點的控制消息，在ROS中常常使用geometry_msgs::Twist來傳遞控制指令，該消息格式包括兩個三維向量，如下，分別是三軸線速度和三軸角速度：

geometry_msgs/Vector3 linear
geometry_msgs/Vector3 angular

我們在控制指令的消息回調(diào)函數(shù)中，將控制指令下發(fā)給STM32，部分程序如下，其中使用了C++的lambda表達式來替換回調(diào)函數(shù)

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; // 聲明一個聯(lián)合體實例，使用它將待發(fā)送數(shù)據(jù)轉(zhuǎn)換為字節(jié)數(shù)組
    uint8_t data_to_send[100]; // 待發(fā)送的字節(jié)數(shù)組    
    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++){
        // 將要發(fā)送的數(shù)據(jù)賦值給聯(lián)合體的float成員
        // 相應(yīng)的就能更改字節(jié)數(shù)組成員的值
        _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;
    // 串口發(fā)送
    ser.write(data_to_send, _cnt);
});

最后，創(chuàng)建一個控制指令發(fā)布節(jié)點，用來發(fā)布cmd_vel話題，在xrobot功能包下新建一個scripts文件夾，添加remote_ctrl.py，內(nèi)容如下：

#!/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節(jié)點，可以得到如下的節(jié)點圖：