rosserial library for mbed Inspired by nucho's rosserial library This library is still under development
Dependents: mbed_roshydro_test
Library still under development!
moveit_msgs/PlaceLocation.h
- Committer:
- akashvibhute
- Date:
- 2015-02-15
- Revision:
- 0:30537dec6e0b
File content as of revision 0:30537dec6e0b:
#ifndef _ROS_moveit_msgs_PlaceLocation_h #define _ROS_moveit_msgs_PlaceLocation_h #include <stdint.h> #include <string.h> #include <stdlib.h> #include "ros/msg.h" #include "trajectory_msgs/JointTrajectory.h" #include "geometry_msgs/PoseStamped.h" #include "moveit_msgs/GripperTranslation.h" namespace moveit_msgs { class PlaceLocation : public ros::Msg { public: const char* id; trajectory_msgs::JointTrajectory post_place_posture; geometry_msgs::PoseStamped place_pose; moveit_msgs::GripperTranslation pre_place_approach; moveit_msgs::GripperTranslation post_place_retreat; uint8_t allowed_touch_objects_length; char* st_allowed_touch_objects; char* * allowed_touch_objects; PlaceLocation(): id(""), post_place_posture(), place_pose(), pre_place_approach(), post_place_retreat(), allowed_touch_objects_length(0), allowed_touch_objects(NULL) { } virtual int serialize(unsigned char *outbuffer) const { int offset = 0; uint32_t length_id = strlen(this->id); memcpy(outbuffer + offset, &length_id, sizeof(uint32_t)); offset += 4; memcpy(outbuffer + offset, this->id, length_id); offset += length_id; offset += this->post_place_posture.serialize(outbuffer + offset); offset += this->place_pose.serialize(outbuffer + offset); offset += this->pre_place_approach.serialize(outbuffer + offset); offset += this->post_place_retreat.serialize(outbuffer + offset); *(outbuffer + offset++) = allowed_touch_objects_length; *(outbuffer + offset++) = 0; *(outbuffer + offset++) = 0; *(outbuffer + offset++) = 0; for( uint8_t i = 0; i < allowed_touch_objects_length; i++){ uint32_t length_allowed_touch_objectsi = strlen(this->allowed_touch_objects[i]); memcpy(outbuffer + offset, &length_allowed_touch_objectsi, sizeof(uint32_t)); offset += 4; memcpy(outbuffer + offset, this->allowed_touch_objects[i], length_allowed_touch_objectsi); offset += length_allowed_touch_objectsi; } return offset; } virtual int deserialize(unsigned char *inbuffer) { int offset = 0; uint32_t length_id; memcpy(&length_id, (inbuffer + offset), sizeof(uint32_t)); offset += 4; for(unsigned int k= offset; k< offset+length_id; ++k){ inbuffer[k-1]=inbuffer[k]; } inbuffer[offset+length_id-1]=0; this->id = (char *)(inbuffer + offset-1); offset += length_id; offset += this->post_place_posture.deserialize(inbuffer + offset); offset += this->place_pose.deserialize(inbuffer + offset); offset += this->pre_place_approach.deserialize(inbuffer + offset); offset += this->post_place_retreat.deserialize(inbuffer + offset); uint8_t allowed_touch_objects_lengthT = *(inbuffer + offset++); if(allowed_touch_objects_lengthT > allowed_touch_objects_length) this->allowed_touch_objects = (char**)realloc(this->allowed_touch_objects, allowed_touch_objects_lengthT * sizeof(char*)); offset += 3; allowed_touch_objects_length = allowed_touch_objects_lengthT; for( uint8_t i = 0; i < allowed_touch_objects_length; i++){ uint32_t length_st_allowed_touch_objects; memcpy(&length_st_allowed_touch_objects, (inbuffer + offset), sizeof(uint32_t)); offset += 4; for(unsigned int k= offset; k< offset+length_st_allowed_touch_objects; ++k){ inbuffer[k-1]=inbuffer[k]; } inbuffer[offset+length_st_allowed_touch_objects-1]=0; this->st_allowed_touch_objects = (char *)(inbuffer + offset-1); offset += length_st_allowed_touch_objects; memcpy( &(this->allowed_touch_objects[i]), &(this->st_allowed_touch_objects), sizeof(char*)); } return offset; } const char * getType(){ return "moveit_msgs/PlaceLocation"; }; const char * getMD5(){ return "f3dbcaca40fb29ede2af78b3e1831128"; }; }; } #endif