Rosserial Arduino Library中从一行代码开始探究系统原理
俗话说,管中窥豹,可见一斑。
从一行代码开始,分析rosserial arduino库的脉络。走码观花,到哪儿是哪儿。
编程写个ros节点,这是不可缺少的一句。
ros::NodeHandle nh;
其定义在文件ros.h内
typedef NodeHandle_<ArduinoHardware, 25, 25, 280, 280> NodeHandle;
从这个类型的定义开始提线,看25, 25, 280, 280都分别配置了什么?
找到文件ros/node_handle.h
/* Node Handle */
template<class Hardware,
int MAX_SUBSCRIBERS = 25,/* 最大订阅数 */
int MAX_PUBLISHERS = 25,/* 最大发布数 */
int INPUT_SIZE = 512,/* 输入缓冲大小 */
int OUTPUT_SIZE = 512>/* 输出缓冲大小 */
实现:
namespace ros
{
class NodeHandle
{
public:
virtual int publish(int id, const Msg* msg) = 0;
virtual int spinOnce() = 0;
virtual bool connected() = 0;
protected:
Hardware hardware_;
uint8_t message_in[INPUT_SIZE];//接收数据缓冲区大小,这个size握手协商阶段会上报,上位机数据包字节数不能超过此限制。
uint8_t message_out[OUTPUT_SIZE];//发送数据缓冲区大小,应估计最长数据来定义size。否则超出buf装不下,消息将会dropped
//MAX_PUBLISHERS; MAX_SUBSCRIBERS这里定义的数量为aduino内创建的puber和suber的最大数量
Publisher * publishers[MAX_PUBLISHERS];
Subscriber_ * subscribers[MAX_SUBSCRIBERS];
/*
定义好er们,一般在setup中会执行这样的语句。
nh.advertise(puber);
nh.subscribe(suber);
nh.advertiseService(servicer_server);
nh.serviceClient(servicer_client);
*/
//当调用advertise(puber)、subscribe(suber)时,就会被限定不能超出配置的数量。如果记不清有几个puber和suber,最好看看返回值是否成功。
/* Register a new publisher */
bool advertise(Publisher & p)
{
for (int i = 0; i < MAX_PUBLISHERS; i++)
{
if (publishers[i] == 0) // empty slot
{
publishers[i] = &p;//------------------add
p.id_ = i + 100 + MAX_SUBSCRIBERS;//起始100,之前的id保留, + MAX_SUBSCRIBERS 意图是让pubid比subid大并且不会重复。
p.nh_ = this;
return true;
}
}
return false;
}
/* Register a new subscriber */
template<typename SubscriberT>
bool subscribe(SubscriberT& s)
{
for (int i = 0; i < MAX_SUBSCRIBERS; i++)
{
if (subscribers[i] == 0) // empty slot
{
subscribers[i] = static_cast<Subscriber_*>(&s);//------------------add
s.id_ = i + 100;//起始100,之前的id保留,
return true;
}
}
return false;
}
可以看到定义的puber和suber的指针已经存储到对应的“池”内,备用。
puber\suber是用来发布\获取数据的,值得注意的是serviceserver和service client,他们工作要发布\获取数据。
puber和suber、serviceserver,serviceclient示例汇总
ros::NodeHandle nh;
void messageCb( const std_msgs::Empty& toggle_msg){/*suber获取利用msg的数据*/}
ros::Subscriber<std_msgs::Empty> sub("toggle_led", messageCb );
std_msgs::String str_msg;
ros::Publisher chatter("chatter", &str_msg);
char hello[13] = "hello world!";
void setup(){ nh.initNode(); nh.advertise(chatter); nh.subscribe(sub);}//---------------------add suber和puber
void loop(){
str_msg.data = hello;
chatter.publish( &str_msg );//---------puber发布数据
nh.spinOnce();
delay(500);
}
ros::NodeHandle nh;
void callback(const Test::Request & req, Test::Response & res){/*获取 req数据,填充res数据*/}
ros::ServiceServer<Test::Request, Test::Response> server("test_srv",&callback);
void setup(){ nh.initNode(); nh.advertiseService(server);}//-----------add servicer
void loop(){
nh.spinOnce();
}
ros::NodeHandle nh;
ros::ServiceClient<Test::Request, Test::Response> client("test_srv");
void setup(){ nh.initNode(); nh.serviceClient(client);//-----------------add servicer
while(!nh.connected()) nh.spinOnce();
nh.loginfo("Startup complete");
}
void loop(){
Test::Request req; Test::Response res;
req.input = hello;//---------------填充请求数据
client.call(req, res);
//zzzzzz = res.output;//-----------利用获取的响应数据
nh.spinOnce();
}
Publisher、Subscriber源码比较简单,略过。分析ServiceServer、ServiceClient。
他们继承了suber,包含了一个puber协同工作,
看ServiceServer 代码
template<typename MReq , typename MRes, typename ObjT = void>
class ServiceServer : public Subscriber_
{
public:
typedef void(ObjT::*CallbackT)(const MReq&, MRes&);
ServiceServer(const char* topic_name, CallbackT cb, ObjT* obj) :
pub(topic_name, &resp, rosserial_msgs::TopicInfo::ID_SERVICE_SERVER + rosserial_msgs::TopicInfo::ID_PUBLISHER),//注意类型值叠加
obj_(obj)
{
this->topic_ = topic_name;
this->cb_ = cb;
}
// these refer to the subscriber
virtual void callback(unsigned char *data)
{
req.deserialize(data);
(obj_->*cb_)(req, resp);//--------------处理获取到的数据,填充响应数据
pub.publish(&resp);//------------------发布响应数据
}
//...
virtual int getEndpointType()
{
return rosserial_msgs::TopicInfo::ID_SERVICE_SERVER + rosserial_msgs::TopicInfo::ID_SUBSCRIBER;//注意类型值叠加
}
//...
Publisher pub;
//...
看ServiceClient 代码
template<typename MReq , typename MRes>
class ServiceClient : public Subscriber_
{
public:
ServiceClient(const char* topic_name) :
pub(topic_name, &req, rosserial_msgs::TopicInfo::ID_SERVICE_CLIENT + rosserial_msgs::TopicInfo::ID_PUBLISHER)//注意类型值叠加
{
this->topic_ = topic_name;
this->waiting = true;
}
virtual void call(const MReq & request, MRes & response)
{
if (!pub.nh_->connected()) return;
ret = &response;
waiting = true;
pub.publish(&request);//----------------------------发布请求数据
while (waiting && pub.nh_->connected())//----------------->>>等待响应,单片机spinOnce()卡住
if (pub.nh_->spinOnce() < 0) break;//直到spinOnce接收数据成功,回调callback,完成整个流程;或者响应异常,请求失败,注意失败无返回数据。
}
// these refer to the subscriber
virtual void callback(unsigned char *data)
{
ret->deserialize(data);
waiting = false;//----------------------------------------------<<<正常响应完成
}
//....
virtual int getEndpointType()
{
return rosserial_msgs::TopicInfo::ID_SERVICE_CLIENT + rosserial_msgs::TopicInfo::ID_SUBSCRIBER;//注意类型值叠加
}
//....
Publisher pub;
//....
servicer中实例化的puber、suber。
它们也是存储在“池”内的,定义所以定义MAX_PUBLISHERS MAX_SUBSCRIBERS要考虑servicer 的量
/* Register a new Service Server */
template<typename MReq, typename MRes, typename ObjT>
bool advertiseService(ServiceServer<MReq, MRes, ObjT>& srv)
{
bool v = advertise(srv.pub);//-------------------------------------------ServiceServer 添加srv的puber
for (int i = 0; i < MAX_SUBSCRIBERS; i++)
{
if (subscribers[i] == 0) // empty slot
{
subscribers[i] = static_cast<Subscriber_*>(&srv);//-----------------ServiceServer 添加srv的suber
srv.id_ = i + 100;
return v;
}
}
return false;
}
/* Register a new Service Client */
template<typename MReq, typename MRes>
bool serviceClient(ServiceClient<MReq, MRes>& srv)
{
bool v = advertise(srv.pub);//-------------------------------------------ServiceClient 添加srv的puber
for (int i = 0; i < MAX_SUBSCRIBERS; i++)
{
if (subscribers[i] == 0) // empty slot
{
subscribers[i] = static_cast<Subscriber_*>(&srv);//-----------------ServiceClient 添加srv的suber
srv.id_ = i + 100;
return v;
}
}
return false;
}
//当配置好了puber、suber、servicer,如何运作?消息怎么传出去,又怎么传进来?
如果没有spinOnce()驱动,他们中获取数据的er们将会一直游手好闲,无所事事。
spinOnce()是放在loop循环中不断重复执行的。
spinOnce()内主要是接收数据解包的工作,处理好接收到的数据往message_in 里填充,然后驱动对应er上工的逻辑。
首先spinOnce会等上位机发数据,来检测上位机的ros程序已经正常启动。
就像种子等待发芽的信号,春天来了,接收到暖阳和雨露的信号,就开始萌发。
触发设备第一个数据包是这样的
0xff 0xfe 0x00 0x00 0xff 0x00 0x00 0xff
spinOnce()内,接收数据逐个字节进行,解包并验证。
其中的mode_变量控制解析过程的不同阶段。
默认值为MODE_FIRST_FF,
先检查第一字节是否 0xff
然后 mode_++
看版本是否是0xfe
再 mode_++
...
根据下列数据看到解析过程经过8个阶段
const uint8_t SYNC_SECONDS = 5;
const uint8_t MODE_FIRST_FF = 0;
const uint8_t MODE_PROTOCOL_VER = 1;
const uint8_t PROTOCOL_VER1 = 0xff; // through groovy
const uint8_t PROTOCOL_VER2 = 0xfe; // in hydro
const uint8_t PROTOCOL_VER = PROTOCOL_VER2;
const uint8_t MODE_SIZE_L = 2;
const uint8_t MODE_SIZE_H = 3;
const uint8_t MODE_SIZE_CHECKSUM = 4; // checksum for msg size received from size L and H
const uint8_t MODE_TOPIC_L = 5; // waiting for topic id
const uint8_t MODE_TOPIC_H = 6;
const uint8_t MODE_MESSAGE = 7;
const uint8_t MODE_MSG_CHECKSUM = 8; // checksum for msg and topic id
这里不一一展开,有兴趣可以对照源码参考前文进行测试。
前文
ROS中rosserial通讯协议初探
https://blog.csdn.net/qq_38288618/article/details/102931684
只说最后一步
else if (mode_ == MODE_MSG_CHECKSUM) /* do checksum */
{
mode_ = MODE_FIRST_FF;
if ((checksum_ % 256) == 255)
{
if (topic_ == TopicInfo::ID_PUBLISHER)
{
//收到上位机第一个topicid==0的包,执行发送同步请求requestSyncTime,和协商话题。negotiateTopics,
requestSyncTime();
negotiateTopics();
last_sync_time = c_time;
last_sync_receive_time = c_time;
return SPIN_ERR;
}
else if (topic_ == TopicInfo::ID_TIME)
{
syncTime(message_in);//--------------------------------------这里用到数据buf
}
else if (topic_ == TopicInfo::ID_PARAMETER_REQUEST)
{
req_param_resp.deserialize(message_in);//--------------------------------------这里用到数据buf
param_recieved = true;
}
else if (topic_ == TopicInfo::ID_TX_STOP)
{
configured_ = false;
}
else
{
//这里订阅的suber执行回调
if (subscribers[topic_ - 100])//因为之前add的时候subid=下标+100,这里用topic_ 来找是哪个suber的数据
subscribers[topic_ - 100]->callback(message_in);//--------------------------------------这里用到数据buf
//如果是serviceserver回调处理完数据,再publish()发送数据
}
}
}
这部分是接收完整包之后的逻辑
根据topic_ 不同类别分了5个分支
握手阶段
分支1、同步时间与协商话题,
requestSyncTime();
negotiateTopics();
把池里的suber和puber情况发送给上位机,上位机serial_node.py 的node根据情况进行配置,一边与ros沟通,一边与下位机沟通。
用到message_in数据的有3个分支
分支2、suber、servicer(server+client)这些suber的callback、
分支3、时间同步syncTime
分支4、参数获取req_param_resp
还有一个分支是配置开关
分支5、configured_ = false;
message_in是装载发过来的数据的buf,没见有检查溢出的代码句子,因为在握手阶段就把自己的buf大小告诉了上位机。
上位机serial_node.py 应检查要发送数据的大小,发来的数据不会超限。
分析握手阶段,协商话题发送的数据
void negotiateTopics()
{
rosserial_msgs::TopicInfo ti;
int i;
for (i = 0; i < MAX_PUBLISHERS; i++)
{
if (publishers[i] != 0) // non-empty slot
{
ti.topic_id = publishers[i]->id_;
ti.topic_name = (char *) publishers[i]->topic_;
ti.message_type = (char *) publishers[i]->msg_->getType();
ti.md5sum = (char *) publishers[i]->msg_->getMD5();
ti.buffer_size = OUTPUT_SIZE;
publish(publishers[i]->getEndpointType(), &ti);//组织好每个puber的信息发送
}
}
for (i = 0; i < MAX_SUBSCRIBERS; i++)
{
if (subscribers[i] != 0) // non-empty slot
{
ti.topic_id = subscribers[i]->id_;
ti.topic_name = (char *) subscribers[i]->topic_;
ti.message_type = (char *) subscribers[i]->getMsgType();
ti.md5sum = (char *) subscribers[i]->getMsgMD5();
ti.buffer_size = INPUT_SIZE;
publish(subscribers[i]->getEndpointType(), &ti);//组织好每个suber的信息发送
}
}
configured_ = true;
}
看看通用的打包方法
virtual int publish(int id, const Msg * msg)
{
if (id >= 100 && !configured_)
return 0;
/* serialize message */
int l = msg->serialize(message_out + 7);
/* setup the header */
message_out[0] = 0xff;
message_out[1] = PROTOCOL_VER;
message_out[2] = (uint8_t)((uint16_t)l & 255);
message_out[3] = (uint8_t)((uint16_t)l >> 8);
message_out[4] = 255 - ((message_out[2] + message_out[3]) % 256);
message_out[5] = (uint8_t)((int16_t)id & 255);
message_out[6] = (uint8_t)((int16_t)id >> 8);
/* calculate checksum */
int chk = 0;
for (int i = 5; i < l + 7; i++)
chk += message_out[i];
l += 7;
message_out[l++] = 255 - (chk % 256);
//判断是否消息是不是超出message_out范围了。我觉得应该靠前判断,算了半天,结果一看超限了做了无用功。
if (l <= OUTPUT_SIZE)
{
hardware_.write(message_out, l);
//串口发送
return l;
}
else
{
//logerror("Message from device dropped: message larger than buffer.");
logerror("MsgEr:OutBuf");
//log的字符串太长,多占内存?
return -1;
}
}
//上面的topic类型是 rosserial_msgs::TopicInfo,通讯协议依赖的消息类型。
可以找到rosserial_msgs/TopicInfo .h看一眼
namespace rosserial_msgs
{
class TopicInfo : public ros::Msg
{
public:
typedef uint16_t _topic_id_type;
_topic_id_type topic_id;
typedef const char* _topic_name_type;
_topic_name_type topic_name;
typedef const char* _message_type_type;
_message_type_type message_type;
typedef const char* _md5sum_type;
_md5sum_type md5sum;
typedef int32_t _buffer_size_type;
_buffer_size_type buffer_size;
//这里数值就是协议规定的分类
enum { ID_PUBLISHER = 0 };
enum { ID_SUBSCRIBER = 1 };
enum { ID_SERVICE_SERVER = 2 };
enum { ID_SERVICE_CLIENT = 4 };
enum { ID_PARAMETER_REQUEST = 6 };
enum { ID_LOG = 7 };
enum { ID_TIME = 10 };
enum { ID_TX_STOP = 11 };
TopicInfo():
topic_id(0),
topic_name(""),
message_type(""),
md5sum(""),
buffer_size(0)
{
}
virtual int serialize(unsigned char *outbuffer) const
//。。。
virtual int deserialize(unsigned char *inbuffer)
//。。。
const char * getType(){ return "rosserial_msgs/TopicInfo"; };
const char * getMD5(){ return "0ad51f88fc44892f8c10684077646005"; };
};
}
跟其他消息没什么两样。类型和md5太占内存。包名和msg名每一个字符多占1byte的内存
const char * getType(){ return "rosserial_msgs/TopicInfo"; };//24个字节
const char * getMD5(){ return "0ad51f88fc44892f8c10684077646005"; };//32个字节
这个消息类24+32=56个字节就这样被占了。
md5要验证没办法,包名和类型名真得简化简化
整理个简图,有助于看清整个系统运作的脉络。
再回头看
typedef NodeHandle_<ArduinoHardware, 25, 25, 280, 280> NodeHandle;template<class Hardware,
int MAX_SUBSCRIBERS = 25,//最大订阅数
int MAX_PUBLISHERS = 25,//最大发布数
int INPUT_SIZE = 512,//输入缓冲大小
int OUTPUT_SIZE = 512>//输出缓冲大小
如何更改数值大小。
分析得出每多1个servicer不管是server还是client,puber和suber数量各加1
MAX_PUBLISHERS =pubers+servicers
MAX_SUBSCRIBERS =subers+servicers
可以多加几个量,指针数组不怎么影响内存。
INPUT_SIZE 按上位机最大数据量估算
OUTPUT_SIZE 按单片机要发送的最大数据量估算。
上个前文的包
FF FE 48 00 B7 -------------------0x0048=72 包数据长度 0xb7=0xff-((0x48+0x00)%0x100)
00 00 -----------------------------0x0000= Topic ID(ID_PUBLISHER=0)
7D 00 -----------------------------0x7D=125 topic_id (publisher ID)
07 00 00 00 ----------------------0x0007=7,数据长度 topic_name="chatter"
63 68 61 74 74 65 72
0F 00 00 00 ----------------------0x000f=15,数据长度 message_type ="std_msgs/String"
73 74 64 5F 6D 73 67 73 2F 53
74 72 69 6E 67
20 00 00 00 ----------------------0x0020=32,数据长度 md5sum="992ce8a1687cec8c8bd883ec73ca41d1"
39 39 32 63 65 38 61 31 36 38
37 63 65 63 38 63 38 62 64 38
38 33 65 63 37 33 63 61 34 31
64 31
18 01 00 00 ----------------------0x0118=280,是atmage328p的buffer_size (看2.1)
0C --------------------------------0x0C 包校验
这样观察更直接一些。
通信的这个数据72个字节,再加头7个,尾部校验1个,总共80个字节。
数据的大小与名字长短也有关系,
这些类型名字真得斟酌斟酌,毕竟单片机资源捉襟见肘。
比如有自定义包名zzz_protocol,消息名screen_1602
zzz_protocol/screen_1602 24bytes
简化
zp/s1602 8bytes
这样省了16个字节
先告一段落。。。
纲举目张。目太多,不知怎么切入。有点碎。。。。
错误之处请多多指正!