catchrobo2022 / Mbed 2 deprecated 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

ros_lib/ros_lib/moveit_msgs/GraspPlanning.h@0:803105042c95, 2022-09-26 (annotated)

Committer:
shimizuta
Date:
Mon Sep 26 13:45:05 2022 +0000
Revision:
0:803105042c95
catchrobo2022 finish

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shimizuta 0:803105042c95 1 #ifndef _ROS_SERVICE_GraspPlanning_h
shimizuta 0:803105042c95 2 #define _ROS_SERVICE_GraspPlanning_h
shimizuta 0:803105042c95 3 #include <stdint.h>
shimizuta 0:803105042c95 4 #include <string.h>
shimizuta 0:803105042c95 5 #include <stdlib.h>
shimizuta 0:803105042c95 6 #include "ros/msg.h"
shimizuta 0:803105042c95 7 #include "moveit_msgs/Grasp.h"
shimizuta 0:803105042c95 8 #include "moveit_msgs/MoveItErrorCodes.h"
shimizuta 0:803105042c95 9 #include "moveit_msgs/CollisionObject.h"
shimizuta 0:803105042c95 10
shimizuta 0:803105042c95 11 namespace moveit_msgs
shimizuta 0:803105042c95 12 {
shimizuta 0:803105042c95 13
shimizuta 0:803105042c95 14 static const char GRASPPLANNING[] = "moveit_msgs/GraspPlanning";
shimizuta 0:803105042c95 15
shimizuta 0:803105042c95 16 class GraspPlanningRequest : public ros::Msg
shimizuta 0:803105042c95 17 {
shimizuta 0:803105042c95 18 public:
shimizuta 0:803105042c95 19 typedef const char* _group_name_type;
shimizuta 0:803105042c95 20 _group_name_type group_name;
shimizuta 0:803105042c95 21 typedef moveit_msgs::CollisionObject _target_type;
shimizuta 0:803105042c95 22 _target_type target;
shimizuta 0:803105042c95 23 uint32_t support_surfaces_length;
shimizuta 0:803105042c95 24 typedef char* _support_surfaces_type;
shimizuta 0:803105042c95 25 _support_surfaces_type st_support_surfaces;
shimizuta 0:803105042c95 26 _support_surfaces_type * support_surfaces;
shimizuta 0:803105042c95 27 uint32_t candidate_grasps_length;
shimizuta 0:803105042c95 28 typedef moveit_msgs::Grasp _candidate_grasps_type;
shimizuta 0:803105042c95 29 _candidate_grasps_type st_candidate_grasps;
shimizuta 0:803105042c95 30 _candidate_grasps_type * candidate_grasps;
shimizuta 0:803105042c95 31 uint32_t movable_obstacles_length;
shimizuta 0:803105042c95 32 typedef moveit_msgs::CollisionObject _movable_obstacles_type;
shimizuta 0:803105042c95 33 _movable_obstacles_type st_movable_obstacles;
shimizuta 0:803105042c95 34 _movable_obstacles_type * movable_obstacles;
shimizuta 0:803105042c95 35
shimizuta 0:803105042c95 36 GraspPlanningRequest():
shimizuta 0:803105042c95 37 group_name(""),
shimizuta 0:803105042c95 38 target(),
shimizuta 0:803105042c95 39 support_surfaces_length(0), support_surfaces(NULL),
shimizuta 0:803105042c95 40 candidate_grasps_length(0), candidate_grasps(NULL),
shimizuta 0:803105042c95 41 movable_obstacles_length(0), movable_obstacles(NULL)
shimizuta 0:803105042c95 42 {
shimizuta 0:803105042c95 43 }
shimizuta 0:803105042c95 44
shimizuta 0:803105042c95 45 virtual int serialize(unsigned char *outbuffer) const
shimizuta 0:803105042c95 46 {
shimizuta 0:803105042c95 47 int offset = 0;
shimizuta 0:803105042c95 48 uint32_t length_group_name = strlen(this->group_name);
shimizuta 0:803105042c95 49 varToArr(outbuffer + offset, length_group_name);
shimizuta 0:803105042c95 50 offset += 4;
shimizuta 0:803105042c95 51 memcpy(outbuffer + offset, this->group_name, length_group_name);
shimizuta 0:803105042c95 52 offset += length_group_name;
shimizuta 0:803105042c95 53 offset += this->target.serialize(outbuffer + offset);
shimizuta 0:803105042c95 54 *(outbuffer + offset + 0) = (this->support_surfaces_length >> (8 * 0)) & 0xFF;
shimizuta 0:803105042c95 55 *(outbuffer + offset + 1) = (this->support_surfaces_length >> (8 * 1)) & 0xFF;
shimizuta 0:803105042c95 56 *(outbuffer + offset + 2) = (this->support_surfaces_length >> (8 * 2)) & 0xFF;
shimizuta 0:803105042c95 57 *(outbuffer + offset + 3) = (this->support_surfaces_length >> (8 * 3)) & 0xFF;
shimizuta 0:803105042c95 58 offset += sizeof(this->support_surfaces_length);
shimizuta 0:803105042c95 59 for( uint32_t i = 0; i < support_surfaces_length; i++){
shimizuta 0:803105042c95 60 uint32_t length_support_surfacesi = strlen(this->support_surfaces[i]);
shimizuta 0:803105042c95 61 varToArr(outbuffer + offset, length_support_surfacesi);
shimizuta 0:803105042c95 62 offset += 4;
shimizuta 0:803105042c95 63 memcpy(outbuffer + offset, this->support_surfaces[i], length_support_surfacesi);
shimizuta 0:803105042c95 64 offset += length_support_surfacesi;
shimizuta 0:803105042c95 65 }
shimizuta 0:803105042c95 66 *(outbuffer + offset + 0) = (this->candidate_grasps_length >> (8 * 0)) & 0xFF;
shimizuta 0:803105042c95 67 *(outbuffer + offset + 1) = (this->candidate_grasps_length >> (8 * 1)) & 0xFF;
shimizuta 0:803105042c95 68 *(outbuffer + offset + 2) = (this->candidate_grasps_length >> (8 * 2)) & 0xFF;
shimizuta 0:803105042c95 69 *(outbuffer + offset + 3) = (this->candidate_grasps_length >> (8 * 3)) & 0xFF;
shimizuta 0:803105042c95 70 offset += sizeof(this->candidate_grasps_length);
shimizuta 0:803105042c95 71 for( uint32_t i = 0; i < candidate_grasps_length; i++){
shimizuta 0:803105042c95 72 offset += this->candidate_grasps[i].serialize(outbuffer + offset);
shimizuta 0:803105042c95 73 }
shimizuta 0:803105042c95 74 *(outbuffer + offset + 0) = (this->movable_obstacles_length >> (8 * 0)) & 0xFF;
shimizuta 0:803105042c95 75 *(outbuffer + offset + 1) = (this->movable_obstacles_length >> (8 * 1)) & 0xFF;
shimizuta 0:803105042c95 76 *(outbuffer + offset + 2) = (this->movable_obstacles_length >> (8 * 2)) & 0xFF;
shimizuta 0:803105042c95 77 *(outbuffer + offset + 3) = (this->movable_obstacles_length >> (8 * 3)) & 0xFF;
shimizuta 0:803105042c95 78 offset += sizeof(this->movable_obstacles_length);
shimizuta 0:803105042c95 79 for( uint32_t i = 0; i < movable_obstacles_length; i++){
shimizuta 0:803105042c95 80 offset += this->movable_obstacles[i].serialize(outbuffer + offset);
shimizuta 0:803105042c95 81 }
shimizuta 0:803105042c95 82 return offset;
shimizuta 0:803105042c95 83 }
shimizuta 0:803105042c95 84
shimizuta 0:803105042c95 85 virtual int deserialize(unsigned char *inbuffer)
shimizuta 0:803105042c95 86 {
shimizuta 0:803105042c95 87 int offset = 0;
shimizuta 0:803105042c95 88 uint32_t length_group_name;
shimizuta 0:803105042c95 89 arrToVar(length_group_name, (inbuffer + offset));
shimizuta 0:803105042c95 90 offset += 4;
shimizuta 0:803105042c95 91 for(unsigned int k= offset; k< offset+length_group_name; ++k){
shimizuta 0:803105042c95 92 inbuffer[k-1]=inbuffer[k];
shimizuta 0:803105042c95 93 }
shimizuta 0:803105042c95 94 inbuffer[offset+length_group_name-1]=0;
shimizuta 0:803105042c95 95 this->group_name = (char *)(inbuffer + offset-1);
shimizuta 0:803105042c95 96 offset += length_group_name;
shimizuta 0:803105042c95 97 offset += this->target.deserialize(inbuffer + offset);
shimizuta 0:803105042c95 98 uint32_t support_surfaces_lengthT = ((uint32_t) (*(inbuffer + offset)));
shimizuta 0:803105042c95 99 support_surfaces_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
shimizuta 0:803105042c95 100 support_surfaces_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
shimizuta 0:803105042c95 101 support_surfaces_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
shimizuta 0:803105042c95 102 offset += sizeof(this->support_surfaces_length);
shimizuta 0:803105042c95 103 if(support_surfaces_lengthT > support_surfaces_length)
shimizuta 0:803105042c95 104 this->support_surfaces = (char**)realloc(this->support_surfaces, support_surfaces_lengthT * sizeof(char*));
shimizuta 0:803105042c95 105 support_surfaces_length = support_surfaces_lengthT;
shimizuta 0:803105042c95 106 for( uint32_t i = 0; i < support_surfaces_length; i++){
shimizuta 0:803105042c95 107 uint32_t length_st_support_surfaces;
shimizuta 0:803105042c95 108 arrToVar(length_st_support_surfaces, (inbuffer + offset));
shimizuta 0:803105042c95 109 offset += 4;
shimizuta 0:803105042c95 110 for(unsigned int k= offset; k< offset+length_st_support_surfaces; ++k){
shimizuta 0:803105042c95 111 inbuffer[k-1]=inbuffer[k];
shimizuta 0:803105042c95 112 }
shimizuta 0:803105042c95 113 inbuffer[offset+length_st_support_surfaces-1]=0;
shimizuta 0:803105042c95 114 this->st_support_surfaces = (char *)(inbuffer + offset-1);
shimizuta 0:803105042c95 115 offset += length_st_support_surfaces;
shimizuta 0:803105042c95 116 memcpy( &(this->support_surfaces[i]), &(this->st_support_surfaces), sizeof(char*));
shimizuta 0:803105042c95 117 }
shimizuta 0:803105042c95 118 uint32_t candidate_grasps_lengthT = ((uint32_t) (*(inbuffer + offset)));
shimizuta 0:803105042c95 119 candidate_grasps_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
shimizuta 0:803105042c95 120 candidate_grasps_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
shimizuta 0:803105042c95 121 candidate_grasps_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
shimizuta 0:803105042c95 122 offset += sizeof(this->candidate_grasps_length);
shimizuta 0:803105042c95 123 if(candidate_grasps_lengthT > candidate_grasps_length)
shimizuta 0:803105042c95 124 this->candidate_grasps = (moveit_msgs::Grasp*)realloc(this->candidate_grasps, candidate_grasps_lengthT * sizeof(moveit_msgs::Grasp));
shimizuta 0:803105042c95 125 candidate_grasps_length = candidate_grasps_lengthT;
shimizuta 0:803105042c95 126 for( uint32_t i = 0; i < candidate_grasps_length; i++){
shimizuta 0:803105042c95 127 offset += this->st_candidate_grasps.deserialize(inbuffer + offset);
shimizuta 0:803105042c95 128 memcpy( &(this->candidate_grasps[i]), &(this->st_candidate_grasps), sizeof(moveit_msgs::Grasp));
shimizuta 0:803105042c95 129 }
shimizuta 0:803105042c95 130 uint32_t movable_obstacles_lengthT = ((uint32_t) (*(inbuffer + offset)));
shimizuta 0:803105042c95 131 movable_obstacles_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
shimizuta 0:803105042c95 132 movable_obstacles_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
shimizuta 0:803105042c95 133 movable_obstacles_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
shimizuta 0:803105042c95 134 offset += sizeof(this->movable_obstacles_length);
shimizuta 0:803105042c95 135 if(movable_obstacles_lengthT > movable_obstacles_length)
shimizuta 0:803105042c95 136 this->movable_obstacles = (moveit_msgs::CollisionObject*)realloc(this->movable_obstacles, movable_obstacles_lengthT * sizeof(moveit_msgs::CollisionObject));
shimizuta 0:803105042c95 137 movable_obstacles_length = movable_obstacles_lengthT;
shimizuta 0:803105042c95 138 for( uint32_t i = 0; i < movable_obstacles_length; i++){
shimizuta 0:803105042c95 139 offset += this->st_movable_obstacles.deserialize(inbuffer + offset);
shimizuta 0:803105042c95 140 memcpy( &(this->movable_obstacles[i]), &(this->st_movable_obstacles), sizeof(moveit_msgs::CollisionObject));
shimizuta 0:803105042c95 141 }
shimizuta 0:803105042c95 142 return offset;
shimizuta 0:803105042c95 143 }
shimizuta 0:803105042c95 144
shimizuta 0:803105042c95 145 virtual const char * getType(){ return GRASPPLANNING; };
shimizuta 0:803105042c95 146 virtual const char * getMD5(){ return "c234e9a645708cc86b57a43999746ae6"; };
shimizuta 0:803105042c95 147
shimizuta 0:803105042c95 148 };
shimizuta 0:803105042c95 149
shimizuta 0:803105042c95 150 class GraspPlanningResponse : public ros::Msg
shimizuta 0:803105042c95 151 {
shimizuta 0:803105042c95 152 public:
shimizuta 0:803105042c95 153 uint32_t grasps_length;
shimizuta 0:803105042c95 154 typedef moveit_msgs::Grasp _grasps_type;
shimizuta 0:803105042c95 155 _grasps_type st_grasps;
shimizuta 0:803105042c95 156 _grasps_type * grasps;
shimizuta 0:803105042c95 157 typedef moveit_msgs::MoveItErrorCodes _error_code_type;
shimizuta 0:803105042c95 158 _error_code_type error_code;
shimizuta 0:803105042c95 159
shimizuta 0:803105042c95 160 GraspPlanningResponse():
shimizuta 0:803105042c95 161 grasps_length(0), grasps(NULL),
shimizuta 0:803105042c95 162 error_code()
shimizuta 0:803105042c95 163 {
shimizuta 0:803105042c95 164 }
shimizuta 0:803105042c95 165
shimizuta 0:803105042c95 166 virtual int serialize(unsigned char *outbuffer) const
shimizuta 0:803105042c95 167 {
shimizuta 0:803105042c95 168 int offset = 0;
shimizuta 0:803105042c95 169 *(outbuffer + offset + 0) = (this->grasps_length >> (8 * 0)) & 0xFF;
shimizuta 0:803105042c95 170 *(outbuffer + offset + 1) = (this->grasps_length >> (8 * 1)) & 0xFF;
shimizuta 0:803105042c95 171 *(outbuffer + offset + 2) = (this->grasps_length >> (8 * 2)) & 0xFF;
shimizuta 0:803105042c95 172 *(outbuffer + offset + 3) = (this->grasps_length >> (8 * 3)) & 0xFF;
shimizuta 0:803105042c95 173 offset += sizeof(this->grasps_length);
shimizuta 0:803105042c95 174 for( uint32_t i = 0; i < grasps_length; i++){
shimizuta 0:803105042c95 175 offset += this->grasps[i].serialize(outbuffer + offset);
shimizuta 0:803105042c95 176 }
shimizuta 0:803105042c95 177 offset += this->error_code.serialize(outbuffer + offset);
shimizuta 0:803105042c95 178 return offset;
shimizuta 0:803105042c95 179 }
shimizuta 0:803105042c95 180
shimizuta 0:803105042c95 181 virtual int deserialize(unsigned char *inbuffer)
shimizuta 0:803105042c95 182 {
shimizuta 0:803105042c95 183 int offset = 0;
shimizuta 0:803105042c95 184 uint32_t grasps_lengthT = ((uint32_t) (*(inbuffer + offset)));
shimizuta 0:803105042c95 185 grasps_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
shimizuta 0:803105042c95 186 grasps_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
shimizuta 0:803105042c95 187 grasps_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
shimizuta 0:803105042c95 188 offset += sizeof(this->grasps_length);
shimizuta 0:803105042c95 189 if(grasps_lengthT > grasps_length)
shimizuta 0:803105042c95 190 this->grasps = (moveit_msgs::Grasp*)realloc(this->grasps, grasps_lengthT * sizeof(moveit_msgs::Grasp));
shimizuta 0:803105042c95 191 grasps_length = grasps_lengthT;
shimizuta 0:803105042c95 192 for( uint32_t i = 0; i < grasps_length; i++){
shimizuta 0:803105042c95 193 offset += this->st_grasps.deserialize(inbuffer + offset);
shimizuta 0:803105042c95 194 memcpy( &(this->grasps[i]), &(this->st_grasps), sizeof(moveit_msgs::Grasp));
shimizuta 0:803105042c95 195 }
shimizuta 0:803105042c95 196 offset += this->error_code.deserialize(inbuffer + offset);
shimizuta 0:803105042c95 197 return offset;
shimizuta 0:803105042c95 198 }
shimizuta 0:803105042c95 199
shimizuta 0:803105042c95 200 virtual const char * getType(){ return GRASPPLANNING; };
shimizuta 0:803105042c95 201 virtual const char * getMD5(){ return "bb8169d403b6e9f96bf61e22a50d13ae"; };
shimizuta 0:803105042c95 202
shimizuta 0:803105042c95 203 };
shimizuta 0:803105042c95 204
shimizuta 0:803105042c95 205 class GraspPlanning {
shimizuta 0:803105042c95 206 public:
shimizuta 0:803105042c95 207 typedef GraspPlanningRequest Request;
shimizuta 0:803105042c95 208 typedef GraspPlanningResponse Response;
shimizuta 0:803105042c95 209 };
shimizuta 0:803105042c95 210
shimizuta 0:803105042c95 211 }
shimizuta 0:803105042c95 212 #endif