This program is porting rosserial_arduino for mbed http://www.ros.org/wiki/rosserial_arduino This program supported the revision of 169 of rosserial.
Dependents: rosserial_mbed robot_S2
Revision 4:684f39d0c346, committed 2012-02-29
- Comitter:
- nucho
- Date:
- Wed Feb 29 23:00:21 2012 +0000
- Parent:
- 3:1cf99502f396
- Commit message:
Changed in this revision
diff -r 1cf99502f396 -r 684f39d0c346 dianostic_msgs/DiagnosticStatus.h --- a/dianostic_msgs/DiagnosticStatus.h Sat Nov 12 23:54:45 2011 +0000 +++ b/dianostic_msgs/DiagnosticStatus.h Wed Feb 29 23:00:21 2012 +0000 @@ -1,11 +1,10 @@ -#ifndef _ROS_diagnostic_msgs_DiagnosticStatus_h -#define _ROS_diagnostic_msgs_DiagnosticStatus_h +#ifndef ros_diagnostic_msgs_DiagnosticStatus_h +#define ros_diagnostic_msgs_DiagnosticStatus_h #include <stdint.h> #include <string.h> #include <stdlib.h> #include "ros/msg.h" -#include "diagnostic_msgs/byte.h" #include "diagnostic_msgs/KeyValue.h" namespace diagnostic_msgs @@ -14,32 +13,38 @@ class DiagnosticStatus : public ros::Msg { public: - diagnostic_msgs::byte level; + unsigned char level; char * name; char * message; char * hardware_id; - uint8_t values_length; + unsigned char values_length; diagnostic_msgs::KeyValue st_values; diagnostic_msgs::KeyValue * values; enum { OK = 0 }; enum { WARN = 1 }; enum { ERROR = 2 }; - virtual int serialize(unsigned char *outbuffer) const + virtual int serialize(unsigned char *outbuffer) { int offset = 0; - offset += this->level.serialize(outbuffer + offset); - uint32_t * length_name = (uint32_t *)(outbuffer + offset); + union { + unsigned char real; + unsigned char base; + } u_level; + u_level.real = this->level; + *(outbuffer + offset + 0) = (u_level.base >> (8 * 0)) & 0xFF; + offset += sizeof(this->level); + long * length_name = (long *)(outbuffer + offset); *length_name = strlen( (const char*) this->name); offset += 4; memcpy(outbuffer + offset, this->name, *length_name); offset += *length_name; - uint32_t * length_message = (uint32_t *)(outbuffer + offset); + long * length_message = (long *)(outbuffer + offset); *length_message = strlen( (const char*) this->message); offset += 4; memcpy(outbuffer + offset, this->message, *length_message); offset += *length_message; - uint32_t * length_hardware_id = (uint32_t *)(outbuffer + offset); + long * length_hardware_id = (long *)(outbuffer + offset); *length_hardware_id = strlen( (const char*) this->hardware_id); offset += 4; memcpy(outbuffer + offset, this->hardware_id, *length_hardware_id); @@ -48,7 +53,7 @@ *(outbuffer + offset++) = 0; *(outbuffer + offset++) = 0; *(outbuffer + offset++) = 0; - for( uint8_t i = 0; i < values_length; i++){ + for( unsigned char i = 0; i < values_length; i++){ offset += this->values[i].serialize(outbuffer + offset); } return offset; @@ -57,12 +62,19 @@ virtual int deserialize(unsigned char *inbuffer) { int offset = 0; - offset += this->level.deserialize(inbuffer + offset); + union { + unsigned char real; + unsigned char base; + } u_level; + u_level.base = 0; + u_level.base |= ((typeof(u_level.base)) (*(inbuffer + offset + 0))) << (8 * 0); + this->level = u_level.real; + offset += sizeof(this->level); uint32_t length_name = *(uint32_t *)(inbuffer + offset); offset += 4; for(unsigned int k= offset; k< offset+length_name; ++k){ inbuffer[k-1]=inbuffer[k]; - } + } inbuffer[offset+length_name-1]=0; this->name = (char *)(inbuffer + offset-1); offset += length_name; @@ -70,7 +82,7 @@ offset += 4; for(unsigned int k= offset; k< offset+length_message; ++k){ inbuffer[k-1]=inbuffer[k]; - } + } inbuffer[offset+length_message-1]=0; this->message = (char *)(inbuffer + offset-1); offset += length_message; @@ -78,16 +90,16 @@ offset += 4; for(unsigned int k= offset; k< offset+length_hardware_id; ++k){ inbuffer[k-1]=inbuffer[k]; - } + } inbuffer[offset+length_hardware_id-1]=0; this->hardware_id = (char *)(inbuffer + offset-1); offset += length_hardware_id; - uint8_t values_lengthT = *(inbuffer + offset++); + unsigned char values_lengthT = *(inbuffer + offset++); if(values_lengthT > values_length) this->values = (diagnostic_msgs::KeyValue*)realloc(this->values, values_lengthT * sizeof(diagnostic_msgs::KeyValue)); offset += 3; values_length = values_lengthT; - for( uint8_t i = 0; i < values_length; i++){ + for( unsigned char i = 0; i < values_length; i++){ offset += this->st_values.deserialize(inbuffer + offset); memcpy( &(this->values[i]), &(this->st_values), sizeof(diagnostic_msgs::KeyValue)); }
diff -r 1cf99502f396 -r 684f39d0c346 dianostic_msgs/SelfTest.h --- a/dianostic_msgs/SelfTest.h Sat Nov 12 23:54:45 2011 +0000 +++ b/dianostic_msgs/SelfTest.h Wed Feb 29 23:00:21 2012 +0000 @@ -28,8 +28,8 @@ return offset; } - virtual const char * getType(){ return SELFTEST; }; - virtual const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; }; + const char * getType(){ return SELFTEST; }; + const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; }; }; @@ -85,8 +85,8 @@ return offset; } - virtual const char * getType(){ return SELFTEST; }; - virtual const char * getMD5(){ return "74c9372c870a76da4fc2b3973978b898"; }; + const char * getType(){ return SELFTEST; }; + const char * getMD5(){ return "74c9372c870a76da4fc2b3973978b898"; }; };
diff -r 1cf99502f396 -r 684f39d0c346 nav_msgs/GetMap.h --- a/nav_msgs/GetMap.h Sat Nov 12 23:54:45 2011 +0000 +++ b/nav_msgs/GetMap.h Wed Feb 29 23:00:21 2012 +0000 @@ -27,8 +27,8 @@ return offset; } - virtual const char * getType(){ return GETMAP; }; - virtual const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; }; + const char * getType(){ return GETMAP; }; + const char * getMD5(){ return "d41d8cd98f00b204e9800998ecf8427e"; }; }; @@ -51,8 +51,8 @@ return offset; } - virtual const char * getType(){ return GETMAP; }; - virtual const char * getMD5(){ return "6cdd0a18e0aff5b0a3ca2326a89b54ff"; }; + const char * getType(){ return GETMAP; }; + const char * getMD5(){ return "6cdd0a18e0aff5b0a3ca2326a89b54ff"; }; };
diff -r 1cf99502f396 -r 684f39d0c346 nav_msgs/GetPlan.h --- a/nav_msgs/GetPlan.h Sat Nov 12 23:54:45 2011 +0000 +++ b/nav_msgs/GetPlan.h Wed Feb 29 23:00:21 2012 +0000 @@ -56,8 +56,8 @@ return offset; } - virtual const char * getType(){ return GETPLAN; }; - virtual const char * getMD5(){ return "e25a43e0752bcca599a8c2eef8282df8"; }; + const char * getType(){ return GETPLAN; }; + const char * getMD5(){ return "e25a43e0752bcca599a8c2eef8282df8"; }; }; @@ -80,8 +80,8 @@ return offset; } - virtual const char * getType(){ return GETPLAN; }; - virtual const char * getMD5(){ return "0002bc113c0259d71f6cf8cbc9430e18"; }; + const char * getType(){ return GETPLAN; }; + const char * getMD5(){ return "0002bc113c0259d71f6cf8cbc9430e18"; }; };
diff -r 1cf99502f396 -r 684f39d0c346 nav_msgs/MapMetaData.h --- a/nav_msgs/MapMetaData.h Sat Nov 12 23:54:45 2011 +0000 +++ b/nav_msgs/MapMetaData.h Wed Feb 29 23:00:21 2012 +0000 @@ -60,12 +60,12 @@ virtual int deserialize(unsigned char *inbuffer) { int offset = 0; - this->map_load_time.sec |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + this->map_load_time.sec = ((uint32_t) (*(inbuffer + offset))); this->map_load_time.sec |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); this->map_load_time.sec |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); this->map_load_time.sec |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); offset += sizeof(this->map_load_time.sec); - this->map_load_time.nsec |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + this->map_load_time.nsec = ((uint32_t) (*(inbuffer + offset))); this->map_load_time.nsec |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); this->map_load_time.nsec |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); this->map_load_time.nsec |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); @@ -81,12 +81,12 @@ u_resolution.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); this->resolution = u_resolution.real; offset += sizeof(this->resolution); - this->width |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + this->width = ((uint32_t) (*(inbuffer + offset))); this->width |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); this->width |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); this->width |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); offset += sizeof(this->width); - this->height |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0); + this->height = ((uint32_t) (*(inbuffer + offset))); this->height |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); this->height |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); this->height |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
diff -r 1cf99502f396 -r 684f39d0c346 ros/msg.h --- a/ros/msg.h Sat Nov 12 23:54:45 2011 +0000 +++ b/ros/msg.h Wed Feb 29 23:00:21 2012 +0000 @@ -35,7 +35,6 @@ #ifndef _ROS_MSG_H_ #define _ROS_MSG_H_ - namespace ros { /* Base Message Type */ @@ -43,7 +42,7 @@ { public: virtual int serialize(unsigned char *outbuffer) const = 0; - virtual int deserialize(unsigned char *data) = 0; + virtual int deserialize(unsigned char *data) = 0; virtual const char * getType() = 0; virtual const char * getMD5() = 0; };
diff -r 1cf99502f396 -r 684f39d0c346 ros/node_handle.h --- a/ros/node_handle.h Sat Nov 12 23:54:45 2011 +0000 +++ b/ros/node_handle.h Wed Feb 29 23:00:21 2012 +0000 @@ -115,7 +115,6 @@ bytes_ = 0; index_ = 0; topic_ = 0; - checksum_=0; }; protected: @@ -128,8 +127,6 @@ bool configured_; - int total_receivers; - /* used for syncing the time */ unsigned long last_sync_time; unsigned long last_sync_receive_time; @@ -244,7 +241,6 @@ unsigned long offset = hardware_.time() - rt_time; t.deserialize(data); - t.data.sec += offset/1000; t.data.nsec += (offset%1000)*1000000UL; @@ -328,21 +324,25 @@ void negotiateTopics() { configured_ = true; + rosserial_msgs::TopicInfo ti; int i; - for (i = 0; i < MAX_PUBLISHERS; i++) { - if (publishers[i] != 0) { // non-empty slot + 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 + 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();
diff -r 1cf99502f396 -r 684f39d0c346 ros/publisher.h --- a/ros/publisher.h Sat Nov 12 23:54:45 2011 +0000 +++ b/ros/publisher.h Wed Feb 29 23:00:21 2012 +0000 @@ -38,7 +38,7 @@ #include "rosserial_msgs/TopicInfo.h" #include "node_handle.h" -namespace ros{ +namespace ros { /* Generic Publisher */ class Publisher @@ -53,10 +53,9 @@ int getEndpointType(){ return endpoint_; } const char * topic_; - Msg *msg_; // id_ and no_ are set by NodeHandle when we advertise - int16_t id_; + int id_; NodeHandleBase_* nh_; private: @@ -65,5 +64,4 @@ } - #endif \ No newline at end of file
diff -r 1cf99502f396 -r 684f39d0c346 std_msgs/UInt8.h --- a/std_msgs/UInt8.h Sat Nov 12 23:54:45 2011 +0000 +++ b/std_msgs/UInt8.h Wed Feb 29 23:00:21 2012 +0000 @@ -1,4 +1,4 @@ -#ifndef _ROS_std_msgs_UInt8_h + #ifndef _ROS_std_msgs_UInt8_h #define _ROS_std_msgs_UInt8_h #include <stdint.h> @@ -25,7 +25,7 @@ virtual int deserialize(unsigned char *inbuffer) { int offset = 0; - this->data |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0); + this->data = ((uint8_t) (*(inbuffer + offset))); offset += sizeof(this->data); return offset; }