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個字節
先告一段落。。。
綱舉目張。目太多，不知怎麼切入。有點碎。。。。
錯誤之處請多多指正！