catchrobo2022
/
catchrobo2022_mbed
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: mbed
Embed:
(wiki syntax)
Show/hide line numbers
Constraints.h
00001 #ifndef _ROS_moveit_msgs_Constraints_h 00002 #define _ROS_moveit_msgs_Constraints_h 00003 00004 #include <stdint.h> 00005 #include <string.h> 00006 #include <stdlib.h> 00007 #include "ros/msg.h" 00008 #include "moveit_msgs/JointConstraint.h" 00009 #include "moveit_msgs/PositionConstraint.h" 00010 #include "moveit_msgs/OrientationConstraint.h" 00011 #include "moveit_msgs/VisibilityConstraint.h" 00012 00013 namespace moveit_msgs 00014 { 00015 00016 class Constraints : public ros::Msg 00017 { 00018 public: 00019 typedef const char* _name_type; 00020 _name_type name; 00021 uint32_t joint_constraints_length; 00022 typedef moveit_msgs::JointConstraint _joint_constraints_type; 00023 _joint_constraints_type st_joint_constraints; 00024 _joint_constraints_type * joint_constraints; 00025 uint32_t position_constraints_length; 00026 typedef moveit_msgs::PositionConstraint _position_constraints_type; 00027 _position_constraints_type st_position_constraints; 00028 _position_constraints_type * position_constraints; 00029 uint32_t orientation_constraints_length; 00030 typedef moveit_msgs::OrientationConstraint _orientation_constraints_type; 00031 _orientation_constraints_type st_orientation_constraints; 00032 _orientation_constraints_type * orientation_constraints; 00033 uint32_t visibility_constraints_length; 00034 typedef moveit_msgs::VisibilityConstraint _visibility_constraints_type; 00035 _visibility_constraints_type st_visibility_constraints; 00036 _visibility_constraints_type * visibility_constraints; 00037 00038 Constraints(): 00039 name(""), 00040 joint_constraints_length(0), joint_constraints(NULL), 00041 position_constraints_length(0), position_constraints(NULL), 00042 orientation_constraints_length(0), orientation_constraints(NULL), 00043 visibility_constraints_length(0), visibility_constraints(NULL) 00044 { 00045 } 00046 00047 virtual int serialize(unsigned char *outbuffer) const 00048 { 00049 int offset = 0; 00050 uint32_t length_name = strlen(this->name); 00051 varToArr(outbuffer + offset, length_name); 00052 offset += 4; 00053 memcpy(outbuffer + offset, this->name, length_name); 00054 offset += length_name; 00055 *(outbuffer + offset + 0) = (this->joint_constraints_length >> (8 * 0)) & 0xFF; 00056 *(outbuffer + offset + 1) = (this->joint_constraints_length >> (8 * 1)) & 0xFF; 00057 *(outbuffer + offset + 2) = (this->joint_constraints_length >> (8 * 2)) & 0xFF; 00058 *(outbuffer + offset + 3) = (this->joint_constraints_length >> (8 * 3)) & 0xFF; 00059 offset += sizeof(this->joint_constraints_length); 00060 for( uint32_t i = 0; i < joint_constraints_length; i++){ 00061 offset += this->joint_constraints[i].serialize(outbuffer + offset); 00062 } 00063 *(outbuffer + offset + 0) = (this->position_constraints_length >> (8 * 0)) & 0xFF; 00064 *(outbuffer + offset + 1) = (this->position_constraints_length >> (8 * 1)) & 0xFF; 00065 *(outbuffer + offset + 2) = (this->position_constraints_length >> (8 * 2)) & 0xFF; 00066 *(outbuffer + offset + 3) = (this->position_constraints_length >> (8 * 3)) & 0xFF; 00067 offset += sizeof(this->position_constraints_length); 00068 for( uint32_t i = 0; i < position_constraints_length; i++){ 00069 offset += this->position_constraints[i].serialize(outbuffer + offset); 00070 } 00071 *(outbuffer + offset + 0) = (this->orientation_constraints_length >> (8 * 0)) & 0xFF; 00072 *(outbuffer + offset + 1) = (this->orientation_constraints_length >> (8 * 1)) & 0xFF; 00073 *(outbuffer + offset + 2) = (this->orientation_constraints_length >> (8 * 2)) & 0xFF; 00074 *(outbuffer + offset + 3) = (this->orientation_constraints_length >> (8 * 3)) & 0xFF; 00075 offset += sizeof(this->orientation_constraints_length); 00076 for( uint32_t i = 0; i < orientation_constraints_length; i++){ 00077 offset += this->orientation_constraints[i].serialize(outbuffer + offset); 00078 } 00079 *(outbuffer + offset + 0) = (this->visibility_constraints_length >> (8 * 0)) & 0xFF; 00080 *(outbuffer + offset + 1) = (this->visibility_constraints_length >> (8 * 1)) & 0xFF; 00081 *(outbuffer + offset + 2) = (this->visibility_constraints_length >> (8 * 2)) & 0xFF; 00082 *(outbuffer + offset + 3) = (this->visibility_constraints_length >> (8 * 3)) & 0xFF; 00083 offset += sizeof(this->visibility_constraints_length); 00084 for( uint32_t i = 0; i < visibility_constraints_length; i++){ 00085 offset += this->visibility_constraints[i].serialize(outbuffer + offset); 00086 } 00087 return offset; 00088 } 00089 00090 virtual int deserialize(unsigned char *inbuffer) 00091 { 00092 int offset = 0; 00093 uint32_t length_name; 00094 arrToVar(length_name, (inbuffer + offset)); 00095 offset += 4; 00096 for(unsigned int k= offset; k< offset+length_name; ++k){ 00097 inbuffer[k-1]=inbuffer[k]; 00098 } 00099 inbuffer[offset+length_name-1]=0; 00100 this->name = (char *)(inbuffer + offset-1); 00101 offset += length_name; 00102 uint32_t joint_constraints_lengthT = ((uint32_t) (*(inbuffer + offset))); 00103 joint_constraints_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00104 joint_constraints_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00105 joint_constraints_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00106 offset += sizeof(this->joint_constraints_length); 00107 if(joint_constraints_lengthT > joint_constraints_length) 00108 this->joint_constraints = (moveit_msgs::JointConstraint*)realloc(this->joint_constraints, joint_constraints_lengthT * sizeof(moveit_msgs::JointConstraint)); 00109 joint_constraints_length = joint_constraints_lengthT; 00110 for( uint32_t i = 0; i < joint_constraints_length; i++){ 00111 offset += this->st_joint_constraints.deserialize(inbuffer + offset); 00112 memcpy( &(this->joint_constraints[i]), &(this->st_joint_constraints), sizeof(moveit_msgs::JointConstraint)); 00113 } 00114 uint32_t position_constraints_lengthT = ((uint32_t) (*(inbuffer + offset))); 00115 position_constraints_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00116 position_constraints_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00117 position_constraints_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00118 offset += sizeof(this->position_constraints_length); 00119 if(position_constraints_lengthT > position_constraints_length) 00120 this->position_constraints = (moveit_msgs::PositionConstraint*)realloc(this->position_constraints, position_constraints_lengthT * sizeof(moveit_msgs::PositionConstraint)); 00121 position_constraints_length = position_constraints_lengthT; 00122 for( uint32_t i = 0; i < position_constraints_length; i++){ 00123 offset += this->st_position_constraints.deserialize(inbuffer + offset); 00124 memcpy( &(this->position_constraints[i]), &(this->st_position_constraints), sizeof(moveit_msgs::PositionConstraint)); 00125 } 00126 uint32_t orientation_constraints_lengthT = ((uint32_t) (*(inbuffer + offset))); 00127 orientation_constraints_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00128 orientation_constraints_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00129 orientation_constraints_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00130 offset += sizeof(this->orientation_constraints_length); 00131 if(orientation_constraints_lengthT > orientation_constraints_length) 00132 this->orientation_constraints = (moveit_msgs::OrientationConstraint*)realloc(this->orientation_constraints, orientation_constraints_lengthT * sizeof(moveit_msgs::OrientationConstraint)); 00133 orientation_constraints_length = orientation_constraints_lengthT; 00134 for( uint32_t i = 0; i < orientation_constraints_length; i++){ 00135 offset += this->st_orientation_constraints.deserialize(inbuffer + offset); 00136 memcpy( &(this->orientation_constraints[i]), &(this->st_orientation_constraints), sizeof(moveit_msgs::OrientationConstraint)); 00137 } 00138 uint32_t visibility_constraints_lengthT = ((uint32_t) (*(inbuffer + offset))); 00139 visibility_constraints_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 00140 visibility_constraints_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 00141 visibility_constraints_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 00142 offset += sizeof(this->visibility_constraints_length); 00143 if(visibility_constraints_lengthT > visibility_constraints_length) 00144 this->visibility_constraints = (moveit_msgs::VisibilityConstraint*)realloc(this->visibility_constraints, visibility_constraints_lengthT * sizeof(moveit_msgs::VisibilityConstraint)); 00145 visibility_constraints_length = visibility_constraints_lengthT; 00146 for( uint32_t i = 0; i < visibility_constraints_length; i++){ 00147 offset += this->st_visibility_constraints.deserialize(inbuffer + offset); 00148 memcpy( &(this->visibility_constraints[i]), &(this->st_visibility_constraints), sizeof(moveit_msgs::VisibilityConstraint)); 00149 } 00150 return offset; 00151 } 00152 00153 virtual const char * getType(){ return "moveit_msgs/Constraints"; }; 00154 virtual const char * getMD5(){ return "8d5ce8d34ef26c65fb5d43c9e99bf6e0"; }; 00155 00156 }; 00157 00158 } 00159 #endif
Generated on Mon Sep 26 2022 13:47:00 by
1.7.2