Memory optimization of Rosserial Arduino Library
SCM memory resources are very limited. For more complex items, they have to be calculated in bytes.
If too many long string variables are declared, a large amount of memory (ram space) will be consumed
When we use the ros Library of arduino, we will find that more string variables are declared in it.
as
node_handle.h
logerror("Message from device dropped: message larger than buffer.");
stay
In the rosserial msgs package
TopicInfo.h
const char * getType(){ return "rosserial_msgs/TopicInfo"; }; const char * getMD5(){ return "0ad51f88fc44892f8c10684077646005"; };
Log.h
const char * getType(){ return "rosserial_msgs/Log"; }; const char * getMD5(){ return "11abd731c25933261cd6183bd12d6295"; };
RequestParam.h
const char * getType(){ return REQUESTPARAM; }; const char * getMD5(){ return "9f0e98bda65981986ddf53afa7a40e49"; };
So it seems that each message class occupies 50-60 bytes;
Like the nano 328p, the ram has 2k, 2048 bytes, which can't hold a few messages, so it's overwhelmed.
Think about optimizing it.
Save these strings to flash, read them from flash and save them to a buff when necessary.
In this way, as long as there is a buff space in ram, it is ok to store the message data to be sent. If there are many custom messages, the savings are considerable.
Optimization of space also brings some points that need attention.
1. Reading flash takes time. (real time slightly disturbed)
2. Messages must be processed and sent one by one according to the timeline. (if using a multitask system, switching between tasks to send messages may cause problems.)
The first point is not big.
In view of the second point, we can see the architecture of ros library, and its message processing also shares a buff space. This means that message processing must be sent one by one. Multiple message interleaving is not allowed.
Multitasking can only have one task to process ros messages.
Check the negotiation topic source code (loop a series of messages)
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); } } 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); } } configured_ = true; }
In this way, we can safely change the warehouse.
Write a common header, process the data read from flash, and return the buff pointer.
#ifndef _ROS_zr_buff_h #define _ROS_zr_buff_h #include <stdint.h> #include <string.h> #include <stdlib.h> namespace zr { //the msg type and md5 value //stored in flash //edit the souce file,add the code like this: //const char TopicInfo_Type[] PROGMEM = "rosserial_msgs/TopicInfo"; //const char TopicInfo_MD5[] PROGMEM = "0ad51f88fc44892f8c10684077646005"; //--------------NOTICE---------// //buffType [size] ; size is the length of the type name ,you must set the bigest size. //example: char buffType[55]; for this type //const char * getType(){ return "robotiq_2f_gripper_control/Robotiq2FGripper_robot_input"; }; char buffType[33]; //MD5 32 bytes char buffMD5[33]; char * strcpy_P_offset(char* buf,const char* pstr,int offset=0){ strcpy_P((char*)(buf + sizeof(char) * offset), pstr); return (char*)(buf + sizeof(char) * offset); } char * getType_buff(const char* pstr){ return strcpy_P_offset(buffType,pstr); } char * getMD5_buff(const char* pstr){ return strcpy_P_offset(buffMD5,pstr); } } #endif
For example, TopicInfo.h can be changed as follows:
#ifndef _ROS_rosserial_msgs_TopicInfo_h #define _ROS_rosserial_msgs_TopicInfo_h #include <stdint.h> #include <string.h> #include <stdlib.h> #include "ros/msg.h" #include "zr_protocol/zr_buff.h"//zzz add namespace rosserial_msgs { const char TopicInfo_Type[] PROGMEM = "rosserial_msgs/TopicInfo"; //zzz add const char TopicInfo_MD5[] PROGMEM = "0ad51f88fc44892f8c10684077646005"; //zzz add 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 { int offset = 0; *(outbuffer + offset + 0) = (this->topic_id >> (8 * 0)) & 0xFF; *(outbuffer + offset + 1) = (this->topic_id >> (8 * 1)) & 0xFF; offset += sizeof(this->topic_id); uint32_t length_topic_name = strlen(this->topic_name); varToArr(outbuffer + offset, length_topic_name); offset += 4; memcpy(outbuffer + offset, this->topic_name, length_topic_name); offset += length_topic_name; uint32_t length_message_type = strlen(this->message_type); varToArr(outbuffer + offset, length_message_type); offset += 4; memcpy(outbuffer + offset, this->message_type, length_message_type); offset += length_message_type; uint32_t length_md5sum = strlen(this->md5sum); varToArr(outbuffer + offset, length_md5sum); offset += 4; memcpy(outbuffer + offset, this->md5sum, length_md5sum); offset += length_md5sum; union { int32_t real; uint32_t base; } u_buffer_size; u_buffer_size.real = this->buffer_size; *(outbuffer + offset + 0) = (u_buffer_size.base >> (8 * 0)) & 0xFF; *(outbuffer + offset + 1) = (u_buffer_size.base >> (8 * 1)) & 0xFF; *(outbuffer + offset + 2) = (u_buffer_size.base >> (8 * 2)) & 0xFF; *(outbuffer + offset + 3) = (u_buffer_size.base >> (8 * 3)) & 0xFF; offset += sizeof(this->buffer_size); return offset; } virtual int deserialize(unsigned char *inbuffer) { int offset = 0; this->topic_id = ((uint16_t) (*(inbuffer + offset))); this->topic_id |= ((uint16_t) (*(inbuffer + offset + 1))) << (8 * 1); offset += sizeof(this->topic_id); uint32_t length_topic_name; arrToVar(length_topic_name, (inbuffer + offset)); offset += 4; for(unsigned int k= offset; k< offset+length_topic_name; ++k){ inbuffer[k-1]=inbuffer[k]; } inbuffer[offset+length_topic_name-1]=0; this->topic_name = (char *)(inbuffer + offset-1); offset += length_topic_name; uint32_t length_message_type; arrToVar(length_message_type, (inbuffer + offset)); offset += 4; for(unsigned int k= offset; k< offset+length_message_type; ++k){ inbuffer[k-1]=inbuffer[k]; } inbuffer[offset+length_message_type-1]=0; this->message_type = (char *)(inbuffer + offset-1); offset += length_message_type; uint32_t length_md5sum; arrToVar(length_md5sum, (inbuffer + offset)); offset += 4; for(unsigned int k= offset; k< offset+length_md5sum; ++k){ inbuffer[k-1]=inbuffer[k]; } inbuffer[offset+length_md5sum-1]=0; this->md5sum = (char *)(inbuffer + offset-1); offset += length_md5sum; union { int32_t real; uint32_t base; } u_buffer_size; u_buffer_size.base = 0; u_buffer_size.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); u_buffer_size.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); u_buffer_size.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); u_buffer_size.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); this->buffer_size = u_buffer_size.real; offset += sizeof(this->buffer_size); return offset; } //const char * getType(){ return "rosserial_msgs/TopicInfo"; }; //const char * getMD5(){ return "0ad51f88fc44892f8c10684077646005"; }; const char * getType(){ return zr::getType_buff(TopicInfo_Type);};//zzz add const char * getMD5(){ return zr::getMD5_buff(TopicInfo_MD5); };//zzz add }; } #endif
Another example is Log.h.
#ifndef _ROS_rosserial_msgs_Log_h #define _ROS_rosserial_msgs_Log_h #include <stdint.h> #include <string.h> #include <stdlib.h> #include "ros/msg.h" #include "zr_protocol/zr_buff.h"//zzz add namespace rosserial_msgs { const char Log_Type[] PROGMEM = "rosserial_msgs/Log";//zzz add const char Log_MD5[] PROGMEM = "11abd731c25933261cd6183bd12d6295";//zzz add class Log : public ros::Msg { public: typedef uint8_t _level_type; _level_type level; typedef const char* _msg_type; _msg_type msg; enum { ROSDEBUG = 0 }; enum { INFO = 1 }; enum { WARN = 2 }; enum { ERROR = 3 }; enum { FATAL = 4 }; Log(): level(0), msg("") { } virtual int serialize(unsigned char *outbuffer) const { int offset = 0; *(outbuffer + offset + 0) = (this->level >> (8 * 0)) & 0xFF; offset += sizeof(this->level); uint32_t length_msg = strlen(this->msg); varToArr(outbuffer + offset, length_msg); offset += 4; memcpy(outbuffer + offset, this->msg, length_msg); offset += length_msg; return offset; } virtual int deserialize(unsigned char *inbuffer) { int offset = 0; this->level = ((uint8_t) (*(inbuffer + offset))); offset += sizeof(this->level); uint32_t length_msg; arrToVar(length_msg, (inbuffer + offset)); offset += 4; for(unsigned int k= offset; k< offset+length_msg; ++k){ inbuffer[k-1]=inbuffer[k]; } inbuffer[offset+length_msg-1]=0; this->msg = (char *)(inbuffer + offset-1); offset += length_msg; return offset; } //const char * getType(){ return "rosserial_msgs/Log"; }; //const char * getMD5(){ return "11abd731c25933261cd6183bd12d6295"; }; const char * getType(){ return zr::getType_buff(Log_Type);};//zzz add const char * getMD5(){ return zr::getMD5_buff(Log_MD5); };//zzz add }; } #endif
Last custom message example:
#ifndef _ROS_zr_protocol_screen_1602_h #define _ROS_zr_protocol_screen_1602_h #include <stdint.h> #include <string.h> #include <stdlib.h> #include "ros/msg.h" #include "zr_protocol/zr_buff.h"//zzz add namespace zr_protocol { const char screen_1602_Type[] PROGMEM = "zr_protocol/screen_1602";//zzz add const char screen_1602_MD5[] PROGMEM = "280803ac68321ba092032bbc5f9ff4d9";//zzz add class screen_1602 : public ros::Msg { public: typedef const char* _line1_type; _line1_type line1; typedef const char* _line2_type; _line2_type line2; screen_1602(): line1(""), line2("") { } virtual int serialize(unsigned char *outbuffer) const { int offset = 0; uint32_t length_line1 = strlen(this->line1); varToArr(outbuffer + offset, length_line1); offset += 4; memcpy(outbuffer + offset, this->line1, length_line1); offset += length_line1; uint32_t length_line2 = strlen(this->line2); varToArr(outbuffer + offset, length_line2); offset += 4; memcpy(outbuffer + offset, this->line2, length_line2); offset += length_line2; return offset; } virtual int deserialize(unsigned char *inbuffer) { int offset = 0; uint32_t length_line1; arrToVar(length_line1, (inbuffer + offset)); offset += 4; for(unsigned int k= offset; k< offset+length_line1; ++k){ inbuffer[k-1]=inbuffer[k]; } inbuffer[offset+length_line1-1]=0; this->line1 = (char *)(inbuffer + offset-1); offset += length_line1; uint32_t length_line2; arrToVar(length_line2, (inbuffer + offset)); offset += 4; for(unsigned int k= offset; k< offset+length_line2; ++k){ inbuffer[k-1]=inbuffer[k]; } inbuffer[offset+length_line2-1]=0; this->line2 = (char *)(inbuffer + offset-1); offset += length_line2; return offset; } //const char * getType(){ return "zr_protocol/screen_1602"; }; //const char * getMD5(){ return"280803ac68321ba092032bbc5f9ff4d9"; }; const char * getType(){ return zr::getType_buff(screen_1602_Type);};//zzz add const char * getMD5(){ return zr::getMD5_buff(screen_1602_MD5); };//zzz add }; } #endif
The routine is the same, involving more msg and srv, so the optimization is worth trying, even on the low-end arduino device, it will also receive good curative effect.