Go to the documentation of this file.00001 #ifndef _ROS_moveit_msgs_VisibilityConstraint_h
00002 #define _ROS_moveit_msgs_VisibilityConstraint_h
00003
00004 #include <stdint.h>
00005 #include <string.h>
00006 #include <stdlib.h>
00007 #include "ros/msg.h"
00008 #include "geometry_msgs/PoseStamped.h"
00009
00010 namespace moveit_msgs
00011 {
00012
00013 class VisibilityConstraint : public ros::Msg
00014 {
00015 public:
00016 float target_radius;
00017 geometry_msgs::PoseStamped target_pose;
00018 int32_t cone_sides;
00019 geometry_msgs::PoseStamped sensor_pose;
00020 float max_view_angle;
00021 float max_range_angle;
00022 uint8_t sensor_view_direction;
00023 float weight;
00024 enum { SENSOR_Z = 0 };
00025 enum { SENSOR_Y = 1 };
00026 enum { SENSOR_X = 2 };
00027
00028 virtual int serialize(unsigned char *outbuffer) const
00029 {
00030 int offset = 0;
00031 int32_t * val_target_radius = (int32_t *) &(this->target_radius);
00032 int32_t exp_target_radius = (((*val_target_radius)>>23)&255);
00033 if(exp_target_radius != 0)
00034 exp_target_radius += 1023-127;
00035 int32_t sig_target_radius = *val_target_radius;
00036 *(outbuffer + offset++) = 0;
00037 *(outbuffer + offset++) = 0;
00038 *(outbuffer + offset++) = 0;
00039 *(outbuffer + offset++) = (sig_target_radius<<5) & 0xff;
00040 *(outbuffer + offset++) = (sig_target_radius>>3) & 0xff;
00041 *(outbuffer + offset++) = (sig_target_radius>>11) & 0xff;
00042 *(outbuffer + offset++) = ((exp_target_radius<<4) & 0xF0) | ((sig_target_radius>>19)&0x0F);
00043 *(outbuffer + offset++) = (exp_target_radius>>4) & 0x7F;
00044 if(this->target_radius < 0) *(outbuffer + offset -1) |= 0x80;
00045 offset += this->target_pose.serialize(outbuffer + offset);
00046 union {
00047 int32_t real;
00048 uint32_t base;
00049 } u_cone_sides;
00050 u_cone_sides.real = this->cone_sides;
00051 *(outbuffer + offset + 0) = (u_cone_sides.base >> (8 * 0)) & 0xFF;
00052 *(outbuffer + offset + 1) = (u_cone_sides.base >> (8 * 1)) & 0xFF;
00053 *(outbuffer + offset + 2) = (u_cone_sides.base >> (8 * 2)) & 0xFF;
00054 *(outbuffer + offset + 3) = (u_cone_sides.base >> (8 * 3)) & 0xFF;
00055 offset += sizeof(this->cone_sides);
00056 offset += this->sensor_pose.serialize(outbuffer + offset);
00057 int32_t * val_max_view_angle = (int32_t *) &(this->max_view_angle);
00058 int32_t exp_max_view_angle = (((*val_max_view_angle)>>23)&255);
00059 if(exp_max_view_angle != 0)
00060 exp_max_view_angle += 1023-127;
00061 int32_t sig_max_view_angle = *val_max_view_angle;
00062 *(outbuffer + offset++) = 0;
00063 *(outbuffer + offset++) = 0;
00064 *(outbuffer + offset++) = 0;
00065 *(outbuffer + offset++) = (sig_max_view_angle<<5) & 0xff;
00066 *(outbuffer + offset++) = (sig_max_view_angle>>3) & 0xff;
00067 *(outbuffer + offset++) = (sig_max_view_angle>>11) & 0xff;
00068 *(outbuffer + offset++) = ((exp_max_view_angle<<4) & 0xF0) | ((sig_max_view_angle>>19)&0x0F);
00069 *(outbuffer + offset++) = (exp_max_view_angle>>4) & 0x7F;
00070 if(this->max_view_angle < 0) *(outbuffer + offset -1) |= 0x80;
00071 int32_t * val_max_range_angle = (int32_t *) &(this->max_range_angle);
00072 int32_t exp_max_range_angle = (((*val_max_range_angle)>>23)&255);
00073 if(exp_max_range_angle != 0)
00074 exp_max_range_angle += 1023-127;
00075 int32_t sig_max_range_angle = *val_max_range_angle;
00076 *(outbuffer + offset++) = 0;
00077 *(outbuffer + offset++) = 0;
00078 *(outbuffer + offset++) = 0;
00079 *(outbuffer + offset++) = (sig_max_range_angle<<5) & 0xff;
00080 *(outbuffer + offset++) = (sig_max_range_angle>>3) & 0xff;
00081 *(outbuffer + offset++) = (sig_max_range_angle>>11) & 0xff;
00082 *(outbuffer + offset++) = ((exp_max_range_angle<<4) & 0xF0) | ((sig_max_range_angle>>19)&0x0F);
00083 *(outbuffer + offset++) = (exp_max_range_angle>>4) & 0x7F;
00084 if(this->max_range_angle < 0) *(outbuffer + offset -1) |= 0x80;
00085 *(outbuffer + offset + 0) = (this->sensor_view_direction >> (8 * 0)) & 0xFF;
00086 offset += sizeof(this->sensor_view_direction);
00087 int32_t * val_weight = (int32_t *) &(this->weight);
00088 int32_t exp_weight = (((*val_weight)>>23)&255);
00089 if(exp_weight != 0)
00090 exp_weight += 1023-127;
00091 int32_t sig_weight = *val_weight;
00092 *(outbuffer + offset++) = 0;
00093 *(outbuffer + offset++) = 0;
00094 *(outbuffer + offset++) = 0;
00095 *(outbuffer + offset++) = (sig_weight<<5) & 0xff;
00096 *(outbuffer + offset++) = (sig_weight>>3) & 0xff;
00097 *(outbuffer + offset++) = (sig_weight>>11) & 0xff;
00098 *(outbuffer + offset++) = ((exp_weight<<4) & 0xF0) | ((sig_weight>>19)&0x0F);
00099 *(outbuffer + offset++) = (exp_weight>>4) & 0x7F;
00100 if(this->weight < 0) *(outbuffer + offset -1) |= 0x80;
00101 return offset;
00102 }
00103
00104 virtual int deserialize(unsigned char *inbuffer)
00105 {
00106 int offset = 0;
00107 uint32_t * val_target_radius = (uint32_t*) &(this->target_radius);
00108 offset += 3;
00109 *val_target_radius = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
00110 *val_target_radius |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
00111 *val_target_radius |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
00112 *val_target_radius |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
00113 uint32_t exp_target_radius = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
00114 exp_target_radius |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
00115 if(exp_target_radius !=0)
00116 *val_target_radius |= ((exp_target_radius)-1023+127)<<23;
00117 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->target_radius = -this->target_radius;
00118 offset += this->target_pose.deserialize(inbuffer + offset);
00119 union {
00120 int32_t real;
00121 uint32_t base;
00122 } u_cone_sides;
00123 u_cone_sides.base = 0;
00124 u_cone_sides.base |= ((uint32_t) (*(inbuffer + offset + 0))) << (8 * 0);
00125 u_cone_sides.base |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
00126 u_cone_sides.base |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
00127 u_cone_sides.base |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
00128 this->cone_sides = u_cone_sides.real;
00129 offset += sizeof(this->cone_sides);
00130 offset += this->sensor_pose.deserialize(inbuffer + offset);
00131 uint32_t * val_max_view_angle = (uint32_t*) &(this->max_view_angle);
00132 offset += 3;
00133 *val_max_view_angle = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
00134 *val_max_view_angle |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
00135 *val_max_view_angle |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
00136 *val_max_view_angle |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
00137 uint32_t exp_max_view_angle = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
00138 exp_max_view_angle |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
00139 if(exp_max_view_angle !=0)
00140 *val_max_view_angle |= ((exp_max_view_angle)-1023+127)<<23;
00141 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->max_view_angle = -this->max_view_angle;
00142 uint32_t * val_max_range_angle = (uint32_t*) &(this->max_range_angle);
00143 offset += 3;
00144 *val_max_range_angle = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
00145 *val_max_range_angle |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
00146 *val_max_range_angle |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
00147 *val_max_range_angle |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
00148 uint32_t exp_max_range_angle = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
00149 exp_max_range_angle |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
00150 if(exp_max_range_angle !=0)
00151 *val_max_range_angle |= ((exp_max_range_angle)-1023+127)<<23;
00152 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->max_range_angle = -this->max_range_angle;
00153 this->sensor_view_direction = ((uint8_t) (*(inbuffer + offset)));
00154 offset += sizeof(this->sensor_view_direction);
00155 uint32_t * val_weight = (uint32_t*) &(this->weight);
00156 offset += 3;
00157 *val_weight = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
00158 *val_weight |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
00159 *val_weight |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
00160 *val_weight |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
00161 uint32_t exp_weight = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
00162 exp_weight |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
00163 if(exp_weight !=0)
00164 *val_weight |= ((exp_weight)-1023+127)<<23;
00165 if( ((*(inbuffer+offset++)) & 0x80) > 0) this->weight = -this->weight;
00166 return offset;
00167 }
00168
00169 const char * getType(){ return "moveit_msgs/VisibilityConstraint"; };
00170 const char * getMD5(){ return "62cda903bfe31ff2e5fcdc3810d577ad"; };
00171
00172 };
00173
00174 }
00175 #endif