00001 #ifndef _ROS_moveit_msgs_RobotState_h 00002 #define _ROS_moveit_msgs_RobotState_h 00003 00004 #include <stdint.h> 00005 #include <string.h> 00006 #include <stdlib.h> 00007 #include "ros/msg.h" 00008 #include "sensor_msgs/JointState.h" 00009 #include "sensor_msgs/MultiDOFJointState.h" 00010 #include "moveit_msgs/AttachedCollisionObject.h" 00011 00012 namespace moveit_msgs 00013 { 00014 00015 class RobotState : public ros::Msg 00016 { 00017 public: 00018 sensor_msgs::JointState joint_state; 00019 sensor_msgs::MultiDOFJointState multi_dof_joint_state; 00020 uint8_t attached_collision_objects_length; 00021 moveit_msgs::AttachedCollisionObject st_attached_collision_objects; 00022 moveit_msgs::AttachedCollisionObject * attached_collision_objects; 00023 bool is_diff; 00024 00025 virtual int serialize(unsigned char *outbuffer) const 00026 { 00027 int offset = 0; 00028 offset += this->joint_state.serialize(outbuffer + offset); 00029 offset += this->multi_dof_joint_state.serialize(outbuffer + offset); 00030 *(outbuffer + offset++) = attached_collision_objects_length; 00031 *(outbuffer + offset++) = 0; 00032 *(outbuffer + offset++) = 0; 00033 *(outbuffer + offset++) = 0; 00034 for( uint8_t i = 0; i < attached_collision_objects_length; i++){ 00035 offset += this->attached_collision_objects[i].serialize(outbuffer + offset); 00036 } 00037 union { 00038 bool real; 00039 uint8_t base; 00040 } u_is_diff; 00041 u_is_diff.real = this->is_diff; 00042 *(outbuffer + offset + 0) = (u_is_diff.base >> (8 * 0)) & 0xFF; 00043 offset += sizeof(this->is_diff); 00044 return offset; 00045 } 00046 00047 virtual int deserialize(unsigned char *inbuffer) 00048 { 00049 int offset = 0; 00050 offset += this->joint_state.deserialize(inbuffer + offset); 00051 offset += this->multi_dof_joint_state.deserialize(inbuffer + offset); 00052 uint8_t attached_collision_objects_lengthT = *(inbuffer + offset++); 00053 if(attached_collision_objects_lengthT > attached_collision_objects_length) 00054 this->attached_collision_objects = (moveit_msgs::AttachedCollisionObject*)realloc(this->attached_collision_objects, attached_collision_objects_lengthT * sizeof(moveit_msgs::AttachedCollisionObject)); 00055 offset += 3; 00056 attached_collision_objects_length = attached_collision_objects_lengthT; 00057 for( uint8_t i = 0; i < attached_collision_objects_length; i++){ 00058 offset += this->st_attached_collision_objects.deserialize(inbuffer + offset); 00059 memcpy( &(this->attached_collision_objects[i]), &(this->st_attached_collision_objects), sizeof(moveit_msgs::AttachedCollisionObject)); 00060 } 00061 union { 00062 bool real; 00063 uint8_t base; 00064 } u_is_diff; 00065 u_is_diff.base = 0; 00066 u_is_diff.base |= ((uint8_t) (*(inbuffer + offset + 0))) << (8 * 0); 00067 this->is_diff = u_is_diff.real; 00068 offset += sizeof(this->is_diff); 00069 return offset; 00070 } 00071 00072 const char * getType(){ return "moveit_msgs/RobotState"; }; 00073 const char * getMD5(){ return "217a2e8e5547f4162b13a37db9cb4da4"; }; 00074 00075 }; 00076 00077 } 00078 #endif