while (ros::ok()) {
handle->Spin();
ros::spinOnce();
usleep(1000);
}
发布消息的实现
创建Pulisher的函数原型如下:
/**
* @brief Create the publisher for the protocol command without need of ack (Send command)
* @tparam Cmd Command DataType
* @param cmd_set Command set for different module, i.e. gimbal, chassis
* @param cmd_id Command id for different commands in the module
* @param sender Sender address
* @param receiver Receiver address
* @return Pointer of publisher handle
*/
template<typename Cmd>
std::shared_ptr<Publisher<Cmd>>
CreatePublisher(uint8_t cmd_set, uint8_t cmd_id, uint8_t sender, uint8_t receiver) {
auto publisher = std::make_shared<Publisher<Cmd>>(shared_from_this(),
cmd_set, cmd_id,
sender, receiver);
publisher_factory_.push_back(
std::dynamic_pointer_cast<PublisherBase>(publisher));
return publisher;
}
源码地址:https://github.com/RoboMaster/RoboRTS/tree/ros/roborts_base
1.文件目录
在该主函数中,创建了一个信息收发解析处理(Handle)对象,一个底盘(Chassis)对象,一个云台(Gimbal)对象,一个裁判系统(RefereeSystem)对象。
(1)底盘对象
底盘对象的定义在
chassis.cpp
里,这里开启了一个专门发送心跳包(heartbeat)的线程,设置了里程计(odom),定位信息(uwb),弹丸供应(projectile_info),调试信息(chassis_gimbal_mode)四个消息发布器,订阅了底盘速度(cmd_vel),底盘加速度(cmd_vel_acc)两个消息,提供底盘控制接口。创建了一个设置底盘工作模式(set_chassis_mode)的服务器。对应的消息类型如下:
(2)云台对象
云台对象的定义在
gimbal.cpp
里,和底盘对象差别不大,也有心跳包的发送(为什么底盘有了云台还要?),订阅了云台角度(cmd_gimbale_angle)、弹丸供应(projectile_info)、弹丸速度(set_fric_speed)三个消息,用于提供云台控制接口。创建了设置云台工作模式(set_gimbal_mode),控制摩擦轮(cmd_fric_wheel),控制发射(cmd_shoot)三个服务器。对应的消息类型如下:
(3)裁判系统对象
裁判系统对象的定义在
referee_system.cpp
中。每一种裁判系统数据(比赛状态,比赛结果,存活机器人,buff信息,机器人状态,热量,伤害,发射,机器人间通信等)都设置了对应的消息发布器,供其他节点订阅。订阅补给站补弹(projectile_supply)、机器人间通信(send_to_teammate)两个消息。消息类型过多不作整理,具体参考ICRA2019裁判系统手册。
(4)信息收发解析处理对象
Handle
对象是整个节点的灵魂所在,主要负责PC与STM32的通信工作,实现了消息的打包发送和解包接收功能。具体的实现在roborts_sdk
中,roborts_sdk
中有五个文件夹,各个文件夹的简介见前面的文件目录一节。该模块不依赖其他模块实现了一个类似ROS的消息发布订阅机制,本质是回调函数,该机制的实现在
dispatch
文件夹中,核心是cmd_set
和cmd_id
两个uint8_t
数字,相关的定义见protocol_defeine.h
(这里和github上底层STM32代码的协议文档里有差别,以实际机器人上跑的代码定义为准)。订阅消息的实现
订阅消息的函数原型如下:
创建订阅器时提供了
cmd_set``cmd_id``sender``receiver
和回调函数,每个订阅器的信息都存储在subscription_factory_
中。在
protocol.cpp
中开启了一个线程专门用于接收STM32的信息,并对信息进行解包处理,将相关信息提取出来,存储在subscription_factory_
中,在Handle::Spin()
中对解析后的信息进行处理,根据信息的帧头执行对应的订阅回调函数。该函数在roborts_base_node.cpp
中调用:发布消息的实现
创建Pulisher的函数原型如下:
同样的,创建时提供
cmd_set``cmd_id``sender``receiver
,用于数据打包时使用。发布消息使用
Pulish
函数,该函数最终会调用SendCMD
函数,该函数在protocol.cpp
中定义。发布消息主要就是在真正的数据前面加上帧头信息和CRC校验位,简化函数如下:
打包完数据后通过串口发送出去即可。
3.通信数据结构
一帧协议数据分为 4 个部分,分别是帧头数据、命令码ID、数据、帧尾校验数据。
(1)帧头数据
在ROS的代码中帧头结构体定义如下:
(2)命令码
一个命令码对应一帧包含具体信息的数据,具体见
protocol_define.h
中的定义。4.调试
运行该节点之前请确认端口号,端口号默认为
/dev/serial_sdk
,它是通过udev
规则映射的。连接STM32设备的虚拟串口,
lsusb
可以查看Vendor和Product的ID,然后创建并配置/etc/udev/rules.d/roborts.rules
文件:编译
运行
控制
可以运行
RoboRTS
的其他模块控制机器人,也可以使用以下两种方式(以底盘为例):(1)发布Twist消息
新建一个节点来控制,实例代码如下:
(2)直接发送串口命令
实例代码如下: