00001
00002 #ifndef MOTION_PLANNING_MSGS_MESSAGE_MOTIONPLANREQUEST_H
00003 #define MOTION_PLANNING_MSGS_MESSAGE_MOTIONPLANREQUEST_H
00004 #include <string>
00005 #include <vector>
00006 #include <ostream>
00007 #include "ros/serialization.h"
00008 #include "ros/builtin_message_traits.h"
00009 #include "ros/message_operations.h"
00010 #include "ros/message.h"
00011 #include "ros/time.h"
00012
00013 #include "motion_planning_msgs/WorkspaceParameters.h"
00014 #include "motion_planning_msgs/RobotState.h"
00015 #include "motion_planning_msgs/Constraints.h"
00016 #include "motion_planning_msgs/Constraints.h"
00017 #include "motion_planning_msgs/AllowedContactSpecification.h"
00018 #include "motion_planning_msgs/OrderedCollisionOperations.h"
00019 #include "motion_planning_msgs/LinkPadding.h"
00020
00021 namespace motion_planning_msgs
00022 {
00023 template <class ContainerAllocator>
00024 struct MotionPlanRequest_ : public ros::Message
00025 {
00026 typedef MotionPlanRequest_<ContainerAllocator> Type;
00027
00028 MotionPlanRequest_()
00029 : workspace_parameters()
00030 , start_state()
00031 , goal_constraints()
00032 , path_constraints()
00033 , allowed_contacts()
00034 , ordered_collision_operations()
00035 , link_padding()
00036 , planner_id()
00037 , group_name()
00038 , num_planning_attempts(0)
00039 , allowed_planning_time()
00040 , expected_path_duration()
00041 , expected_path_dt()
00042 {
00043 }
00044
00045 MotionPlanRequest_(const ContainerAllocator& _alloc)
00046 : workspace_parameters(_alloc)
00047 , start_state(_alloc)
00048 , goal_constraints(_alloc)
00049 , path_constraints(_alloc)
00050 , allowed_contacts(_alloc)
00051 , ordered_collision_operations(_alloc)
00052 , link_padding(_alloc)
00053 , planner_id(_alloc)
00054 , group_name(_alloc)
00055 , num_planning_attempts(0)
00056 , allowed_planning_time()
00057 , expected_path_duration()
00058 , expected_path_dt()
00059 {
00060 }
00061
00062 typedef ::motion_planning_msgs::WorkspaceParameters_<ContainerAllocator> _workspace_parameters_type;
00063 ::motion_planning_msgs::WorkspaceParameters_<ContainerAllocator> workspace_parameters;
00064
00065 typedef ::motion_planning_msgs::RobotState_<ContainerAllocator> _start_state_type;
00066 ::motion_planning_msgs::RobotState_<ContainerAllocator> start_state;
00067
00068 typedef ::motion_planning_msgs::Constraints_<ContainerAllocator> _goal_constraints_type;
00069 ::motion_planning_msgs::Constraints_<ContainerAllocator> goal_constraints;
00070
00071 typedef ::motion_planning_msgs::Constraints_<ContainerAllocator> _path_constraints_type;
00072 ::motion_planning_msgs::Constraints_<ContainerAllocator> path_constraints;
00073
00074 typedef std::vector< ::motion_planning_msgs::AllowedContactSpecification_<ContainerAllocator> , typename ContainerAllocator::template rebind< ::motion_planning_msgs::AllowedContactSpecification_<ContainerAllocator> >::other > _allowed_contacts_type;
00075 std::vector< ::motion_planning_msgs::AllowedContactSpecification_<ContainerAllocator> , typename ContainerAllocator::template rebind< ::motion_planning_msgs::AllowedContactSpecification_<ContainerAllocator> >::other > allowed_contacts;
00076
00077 typedef ::motion_planning_msgs::OrderedCollisionOperations_<ContainerAllocator> _ordered_collision_operations_type;
00078 ::motion_planning_msgs::OrderedCollisionOperations_<ContainerAllocator> ordered_collision_operations;
00079
00080 typedef std::vector< ::motion_planning_msgs::LinkPadding_<ContainerAllocator> , typename ContainerAllocator::template rebind< ::motion_planning_msgs::LinkPadding_<ContainerAllocator> >::other > _link_padding_type;
00081 std::vector< ::motion_planning_msgs::LinkPadding_<ContainerAllocator> , typename ContainerAllocator::template rebind< ::motion_planning_msgs::LinkPadding_<ContainerAllocator> >::other > link_padding;
00082
00083 typedef std::basic_string<char, std::char_traits<char>, typename ContainerAllocator::template rebind<char>::other > _planner_id_type;
00084 std::basic_string<char, std::char_traits<char>, typename ContainerAllocator::template rebind<char>::other > planner_id;
00085
00086 typedef std::basic_string<char, std::char_traits<char>, typename ContainerAllocator::template rebind<char>::other > _group_name_type;
00087 std::basic_string<char, std::char_traits<char>, typename ContainerAllocator::template rebind<char>::other > group_name;
00088
00089 typedef int32_t _num_planning_attempts_type;
00090 int32_t num_planning_attempts;
00091
00092 typedef ros::Duration _allowed_planning_time_type;
00093 ros::Duration allowed_planning_time;
00094
00095 typedef ros::Duration _expected_path_duration_type;
00096 ros::Duration expected_path_duration;
00097
00098 typedef ros::Duration _expected_path_dt_type;
00099 ros::Duration expected_path_dt;
00100
00101
00102 ROS_DEPRECATED uint32_t get_allowed_contacts_size() const { return (uint32_t)allowed_contacts.size(); }
00103 ROS_DEPRECATED void set_allowed_contacts_size(uint32_t size) { allowed_contacts.resize((size_t)size); }
00104 ROS_DEPRECATED void get_allowed_contacts_vec(std::vector< ::motion_planning_msgs::AllowedContactSpecification_<ContainerAllocator> , typename ContainerAllocator::template rebind< ::motion_planning_msgs::AllowedContactSpecification_<ContainerAllocator> >::other > & vec) const { vec = this->allowed_contacts; }
00105 ROS_DEPRECATED void set_allowed_contacts_vec(const std::vector< ::motion_planning_msgs::AllowedContactSpecification_<ContainerAllocator> , typename ContainerAllocator::template rebind< ::motion_planning_msgs::AllowedContactSpecification_<ContainerAllocator> >::other > & vec) { this->allowed_contacts = vec; }
00106 ROS_DEPRECATED uint32_t get_link_padding_size() const { return (uint32_t)link_padding.size(); }
00107 ROS_DEPRECATED void set_link_padding_size(uint32_t size) { link_padding.resize((size_t)size); }
00108 ROS_DEPRECATED void get_link_padding_vec(std::vector< ::motion_planning_msgs::LinkPadding_<ContainerAllocator> , typename ContainerAllocator::template rebind< ::motion_planning_msgs::LinkPadding_<ContainerAllocator> >::other > & vec) const { vec = this->link_padding; }
00109 ROS_DEPRECATED void set_link_padding_vec(const std::vector< ::motion_planning_msgs::LinkPadding_<ContainerAllocator> , typename ContainerAllocator::template rebind< ::motion_planning_msgs::LinkPadding_<ContainerAllocator> >::other > & vec) { this->link_padding = vec; }
00110 private:
00111 static const char* __s_getDataType_() { return "motion_planning_msgs/MotionPlanRequest"; }
00112 public:
00113 ROS_DEPRECATED static const std::string __s_getDataType() { return __s_getDataType_(); }
00114
00115 ROS_DEPRECATED const std::string __getDataType() const { return __s_getDataType_(); }
00116
00117 private:
00118 static const char* __s_getMD5Sum_() { return "d7f3952531fa0e5f7133e2875517c71d"; }
00119 public:
00120 ROS_DEPRECATED static const std::string __s_getMD5Sum() { return __s_getMD5Sum_(); }
00121
00122 ROS_DEPRECATED const std::string __getMD5Sum() const { return __s_getMD5Sum_(); }
00123
00124 private:
00125 static const char* __s_getMessageDefinition_() { return "# This service contains the definition for a request to the motion\n\
00126 # planner and the output it provides\n\
00127 \n\
00128 # Parameters for the workspace that the planner should work inside\n\
00129 motion_planning_msgs/WorkspaceParameters workspace_parameters\n\
00130 \n\
00131 # Starting state updates. If certain joints should be considered\n\
00132 # at positions other than the current ones, these positions should\n\
00133 # be set here\n\
00134 motion_planning_msgs/RobotState start_state\n\
00135 \n\
00136 # The goal state for the model to plan for. The goal is achieved\n\
00137 # if all constraints are satisfied\n\
00138 motion_planning_msgs/Constraints goal_constraints\n\
00139 \n\
00140 # No state at any point along the path in the produced motion plan will violate these constraints\n\
00141 motion_planning_msgs/Constraints path_constraints\n\
00142 \n\
00143 # A specification for regions where contact is \n\
00144 # allowed up to a certain depth\n\
00145 # Any collision within this set of regions with a link \n\
00146 # specified in the message will be allowed if\n\
00147 # it is less than the penetration depth specified in the message\n\
00148 AllowedContactSpecification[] allowed_contacts\n\
00149 \n\
00150 # A set of ordered collision operations, \n\
00151 # these are applied to all links, objects, \n\
00152 # namespaces in the collision space\n\
00153 OrderedCollisionOperations ordered_collision_operations\n\
00154 \n\
00155 # Specifies a set of links and paddings to change from the default\n\
00156 # specified in the yaml file\n\
00157 motion_planning_msgs/LinkPadding[] link_padding\n\
00158 \n\
00159 # The name of the motion planner to use. If no name is specified,\n\
00160 # a default motion planner will be used\n\
00161 string planner_id\n\
00162 \n\
00163 # The name of the group of joints on which this planner is operating\n\
00164 string group_name\n\
00165 \n\
00166 # The number of times this plan is to be computed. Shortest solution\n\
00167 # will be reported.\n\
00168 int32 num_planning_attempts\n\
00169 \n\
00170 # The maximum amount of time the motion planner is allowed to plan for\n\
00171 duration allowed_planning_time\n\
00172 \n\
00173 # An expected path duration (in seconds) along with an expected discretization of the path allows the planner to determine the discretization of the trajectory that it returns\n\
00174 duration expected_path_duration\n\
00175 duration expected_path_dt\n\
00176 \n\
00177 ================================================================================\n\
00178 MSG: motion_planning_msgs/WorkspaceParameters\n\
00179 # This message contains a set of parameters useful in\n\
00180 # setting up the workspace for planning\n\
00181 geometric_shapes_msgs/Shape workspace_region_shape\n\
00182 geometry_msgs/PoseStamped workspace_region_pose\n\
00183 \n\
00184 \n\
00185 ================================================================================\n\
00186 MSG: geometric_shapes_msgs/Shape\n\
00187 byte SPHERE=0\n\
00188 byte BOX=1\n\
00189 byte CYLINDER=2\n\
00190 byte MESH=3\n\
00191 \n\
00192 byte type\n\
00193 \n\
00194 \n\
00195 #### define sphere, box, cylinder ####\n\
00196 # the origin of each shape is considered at the shape's center\n\
00197 \n\
00198 # for sphere\n\
00199 # radius := dimensions[0]\n\
00200 \n\
00201 # for cylinder\n\
00202 # radius := dimensions[0]\n\
00203 # length := dimensions[1]\n\
00204 # the length is along the Z axis\n\
00205 \n\
00206 # for box\n\
00207 # size_x := dimensions[0]\n\
00208 # size_y := dimensions[1]\n\
00209 # size_z := dimensions[2]\n\
00210 float64[] dimensions\n\
00211 \n\
00212 \n\
00213 #### define mesh ####\n\
00214 \n\
00215 # list of triangles; triangle k is defined by tre vertices located\n\
00216 # at indices triangles[3k], triangles[3k+1], triangles[3k+2]\n\
00217 int32[] triangles\n\
00218 geometry_msgs/Point[] vertices\n\
00219 \n\
00220 ================================================================================\n\
00221 MSG: geometry_msgs/Point\n\
00222 # This contains the position of a point in free space\n\
00223 float64 x\n\
00224 float64 y\n\
00225 float64 z\n\
00226 \n\
00227 ================================================================================\n\
00228 MSG: geometry_msgs/PoseStamped\n\
00229 # A Pose with reference coordinate frame and timestamp\n\
00230 Header header\n\
00231 Pose pose\n\
00232 \n\
00233 ================================================================================\n\
00234 MSG: std_msgs/Header\n\
00235 # Standard metadata for higher-level stamped data types.\n\
00236 # This is generally used to communicate timestamped data \n\
00237 # in a particular coordinate frame.\n\
00238 # \n\
00239 # sequence ID: consecutively increasing ID \n\
00240 uint32 seq\n\
00241 #Two-integer timestamp that is expressed as:\n\
00242 # * stamp.secs: seconds (stamp_secs) since epoch\n\
00243 # * stamp.nsecs: nanoseconds since stamp_secs\n\
00244 # time-handling sugar is provided by the client library\n\
00245 time stamp\n\
00246 #Frame this data is associated with\n\
00247 # 0: no frame\n\
00248 # 1: global frame\n\
00249 string frame_id\n\
00250 \n\
00251 ================================================================================\n\
00252 MSG: geometry_msgs/Pose\n\
00253 # A representation of pose in free space, composed of postion and orientation. \n\
00254 Point position\n\
00255 Quaternion orientation\n\
00256 \n\
00257 ================================================================================\n\
00258 MSG: geometry_msgs/Quaternion\n\
00259 # This represents an orientation in free space in quaternion form.\n\
00260 \n\
00261 float64 x\n\
00262 float64 y\n\
00263 float64 z\n\
00264 float64 w\n\
00265 \n\
00266 ================================================================================\n\
00267 MSG: motion_planning_msgs/RobotState\n\
00268 # This message contains information about the robot state, i.e. the positions of its joints and links\n\
00269 sensor_msgs/JointState joint_state\n\
00270 motion_planning_msgs/MultiDOFJointState multi_dof_joint_state\n\
00271 ================================================================================\n\
00272 MSG: sensor_msgs/JointState\n\
00273 # This is a message that holds data to describe the state of a set of torque controlled joints. \n\
00274 #\n\
00275 # The state of each joint (revolute or prismatic) is defined by:\n\
00276 # * the position of the joint (rad or m),\n\
00277 # * the velocity of the joint (rad/s or m/s) and \n\
00278 # * the effort that is applied in the joint (Nm or N).\n\
00279 #\n\
00280 # Each joint is uniquely identified by its name\n\
00281 # The header specifies the time at which the joint states were recorded. All the joint states\n\
00282 # in one message have to be recorded at the same time.\n\
00283 #\n\
00284 # This message consists of a multiple arrays, one for each part of the joint state. \n\
00285 # The goal is to make each of the fields optional. When e.g. your joints have no\n\
00286 # effort associated with them, you can leave the effort array empty. \n\
00287 #\n\
00288 # All arrays in this message should have the same size, or be empty.\n\
00289 # This is the only way to uniquely associate the joint name with the correct\n\
00290 # states.\n\
00291 \n\
00292 \n\
00293 Header header\n\
00294 \n\
00295 string[] name\n\
00296 float64[] position\n\
00297 float64[] velocity\n\
00298 float64[] effort\n\
00299 \n\
00300 ================================================================================\n\
00301 MSG: motion_planning_msgs/MultiDOFJointState\n\
00302 #A representation of a multi-dof joint state\n\
00303 time stamp\n\
00304 string[] joint_names\n\
00305 string[] frame_ids\n\
00306 string[] child_frame_ids\n\
00307 geometry_msgs/Pose[] poses\n\
00308 \n\
00309 ================================================================================\n\
00310 MSG: motion_planning_msgs/Constraints\n\
00311 # This message contains a list of motion planning constraints.\n\
00312 \n\
00313 motion_planning_msgs/JointConstraint[] joint_constraints\n\
00314 motion_planning_msgs/PositionConstraint[] position_constraints\n\
00315 motion_planning_msgs/OrientationConstraint[] orientation_constraints\n\
00316 motion_planning_msgs/VisibilityConstraint[] visibility_constraints\n\
00317 \n\
00318 ================================================================================\n\
00319 MSG: motion_planning_msgs/JointConstraint\n\
00320 # Constrain the position of a joint to be within a certain bound\n\
00321 string joint_name\n\
00322 \n\
00323 # the bound to be achieved is [position - tolerance_below, position + tolerance_above]\n\
00324 float64 position\n\
00325 float64 tolerance_above\n\
00326 float64 tolerance_below\n\
00327 \n\
00328 # A weighting factor for this constraint\n\
00329 float64 weight\n\
00330 ================================================================================\n\
00331 MSG: motion_planning_msgs/PositionConstraint\n\
00332 # This message contains the definition of a position constraint.\n\
00333 Header header\n\
00334 \n\
00335 # The robot link this constraint refers to\n\
00336 string link_name\n\
00337 \n\
00338 # The offset (in the link frame) for the target point on the link we are planning for\n\
00339 geometry_msgs/Point target_point_offset\n\
00340 \n\
00341 # The nominal/target position for the point we are planning for\n\
00342 geometry_msgs/Point position\n\
00343 \n\
00344 # The shape of the bounded region that constrains the position of the end-effector\n\
00345 # This region is always centered at the position defined above\n\
00346 geometric_shapes_msgs/Shape constraint_region_shape\n\
00347 \n\
00348 # The orientation of the bounded region that constrains the position of the end-effector. \n\
00349 # This allows the specification of non-axis aligned constraints\n\
00350 geometry_msgs/Quaternion constraint_region_orientation\n\
00351 \n\
00352 # Constraint weighting factor - a weight for this constraint\n\
00353 float64 weight\n\
00354 ================================================================================\n\
00355 MSG: motion_planning_msgs/OrientationConstraint\n\
00356 # This message contains the definition of an orientation constraint.\n\
00357 Header header\n\
00358 \n\
00359 # The robot link this constraint refers to\n\
00360 string link_name\n\
00361 \n\
00362 # The type of the constraint\n\
00363 int32 type\n\
00364 int32 LINK_FRAME=0\n\
00365 int32 HEADER_FRAME=1\n\
00366 \n\
00367 # The desired orientation of the robot link specified as a quaternion\n\
00368 geometry_msgs/Quaternion orientation\n\
00369 \n\
00370 # optional RPY error tolerances specified if \n\
00371 float64 absolute_roll_tolerance\n\
00372 float64 absolute_pitch_tolerance\n\
00373 float64 absolute_yaw_tolerance\n\
00374 \n\
00375 # Constraint weighting factor - a weight for this constraint\n\
00376 float64 weight\n\
00377 \n\
00378 ================================================================================\n\
00379 MSG: motion_planning_msgs/VisibilityConstraint\n\
00380 # This message contains the definition of a visibility constraint.\n\
00381 Header header\n\
00382 \n\
00383 # The point stamped target that needs to be kept within view of the sensor\n\
00384 geometry_msgs/PointStamped target\n\
00385 \n\
00386 # The local pose of the frame in which visibility is to be maintained\n\
00387 # The frame id should represent the robot link to which the sensor is attached\n\
00388 # The visual axis of the sensor is assumed to be along the X axis of this frame\n\
00389 geometry_msgs/PoseStamped sensor_pose\n\
00390 \n\
00391 # The deviation (in radians) that will be tolerated\n\
00392 # Constraint error will be measured as the solid angle between the \n\
00393 # X axis of the frame defined above and the vector between the origin \n\
00394 # of the frame defined above and the target location\n\
00395 float64 absolute_tolerance\n\
00396 \n\
00397 \n\
00398 ================================================================================\n\
00399 MSG: geometry_msgs/PointStamped\n\
00400 # This represents a Point with reference coordinate frame and timestamp\n\
00401 Header header\n\
00402 Point point\n\
00403 \n\
00404 ================================================================================\n\
00405 MSG: motion_planning_msgs/AllowedContactSpecification\n\
00406 # The names of the regions\n\
00407 string name\n\
00408 \n\
00409 # The shape of the region in the environment\n\
00410 geometric_shapes_msgs/Shape shape\n\
00411 \n\
00412 # The pose of the space defining the region\n\
00413 geometry_msgs/PoseStamped pose_stamped\n\
00414 \n\
00415 # The set of links that will be allowed to have penetration contact within this region\n\
00416 string[] link_names\n\
00417 \n\
00418 # The maximum penetration depth allowed for every link\n\
00419 float64 penetration_depth\n\
00420 ================================================================================\n\
00421 MSG: motion_planning_msgs/OrderedCollisionOperations\n\
00422 # A set of collision operations that will be performed in the order they are specified\n\
00423 CollisionOperation[] collision_operations\n\
00424 ================================================================================\n\
00425 MSG: motion_planning_msgs/CollisionOperation\n\
00426 # A definition of a collision operation\n\
00427 # E.g. (\"gripper\",COLLISION_SET_ALL,ENABLE) will enable collisions \n\
00428 # between the gripper and all objects in the collision space\n\
00429 \n\
00430 string object1\n\
00431 string object2\n\
00432 string COLLISION_SET_ALL=\"all\"\n\
00433 string COLLISION_SET_OBJECTS=\"objects\"\n\
00434 string COLLISION_SET_ATTACHED_OBJECTS=\"attached\"\n\
00435 \n\
00436 # The penetration distance to which collisions are allowed. This is 0.0 by default.\n\
00437 float64 penetration_distance\n\
00438 \n\
00439 # Flag that determines whether collisions will be enabled or disabled for the pair of objects specified above\n\
00440 int32 operation\n\
00441 int32 DISABLE=0\n\
00442 int32 ENABLE=1\n\
00443 \n\
00444 ================================================================================\n\
00445 MSG: motion_planning_msgs/LinkPadding\n\
00446 #name for the link\n\
00447 string link_name\n\
00448 \n\
00449 # padding to apply to the link\n\
00450 float64 padding\n\
00451 \n\
00452 "; }
00453 public:
00454 ROS_DEPRECATED static const std::string __s_getMessageDefinition() { return __s_getMessageDefinition_(); }
00455
00456 ROS_DEPRECATED const std::string __getMessageDefinition() const { return __s_getMessageDefinition_(); }
00457
00458 ROS_DEPRECATED virtual uint8_t *serialize(uint8_t *write_ptr, uint32_t seq) const
00459 {
00460 ros::serialization::OStream stream(write_ptr, 1000000000);
00461 ros::serialization::serialize(stream, workspace_parameters);
00462 ros::serialization::serialize(stream, start_state);
00463 ros::serialization::serialize(stream, goal_constraints);
00464 ros::serialization::serialize(stream, path_constraints);
00465 ros::serialization::serialize(stream, allowed_contacts);
00466 ros::serialization::serialize(stream, ordered_collision_operations);
00467 ros::serialization::serialize(stream, link_padding);
00468 ros::serialization::serialize(stream, planner_id);
00469 ros::serialization::serialize(stream, group_name);
00470 ros::serialization::serialize(stream, num_planning_attempts);
00471 ros::serialization::serialize(stream, allowed_planning_time);
00472 ros::serialization::serialize(stream, expected_path_duration);
00473 ros::serialization::serialize(stream, expected_path_dt);
00474 return stream.getData();
00475 }
00476
00477 ROS_DEPRECATED virtual uint8_t *deserialize(uint8_t *read_ptr)
00478 {
00479 ros::serialization::IStream stream(read_ptr, 1000000000);
00480 ros::serialization::deserialize(stream, workspace_parameters);
00481 ros::serialization::deserialize(stream, start_state);
00482 ros::serialization::deserialize(stream, goal_constraints);
00483 ros::serialization::deserialize(stream, path_constraints);
00484 ros::serialization::deserialize(stream, allowed_contacts);
00485 ros::serialization::deserialize(stream, ordered_collision_operations);
00486 ros::serialization::deserialize(stream, link_padding);
00487 ros::serialization::deserialize(stream, planner_id);
00488 ros::serialization::deserialize(stream, group_name);
00489 ros::serialization::deserialize(stream, num_planning_attempts);
00490 ros::serialization::deserialize(stream, allowed_planning_time);
00491 ros::serialization::deserialize(stream, expected_path_duration);
00492 ros::serialization::deserialize(stream, expected_path_dt);
00493 return stream.getData();
00494 }
00495
00496 ROS_DEPRECATED virtual uint32_t serializationLength() const
00497 {
00498 uint32_t size = 0;
00499 size += ros::serialization::serializationLength(workspace_parameters);
00500 size += ros::serialization::serializationLength(start_state);
00501 size += ros::serialization::serializationLength(goal_constraints);
00502 size += ros::serialization::serializationLength(path_constraints);
00503 size += ros::serialization::serializationLength(allowed_contacts);
00504 size += ros::serialization::serializationLength(ordered_collision_operations);
00505 size += ros::serialization::serializationLength(link_padding);
00506 size += ros::serialization::serializationLength(planner_id);
00507 size += ros::serialization::serializationLength(group_name);
00508 size += ros::serialization::serializationLength(num_planning_attempts);
00509 size += ros::serialization::serializationLength(allowed_planning_time);
00510 size += ros::serialization::serializationLength(expected_path_duration);
00511 size += ros::serialization::serializationLength(expected_path_dt);
00512 return size;
00513 }
00514
00515 typedef boost::shared_ptr< ::motion_planning_msgs::MotionPlanRequest_<ContainerAllocator> > Ptr;
00516 typedef boost::shared_ptr< ::motion_planning_msgs::MotionPlanRequest_<ContainerAllocator> const> ConstPtr;
00517 };
00518 typedef ::motion_planning_msgs::MotionPlanRequest_<std::allocator<void> > MotionPlanRequest;
00519
00520 typedef boost::shared_ptr< ::motion_planning_msgs::MotionPlanRequest> MotionPlanRequestPtr;
00521 typedef boost::shared_ptr< ::motion_planning_msgs::MotionPlanRequest const> MotionPlanRequestConstPtr;
00522
00523
00524 template<typename ContainerAllocator>
00525 std::ostream& operator<<(std::ostream& s, const ::motion_planning_msgs::MotionPlanRequest_<ContainerAllocator> & v)
00526 {
00527 ros::message_operations::Printer< ::motion_planning_msgs::MotionPlanRequest_<ContainerAllocator> >::stream(s, "", v);
00528 return s;}
00529
00530 }
00531
00532 namespace ros
00533 {
00534 namespace message_traits
00535 {
00536 template<class ContainerAllocator>
00537 struct MD5Sum< ::motion_planning_msgs::MotionPlanRequest_<ContainerAllocator> > {
00538 static const char* value()
00539 {
00540 return "d7f3952531fa0e5f7133e2875517c71d";
00541 }
00542
00543 static const char* value(const ::motion_planning_msgs::MotionPlanRequest_<ContainerAllocator> &) { return value(); }
00544 static const uint64_t static_value1 = 0xd7f3952531fa0e5fULL;
00545 static const uint64_t static_value2 = 0x7133e2875517c71dULL;
00546 };
00547
00548 template<class ContainerAllocator>
00549 struct DataType< ::motion_planning_msgs::MotionPlanRequest_<ContainerAllocator> > {
00550 static const char* value()
00551 {
00552 return "motion_planning_msgs/MotionPlanRequest";
00553 }
00554
00555 static const char* value(const ::motion_planning_msgs::MotionPlanRequest_<ContainerAllocator> &) { return value(); }
00556 };
00557
00558 template<class ContainerAllocator>
00559 struct Definition< ::motion_planning_msgs::MotionPlanRequest_<ContainerAllocator> > {
00560 static const char* value()
00561 {
00562 return "# This service contains the definition for a request to the motion\n\
00563 # planner and the output it provides\n\
00564 \n\
00565 # Parameters for the workspace that the planner should work inside\n\
00566 motion_planning_msgs/WorkspaceParameters workspace_parameters\n\
00567 \n\
00568 # Starting state updates. If certain joints should be considered\n\
00569 # at positions other than the current ones, these positions should\n\
00570 # be set here\n\
00571 motion_planning_msgs/RobotState start_state\n\
00572 \n\
00573 # The goal state for the model to plan for. The goal is achieved\n\
00574 # if all constraints are satisfied\n\
00575 motion_planning_msgs/Constraints goal_constraints\n\
00576 \n\
00577 # No state at any point along the path in the produced motion plan will violate these constraints\n\
00578 motion_planning_msgs/Constraints path_constraints\n\
00579 \n\
00580 # A specification for regions where contact is \n\
00581 # allowed up to a certain depth\n\
00582 # Any collision within this set of regions with a link \n\
00583 # specified in the message will be allowed if\n\
00584 # it is less than the penetration depth specified in the message\n\
00585 AllowedContactSpecification[] allowed_contacts\n\
00586 \n\
00587 # A set of ordered collision operations, \n\
00588 # these are applied to all links, objects, \n\
00589 # namespaces in the collision space\n\
00590 OrderedCollisionOperations ordered_collision_operations\n\
00591 \n\
00592 # Specifies a set of links and paddings to change from the default\n\
00593 # specified in the yaml file\n\
00594 motion_planning_msgs/LinkPadding[] link_padding\n\
00595 \n\
00596 # The name of the motion planner to use. If no name is specified,\n\
00597 # a default motion planner will be used\n\
00598 string planner_id\n\
00599 \n\
00600 # The name of the group of joints on which this planner is operating\n\
00601 string group_name\n\
00602 \n\
00603 # The number of times this plan is to be computed. Shortest solution\n\
00604 # will be reported.\n\
00605 int32 num_planning_attempts\n\
00606 \n\
00607 # The maximum amount of time the motion planner is allowed to plan for\n\
00608 duration allowed_planning_time\n\
00609 \n\
00610 # An expected path duration (in seconds) along with an expected discretization of the path allows the planner to determine the discretization of the trajectory that it returns\n\
00611 duration expected_path_duration\n\
00612 duration expected_path_dt\n\
00613 \n\
00614 ================================================================================\n\
00615 MSG: motion_planning_msgs/WorkspaceParameters\n\
00616 # This message contains a set of parameters useful in\n\
00617 # setting up the workspace for planning\n\
00618 geometric_shapes_msgs/Shape workspace_region_shape\n\
00619 geometry_msgs/PoseStamped workspace_region_pose\n\
00620 \n\
00621 \n\
00622 ================================================================================\n\
00623 MSG: geometric_shapes_msgs/Shape\n\
00624 byte SPHERE=0\n\
00625 byte BOX=1\n\
00626 byte CYLINDER=2\n\
00627 byte MESH=3\n\
00628 \n\
00629 byte type\n\
00630 \n\
00631 \n\
00632 #### define sphere, box, cylinder ####\n\
00633 # the origin of each shape is considered at the shape's center\n\
00634 \n\
00635 # for sphere\n\
00636 # radius := dimensions[0]\n\
00637 \n\
00638 # for cylinder\n\
00639 # radius := dimensions[0]\n\
00640 # length := dimensions[1]\n\
00641 # the length is along the Z axis\n\
00642 \n\
00643 # for box\n\
00644 # size_x := dimensions[0]\n\
00645 # size_y := dimensions[1]\n\
00646 # size_z := dimensions[2]\n\
00647 float64[] dimensions\n\
00648 \n\
00649 \n\
00650 #### define mesh ####\n\
00651 \n\
00652 # list of triangles; triangle k is defined by tre vertices located\n\
00653 # at indices triangles[3k], triangles[3k+1], triangles[3k+2]\n\
00654 int32[] triangles\n\
00655 geometry_msgs/Point[] vertices\n\
00656 \n\
00657 ================================================================================\n\
00658 MSG: geometry_msgs/Point\n\
00659 # This contains the position of a point in free space\n\
00660 float64 x\n\
00661 float64 y\n\
00662 float64 z\n\
00663 \n\
00664 ================================================================================\n\
00665 MSG: geometry_msgs/PoseStamped\n\
00666 # A Pose with reference coordinate frame and timestamp\n\
00667 Header header\n\
00668 Pose pose\n\
00669 \n\
00670 ================================================================================\n\
00671 MSG: std_msgs/Header\n\
00672 # Standard metadata for higher-level stamped data types.\n\
00673 # This is generally used to communicate timestamped data \n\
00674 # in a particular coordinate frame.\n\
00675 # \n\
00676 # sequence ID: consecutively increasing ID \n\
00677 uint32 seq\n\
00678 #Two-integer timestamp that is expressed as:\n\
00679 # * stamp.secs: seconds (stamp_secs) since epoch\n\
00680 # * stamp.nsecs: nanoseconds since stamp_secs\n\
00681 # time-handling sugar is provided by the client library\n\
00682 time stamp\n\
00683 #Frame this data is associated with\n\
00684 # 0: no frame\n\
00685 # 1: global frame\n\
00686 string frame_id\n\
00687 \n\
00688 ================================================================================\n\
00689 MSG: geometry_msgs/Pose\n\
00690 # A representation of pose in free space, composed of postion and orientation. \n\
00691 Point position\n\
00692 Quaternion orientation\n\
00693 \n\
00694 ================================================================================\n\
00695 MSG: geometry_msgs/Quaternion\n\
00696 # This represents an orientation in free space in quaternion form.\n\
00697 \n\
00698 float64 x\n\
00699 float64 y\n\
00700 float64 z\n\
00701 float64 w\n\
00702 \n\
00703 ================================================================================\n\
00704 MSG: motion_planning_msgs/RobotState\n\
00705 # This message contains information about the robot state, i.e. the positions of its joints and links\n\
00706 sensor_msgs/JointState joint_state\n\
00707 motion_planning_msgs/MultiDOFJointState multi_dof_joint_state\n\
00708 ================================================================================\n\
00709 MSG: sensor_msgs/JointState\n\
00710 # This is a message that holds data to describe the state of a set of torque controlled joints. \n\
00711 #\n\
00712 # The state of each joint (revolute or prismatic) is defined by:\n\
00713 # * the position of the joint (rad or m),\n\
00714 # * the velocity of the joint (rad/s or m/s) and \n\
00715 # * the effort that is applied in the joint (Nm or N).\n\
00716 #\n\
00717 # Each joint is uniquely identified by its name\n\
00718 # The header specifies the time at which the joint states were recorded. All the joint states\n\
00719 # in one message have to be recorded at the same time.\n\
00720 #\n\
00721 # This message consists of a multiple arrays, one for each part of the joint state. \n\
00722 # The goal is to make each of the fields optional. When e.g. your joints have no\n\
00723 # effort associated with them, you can leave the effort array empty. \n\
00724 #\n\
00725 # All arrays in this message should have the same size, or be empty.\n\
00726 # This is the only way to uniquely associate the joint name with the correct\n\
00727 # states.\n\
00728 \n\
00729 \n\
00730 Header header\n\
00731 \n\
00732 string[] name\n\
00733 float64[] position\n\
00734 float64[] velocity\n\
00735 float64[] effort\n\
00736 \n\
00737 ================================================================================\n\
00738 MSG: motion_planning_msgs/MultiDOFJointState\n\
00739 #A representation of a multi-dof joint state\n\
00740 time stamp\n\
00741 string[] joint_names\n\
00742 string[] frame_ids\n\
00743 string[] child_frame_ids\n\
00744 geometry_msgs/Pose[] poses\n\
00745 \n\
00746 ================================================================================\n\
00747 MSG: motion_planning_msgs/Constraints\n\
00748 # This message contains a list of motion planning constraints.\n\
00749 \n\
00750 motion_planning_msgs/JointConstraint[] joint_constraints\n\
00751 motion_planning_msgs/PositionConstraint[] position_constraints\n\
00752 motion_planning_msgs/OrientationConstraint[] orientation_constraints\n\
00753 motion_planning_msgs/VisibilityConstraint[] visibility_constraints\n\
00754 \n\
00755 ================================================================================\n\
00756 MSG: motion_planning_msgs/JointConstraint\n\
00757 # Constrain the position of a joint to be within a certain bound\n\
00758 string joint_name\n\
00759 \n\
00760 # the bound to be achieved is [position - tolerance_below, position + tolerance_above]\n\
00761 float64 position\n\
00762 float64 tolerance_above\n\
00763 float64 tolerance_below\n\
00764 \n\
00765 # A weighting factor for this constraint\n\
00766 float64 weight\n\
00767 ================================================================================\n\
00768 MSG: motion_planning_msgs/PositionConstraint\n\
00769 # This message contains the definition of a position constraint.\n\
00770 Header header\n\
00771 \n\
00772 # The robot link this constraint refers to\n\
00773 string link_name\n\
00774 \n\
00775 # The offset (in the link frame) for the target point on the link we are planning for\n\
00776 geometry_msgs/Point target_point_offset\n\
00777 \n\
00778 # The nominal/target position for the point we are planning for\n\
00779 geometry_msgs/Point position\n\
00780 \n\
00781 # The shape of the bounded region that constrains the position of the end-effector\n\
00782 # This region is always centered at the position defined above\n\
00783 geometric_shapes_msgs/Shape constraint_region_shape\n\
00784 \n\
00785 # The orientation of the bounded region that constrains the position of the end-effector. \n\
00786 # This allows the specification of non-axis aligned constraints\n\
00787 geometry_msgs/Quaternion constraint_region_orientation\n\
00788 \n\
00789 # Constraint weighting factor - a weight for this constraint\n\
00790 float64 weight\n\
00791 ================================================================================\n\
00792 MSG: motion_planning_msgs/OrientationConstraint\n\
00793 # This message contains the definition of an orientation constraint.\n\
00794 Header header\n\
00795 \n\
00796 # The robot link this constraint refers to\n\
00797 string link_name\n\
00798 \n\
00799 # The type of the constraint\n\
00800 int32 type\n\
00801 int32 LINK_FRAME=0\n\
00802 int32 HEADER_FRAME=1\n\
00803 \n\
00804 # The desired orientation of the robot link specified as a quaternion\n\
00805 geometry_msgs/Quaternion orientation\n\
00806 \n\
00807 # optional RPY error tolerances specified if \n\
00808 float64 absolute_roll_tolerance\n\
00809 float64 absolute_pitch_tolerance\n\
00810 float64 absolute_yaw_tolerance\n\
00811 \n\
00812 # Constraint weighting factor - a weight for this constraint\n\
00813 float64 weight\n\
00814 \n\
00815 ================================================================================\n\
00816 MSG: motion_planning_msgs/VisibilityConstraint\n\
00817 # This message contains the definition of a visibility constraint.\n\
00818 Header header\n\
00819 \n\
00820 # The point stamped target that needs to be kept within view of the sensor\n\
00821 geometry_msgs/PointStamped target\n\
00822 \n\
00823 # The local pose of the frame in which visibility is to be maintained\n\
00824 # The frame id should represent the robot link to which the sensor is attached\n\
00825 # The visual axis of the sensor is assumed to be along the X axis of this frame\n\
00826 geometry_msgs/PoseStamped sensor_pose\n\
00827 \n\
00828 # The deviation (in radians) that will be tolerated\n\
00829 # Constraint error will be measured as the solid angle between the \n\
00830 # X axis of the frame defined above and the vector between the origin \n\
00831 # of the frame defined above and the target location\n\
00832 float64 absolute_tolerance\n\
00833 \n\
00834 \n\
00835 ================================================================================\n\
00836 MSG: geometry_msgs/PointStamped\n\
00837 # This represents a Point with reference coordinate frame and timestamp\n\
00838 Header header\n\
00839 Point point\n\
00840 \n\
00841 ================================================================================\n\
00842 MSG: motion_planning_msgs/AllowedContactSpecification\n\
00843 # The names of the regions\n\
00844 string name\n\
00845 \n\
00846 # The shape of the region in the environment\n\
00847 geometric_shapes_msgs/Shape shape\n\
00848 \n\
00849 # The pose of the space defining the region\n\
00850 geometry_msgs/PoseStamped pose_stamped\n\
00851 \n\
00852 # The set of links that will be allowed to have penetration contact within this region\n\
00853 string[] link_names\n\
00854 \n\
00855 # The maximum penetration depth allowed for every link\n\
00856 float64 penetration_depth\n\
00857 ================================================================================\n\
00858 MSG: motion_planning_msgs/OrderedCollisionOperations\n\
00859 # A set of collision operations that will be performed in the order they are specified\n\
00860 CollisionOperation[] collision_operations\n\
00861 ================================================================================\n\
00862 MSG: motion_planning_msgs/CollisionOperation\n\
00863 # A definition of a collision operation\n\
00864 # E.g. (\"gripper\",COLLISION_SET_ALL,ENABLE) will enable collisions \n\
00865 # between the gripper and all objects in the collision space\n\
00866 \n\
00867 string object1\n\
00868 string object2\n\
00869 string COLLISION_SET_ALL=\"all\"\n\
00870 string COLLISION_SET_OBJECTS=\"objects\"\n\
00871 string COLLISION_SET_ATTACHED_OBJECTS=\"attached\"\n\
00872 \n\
00873 # The penetration distance to which collisions are allowed. This is 0.0 by default.\n\
00874 float64 penetration_distance\n\
00875 \n\
00876 # Flag that determines whether collisions will be enabled or disabled for the pair of objects specified above\n\
00877 int32 operation\n\
00878 int32 DISABLE=0\n\
00879 int32 ENABLE=1\n\
00880 \n\
00881 ================================================================================\n\
00882 MSG: motion_planning_msgs/LinkPadding\n\
00883 #name for the link\n\
00884 string link_name\n\
00885 \n\
00886 # padding to apply to the link\n\
00887 float64 padding\n\
00888 \n\
00889 ";
00890 }
00891
00892 static const char* value(const ::motion_planning_msgs::MotionPlanRequest_<ContainerAllocator> &) { return value(); }
00893 };
00894
00895 }
00896 }
00897
00898 namespace ros
00899 {
00900 namespace serialization
00901 {
00902
00903 template<class ContainerAllocator> struct Serializer< ::motion_planning_msgs::MotionPlanRequest_<ContainerAllocator> >
00904 {
00905 template<typename Stream, typename T> inline static void allInOne(Stream& stream, T m)
00906 {
00907 stream.next(m.workspace_parameters);
00908 stream.next(m.start_state);
00909 stream.next(m.goal_constraints);
00910 stream.next(m.path_constraints);
00911 stream.next(m.allowed_contacts);
00912 stream.next(m.ordered_collision_operations);
00913 stream.next(m.link_padding);
00914 stream.next(m.planner_id);
00915 stream.next(m.group_name);
00916 stream.next(m.num_planning_attempts);
00917 stream.next(m.allowed_planning_time);
00918 stream.next(m.expected_path_duration);
00919 stream.next(m.expected_path_dt);
00920 }
00921
00922 ROS_DECLARE_ALLINONE_SERIALIZER;
00923 };
00924 }
00925 }
00926
00927 namespace ros
00928 {
00929 namespace message_operations
00930 {
00931
00932 template<class ContainerAllocator>
00933 struct Printer< ::motion_planning_msgs::MotionPlanRequest_<ContainerAllocator> >
00934 {
00935 template<typename Stream> static void stream(Stream& s, const std::string& indent, const ::motion_planning_msgs::MotionPlanRequest_<ContainerAllocator> & v)
00936 {
00937 s << indent << "workspace_parameters: ";
00938 s << std::endl;
00939 Printer< ::motion_planning_msgs::WorkspaceParameters_<ContainerAllocator> >::stream(s, indent + " ", v.workspace_parameters);
00940 s << indent << "start_state: ";
00941 s << std::endl;
00942 Printer< ::motion_planning_msgs::RobotState_<ContainerAllocator> >::stream(s, indent + " ", v.start_state);
00943 s << indent << "goal_constraints: ";
00944 s << std::endl;
00945 Printer< ::motion_planning_msgs::Constraints_<ContainerAllocator> >::stream(s, indent + " ", v.goal_constraints);
00946 s << indent << "path_constraints: ";
00947 s << std::endl;
00948 Printer< ::motion_planning_msgs::Constraints_<ContainerAllocator> >::stream(s, indent + " ", v.path_constraints);
00949 s << indent << "allowed_contacts[]" << std::endl;
00950 for (size_t i = 0; i < v.allowed_contacts.size(); ++i)
00951 {
00952 s << indent << " allowed_contacts[" << i << "]: ";
00953 s << std::endl;
00954 s << indent;
00955 Printer< ::motion_planning_msgs::AllowedContactSpecification_<ContainerAllocator> >::stream(s, indent + " ", v.allowed_contacts[i]);
00956 }
00957 s << indent << "ordered_collision_operations: ";
00958 s << std::endl;
00959 Printer< ::motion_planning_msgs::OrderedCollisionOperations_<ContainerAllocator> >::stream(s, indent + " ", v.ordered_collision_operations);
00960 s << indent << "link_padding[]" << std::endl;
00961 for (size_t i = 0; i < v.link_padding.size(); ++i)
00962 {
00963 s << indent << " link_padding[" << i << "]: ";
00964 s << std::endl;
00965 s << indent;
00966 Printer< ::motion_planning_msgs::LinkPadding_<ContainerAllocator> >::stream(s, indent + " ", v.link_padding[i]);
00967 }
00968 s << indent << "planner_id: ";
00969 Printer<std::basic_string<char, std::char_traits<char>, typename ContainerAllocator::template rebind<char>::other > >::stream(s, indent + " ", v.planner_id);
00970 s << indent << "group_name: ";
00971 Printer<std::basic_string<char, std::char_traits<char>, typename ContainerAllocator::template rebind<char>::other > >::stream(s, indent + " ", v.group_name);
00972 s << indent << "num_planning_attempts: ";
00973 Printer<int32_t>::stream(s, indent + " ", v.num_planning_attempts);
00974 s << indent << "allowed_planning_time: ";
00975 Printer<ros::Duration>::stream(s, indent + " ", v.allowed_planning_time);
00976 s << indent << "expected_path_duration: ";
00977 Printer<ros::Duration>::stream(s, indent + " ", v.expected_path_duration);
00978 s << indent << "expected_path_dt: ";
00979 Printer<ros::Duration>::stream(s, indent + " ", v.expected_path_dt);
00980 }
00981 };
00982
00983
00984 }
00985 }
00986
00987 #endif // MOTION_PLANNING_MSGS_MESSAGE_MOTIONPLANREQUEST_H
00988