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