Go to the documentation of this file.00001
00002 #ifndef KDL_ARM_KINEMATICS_SERVICE_GETWEIGHTEDIK_H
00003 #define KDL_ARM_KINEMATICS_SERVICE_GETWEIGHTEDIK_H
00004 #include <string>
00005 #include <vector>
00006 #include <map>
00007 #include <ostream>
00008 #include "ros/serialization.h"
00009 #include "ros/builtin_message_traits.h"
00010 #include "ros/message_operations.h"
00011 #include "ros/time.h"
00012
00013 #include "ros/macros.h"
00014
00015 #include "ros/assert.h"
00016
00017 #include "ros/service_traits.h"
00018
00019 #include "geometry_msgs/PoseStamped.h"
00020 #include "geometry_msgs/Pose.h"
00021 #include "sensor_msgs/JointState.h"
00022 #include "kdl_arm_kinematics/KDLWeights.h"
00023
00024
00025 #include "sensor_msgs/JointState.h"
00026 #include "arm_navigation_msgs/ArmNavigationErrorCodes.h"
00027
00028 namespace kdl_arm_kinematics
00029 {
00030 template <class ContainerAllocator>
00031 struct GetWeightedIKRequest_ {
00032 typedef GetWeightedIKRequest_<ContainerAllocator> Type;
00033
00034 GetWeightedIKRequest_()
00035 : pose()
00036 , tool_frame()
00037 , ik_seed()
00038 , weights()
00039 {
00040 }
00041
00042 GetWeightedIKRequest_(const ContainerAllocator& _alloc)
00043 : pose(_alloc)
00044 , tool_frame(_alloc)
00045 , ik_seed(_alloc)
00046 , weights(_alloc)
00047 {
00048 }
00049
00050 typedef ::geometry_msgs::PoseStamped_<ContainerAllocator> _pose_type;
00051 ::geometry_msgs::PoseStamped_<ContainerAllocator> pose;
00052
00053 typedef ::geometry_msgs::Pose_<ContainerAllocator> _tool_frame_type;
00054 ::geometry_msgs::Pose_<ContainerAllocator> tool_frame;
00055
00056 typedef ::sensor_msgs::JointState_<ContainerAllocator> _ik_seed_type;
00057 ::sensor_msgs::JointState_<ContainerAllocator> ik_seed;
00058
00059 typedef ::kdl_arm_kinematics::KDLWeights_<ContainerAllocator> _weights_type;
00060 ::kdl_arm_kinematics::KDLWeights_<ContainerAllocator> weights;
00061
00062
00063 typedef boost::shared_ptr< ::kdl_arm_kinematics::GetWeightedIKRequest_<ContainerAllocator> > Ptr;
00064 typedef boost::shared_ptr< ::kdl_arm_kinematics::GetWeightedIKRequest_<ContainerAllocator> const> ConstPtr;
00065 boost::shared_ptr<std::map<std::string, std::string> > __connection_header;
00066 };
00067 typedef ::kdl_arm_kinematics::GetWeightedIKRequest_<std::allocator<void> > GetWeightedIKRequest;
00068
00069 typedef boost::shared_ptr< ::kdl_arm_kinematics::GetWeightedIKRequest> GetWeightedIKRequestPtr;
00070 typedef boost::shared_ptr< ::kdl_arm_kinematics::GetWeightedIKRequest const> GetWeightedIKRequestConstPtr;
00071
00072
00073 template <class ContainerAllocator>
00074 struct GetWeightedIKResponse_ {
00075 typedef GetWeightedIKResponse_<ContainerAllocator> Type;
00076
00077 GetWeightedIKResponse_()
00078 : solution()
00079 , error_code()
00080 {
00081 }
00082
00083 GetWeightedIKResponse_(const ContainerAllocator& _alloc)
00084 : solution(_alloc)
00085 , error_code(_alloc)
00086 {
00087 }
00088
00089 typedef ::sensor_msgs::JointState_<ContainerAllocator> _solution_type;
00090 ::sensor_msgs::JointState_<ContainerAllocator> solution;
00091
00092 typedef ::arm_navigation_msgs::ArmNavigationErrorCodes_<ContainerAllocator> _error_code_type;
00093 ::arm_navigation_msgs::ArmNavigationErrorCodes_<ContainerAllocator> error_code;
00094
00095
00096 typedef boost::shared_ptr< ::kdl_arm_kinematics::GetWeightedIKResponse_<ContainerAllocator> > Ptr;
00097 typedef boost::shared_ptr< ::kdl_arm_kinematics::GetWeightedIKResponse_<ContainerAllocator> const> ConstPtr;
00098 boost::shared_ptr<std::map<std::string, std::string> > __connection_header;
00099 };
00100 typedef ::kdl_arm_kinematics::GetWeightedIKResponse_<std::allocator<void> > GetWeightedIKResponse;
00101
00102 typedef boost::shared_ptr< ::kdl_arm_kinematics::GetWeightedIKResponse> GetWeightedIKResponsePtr;
00103 typedef boost::shared_ptr< ::kdl_arm_kinematics::GetWeightedIKResponse const> GetWeightedIKResponseConstPtr;
00104
00105 struct GetWeightedIK
00106 {
00107
00108 typedef GetWeightedIKRequest Request;
00109 typedef GetWeightedIKResponse Response;
00110 Request request;
00111 Response response;
00112
00113 typedef Request RequestType;
00114 typedef Response ResponseType;
00115 };
00116 }
00117
00118 namespace ros
00119 {
00120 namespace message_traits
00121 {
00122 template<class ContainerAllocator> struct IsMessage< ::kdl_arm_kinematics::GetWeightedIKRequest_<ContainerAllocator> > : public TrueType {};
00123 template<class ContainerAllocator> struct IsMessage< ::kdl_arm_kinematics::GetWeightedIKRequest_<ContainerAllocator> const> : public TrueType {};
00124 template<class ContainerAllocator>
00125 struct MD5Sum< ::kdl_arm_kinematics::GetWeightedIKRequest_<ContainerAllocator> > {
00126 static const char* value()
00127 {
00128 return "53b781873d4bc97a81274c9247a551d9";
00129 }
00130
00131 static const char* value(const ::kdl_arm_kinematics::GetWeightedIKRequest_<ContainerAllocator> &) { return value(); }
00132 static const uint64_t static_value1 = 0x53b781873d4bc97aULL;
00133 static const uint64_t static_value2 = 0x81274c9247a551d9ULL;
00134 };
00135
00136 template<class ContainerAllocator>
00137 struct DataType< ::kdl_arm_kinematics::GetWeightedIKRequest_<ContainerAllocator> > {
00138 static const char* value()
00139 {
00140 return "kdl_arm_kinematics/GetWeightedIKRequest";
00141 }
00142
00143 static const char* value(const ::kdl_arm_kinematics::GetWeightedIKRequest_<ContainerAllocator> &) { return value(); }
00144 };
00145
00146 template<class ContainerAllocator>
00147 struct Definition< ::kdl_arm_kinematics::GetWeightedIKRequest_<ContainerAllocator> > {
00148 static const char* value()
00149 {
00150 return "\n\
00151 geometry_msgs/PoseStamped pose\n\
00152 \n\
00153 \n\
00154 geometry_msgs/Pose tool_frame\n\
00155 \n\
00156 \n\
00157 sensor_msgs/JointState ik_seed\n\
00158 \n\
00159 \n\
00160 kdl_arm_kinematics/KDLWeights weights\n\
00161 \n\
00162 \n\
00163 ================================================================================\n\
00164 MSG: geometry_msgs/PoseStamped\n\
00165 # A Pose with reference coordinate frame and timestamp\n\
00166 Header header\n\
00167 Pose pose\n\
00168 \n\
00169 ================================================================================\n\
00170 MSG: std_msgs/Header\n\
00171 # Standard metadata for higher-level stamped data types.\n\
00172 # This is generally used to communicate timestamped data \n\
00173 # in a particular coordinate frame.\n\
00174 # \n\
00175 # sequence ID: consecutively increasing ID \n\
00176 uint32 seq\n\
00177 #Two-integer timestamp that is expressed as:\n\
00178 # * stamp.secs: seconds (stamp_secs) since epoch\n\
00179 # * stamp.nsecs: nanoseconds since stamp_secs\n\
00180 # time-handling sugar is provided by the client library\n\
00181 time stamp\n\
00182 #Frame this data is associated with\n\
00183 # 0: no frame\n\
00184 # 1: global frame\n\
00185 string frame_id\n\
00186 \n\
00187 ================================================================================\n\
00188 MSG: geometry_msgs/Pose\n\
00189 # A representation of pose in free space, composed of postion and orientation. \n\
00190 Point position\n\
00191 Quaternion orientation\n\
00192 \n\
00193 ================================================================================\n\
00194 MSG: geometry_msgs/Point\n\
00195 # This contains the position of a point in free space\n\
00196 float64 x\n\
00197 float64 y\n\
00198 float64 z\n\
00199 \n\
00200 ================================================================================\n\
00201 MSG: geometry_msgs/Quaternion\n\
00202 # This represents an orientation in free space in quaternion form.\n\
00203 \n\
00204 float64 x\n\
00205 float64 y\n\
00206 float64 z\n\
00207 float64 w\n\
00208 \n\
00209 ================================================================================\n\
00210 MSG: sensor_msgs/JointState\n\
00211 # This is a message that holds data to describe the state of a set of torque controlled joints. \n\
00212 #\n\
00213 # The state of each joint (revolute or prismatic) is defined by:\n\
00214 # * the position of the joint (rad or m),\n\
00215 # * the velocity of the joint (rad/s or m/s) and \n\
00216 # * the effort that is applied in the joint (Nm or N).\n\
00217 #\n\
00218 # Each joint is uniquely identified by its name\n\
00219 # The header specifies the time at which the joint states were recorded. All the joint states\n\
00220 # in one message have to be recorded at the same time.\n\
00221 #\n\
00222 # This message consists of a multiple arrays, one for each part of the joint state. \n\
00223 # The goal is to make each of the fields optional. When e.g. your joints have no\n\
00224 # effort associated with them, you can leave the effort array empty. \n\
00225 #\n\
00226 # All arrays in this message should have the same size, or be empty.\n\
00227 # This is the only way to uniquely associate the joint name with the correct\n\
00228 # states.\n\
00229 \n\
00230 \n\
00231 Header header\n\
00232 \n\
00233 string[] name\n\
00234 float64[] position\n\
00235 float64[] velocity\n\
00236 float64[] effort\n\
00237 \n\
00238 ================================================================================\n\
00239 MSG: kdl_arm_kinematics/KDLWeights\n\
00240 byte INVALID_MODE = 0\n\
00241 byte SET_TS = 1\n\
00242 byte SET_JS = 2\n\
00243 byte SET_LAMBDA = 4\n\
00244 byte SET_TS_JS = 3\n\
00245 \n\
00246 byte mode # or-combination of values to set\n\
00247 float64[36] weight_ts # the 6x6 matrix of task space weights in row-major order\n\
00248 float64[] weight_js # the joints x joints matrix of joint space weights in row-major order\n\
00249 float64 lambda # Lambda value\n\
00250 \n\
00251 ";
00252 }
00253
00254 static const char* value(const ::kdl_arm_kinematics::GetWeightedIKRequest_<ContainerAllocator> &) { return value(); }
00255 };
00256
00257 }
00258 }
00259
00260
00261 namespace ros
00262 {
00263 namespace message_traits
00264 {
00265 template<class ContainerAllocator> struct IsMessage< ::kdl_arm_kinematics::GetWeightedIKResponse_<ContainerAllocator> > : public TrueType {};
00266 template<class ContainerAllocator> struct IsMessage< ::kdl_arm_kinematics::GetWeightedIKResponse_<ContainerAllocator> const> : public TrueType {};
00267 template<class ContainerAllocator>
00268 struct MD5Sum< ::kdl_arm_kinematics::GetWeightedIKResponse_<ContainerAllocator> > {
00269 static const char* value()
00270 {
00271 return "26703d3aa2f94a5367558f4749423a09";
00272 }
00273
00274 static const char* value(const ::kdl_arm_kinematics::GetWeightedIKResponse_<ContainerAllocator> &) { return value(); }
00275 static const uint64_t static_value1 = 0x26703d3aa2f94a53ULL;
00276 static const uint64_t static_value2 = 0x67558f4749423a09ULL;
00277 };
00278
00279 template<class ContainerAllocator>
00280 struct DataType< ::kdl_arm_kinematics::GetWeightedIKResponse_<ContainerAllocator> > {
00281 static const char* value()
00282 {
00283 return "kdl_arm_kinematics/GetWeightedIKResponse";
00284 }
00285
00286 static const char* value(const ::kdl_arm_kinematics::GetWeightedIKResponse_<ContainerAllocator> &) { return value(); }
00287 };
00288
00289 template<class ContainerAllocator>
00290 struct Definition< ::kdl_arm_kinematics::GetWeightedIKResponse_<ContainerAllocator> > {
00291 static const char* value()
00292 {
00293 return "sensor_msgs/JointState solution\n\
00294 \n\
00295 arm_navigation_msgs/ArmNavigationErrorCodes error_code\n\
00296 \n\
00297 \n\
00298 ================================================================================\n\
00299 MSG: sensor_msgs/JointState\n\
00300 # This is a message that holds data to describe the state of a set of torque controlled joints. \n\
00301 #\n\
00302 # The state of each joint (revolute or prismatic) is defined by:\n\
00303 # * the position of the joint (rad or m),\n\
00304 # * the velocity of the joint (rad/s or m/s) and \n\
00305 # * the effort that is applied in the joint (Nm or N).\n\
00306 #\n\
00307 # Each joint is uniquely identified by its name\n\
00308 # The header specifies the time at which the joint states were recorded. All the joint states\n\
00309 # in one message have to be recorded at the same time.\n\
00310 #\n\
00311 # This message consists of a multiple arrays, one for each part of the joint state. \n\
00312 # The goal is to make each of the fields optional. When e.g. your joints have no\n\
00313 # effort associated with them, you can leave the effort array empty. \n\
00314 #\n\
00315 # All arrays in this message should have the same size, or be empty.\n\
00316 # This is the only way to uniquely associate the joint name with the correct\n\
00317 # states.\n\
00318 \n\
00319 \n\
00320 Header header\n\
00321 \n\
00322 string[] name\n\
00323 float64[] position\n\
00324 float64[] velocity\n\
00325 float64[] effort\n\
00326 \n\
00327 ================================================================================\n\
00328 MSG: std_msgs/Header\n\
00329 # Standard metadata for higher-level stamped data types.\n\
00330 # This is generally used to communicate timestamped data \n\
00331 # in a particular coordinate frame.\n\
00332 # \n\
00333 # sequence ID: consecutively increasing ID \n\
00334 uint32 seq\n\
00335 #Two-integer timestamp that is expressed as:\n\
00336 # * stamp.secs: seconds (stamp_secs) since epoch\n\
00337 # * stamp.nsecs: nanoseconds since stamp_secs\n\
00338 # time-handling sugar is provided by the client library\n\
00339 time stamp\n\
00340 #Frame this data is associated with\n\
00341 # 0: no frame\n\
00342 # 1: global frame\n\
00343 string frame_id\n\
00344 \n\
00345 ================================================================================\n\
00346 MSG: arm_navigation_msgs/ArmNavigationErrorCodes\n\
00347 int32 val\n\
00348 \n\
00349 # overall behavior\n\
00350 int32 PLANNING_FAILED=-1\n\
00351 int32 SUCCESS=1\n\
00352 int32 TIMED_OUT=-2\n\
00353 \n\
00354 # start state errors\n\
00355 int32 START_STATE_IN_COLLISION=-3\n\
00356 int32 START_STATE_VIOLATES_PATH_CONSTRAINTS=-4\n\
00357 \n\
00358 # goal errors\n\
00359 int32 GOAL_IN_COLLISION=-5\n\
00360 int32 GOAL_VIOLATES_PATH_CONSTRAINTS=-6\n\
00361 \n\
00362 # robot state\n\
00363 int32 INVALID_ROBOT_STATE=-7\n\
00364 int32 INCOMPLETE_ROBOT_STATE=-8\n\
00365 \n\
00366 # planning request errors\n\
00367 int32 INVALID_PLANNER_ID=-9\n\
00368 int32 INVALID_NUM_PLANNING_ATTEMPTS=-10\n\
00369 int32 INVALID_ALLOWED_PLANNING_TIME=-11\n\
00370 int32 INVALID_GROUP_NAME=-12\n\
00371 int32 INVALID_GOAL_JOINT_CONSTRAINTS=-13\n\
00372 int32 INVALID_GOAL_POSITION_CONSTRAINTS=-14\n\
00373 int32 INVALID_GOAL_ORIENTATION_CONSTRAINTS=-15\n\
00374 int32 INVALID_PATH_JOINT_CONSTRAINTS=-16\n\
00375 int32 INVALID_PATH_POSITION_CONSTRAINTS=-17\n\
00376 int32 INVALID_PATH_ORIENTATION_CONSTRAINTS=-18\n\
00377 \n\
00378 # state/trajectory monitor errors\n\
00379 int32 INVALID_TRAJECTORY=-19\n\
00380 int32 INVALID_INDEX=-20\n\
00381 int32 JOINT_LIMITS_VIOLATED=-21\n\
00382 int32 PATH_CONSTRAINTS_VIOLATED=-22\n\
00383 int32 COLLISION_CONSTRAINTS_VIOLATED=-23\n\
00384 int32 GOAL_CONSTRAINTS_VIOLATED=-24\n\
00385 int32 JOINTS_NOT_MOVING=-25\n\
00386 int32 TRAJECTORY_CONTROLLER_FAILED=-26\n\
00387 \n\
00388 # system errors\n\
00389 int32 FRAME_TRANSFORM_FAILURE=-27\n\
00390 int32 COLLISION_CHECKING_UNAVAILABLE=-28\n\
00391 int32 ROBOT_STATE_STALE=-29\n\
00392 int32 SENSOR_INFO_STALE=-30\n\
00393 \n\
00394 # kinematics errors\n\
00395 int32 NO_IK_SOLUTION=-31\n\
00396 int32 INVALID_LINK_NAME=-32\n\
00397 int32 IK_LINK_IN_COLLISION=-33\n\
00398 int32 NO_FK_SOLUTION=-34\n\
00399 int32 KINEMATICS_STATE_IN_COLLISION=-35\n\
00400 \n\
00401 # general errors\n\
00402 int32 INVALID_TIMEOUT=-36\n\
00403 \n\
00404 \n\
00405 ";
00406 }
00407
00408 static const char* value(const ::kdl_arm_kinematics::GetWeightedIKResponse_<ContainerAllocator> &) { return value(); }
00409 };
00410
00411 }
00412 }
00413
00414 namespace ros
00415 {
00416 namespace serialization
00417 {
00418
00419 template<class ContainerAllocator> struct Serializer< ::kdl_arm_kinematics::GetWeightedIKRequest_<ContainerAllocator> >
00420 {
00421 template<typename Stream, typename T> inline static void allInOne(Stream& stream, T m)
00422 {
00423 stream.next(m.pose);
00424 stream.next(m.tool_frame);
00425 stream.next(m.ik_seed);
00426 stream.next(m.weights);
00427 }
00428
00429 ROS_DECLARE_ALLINONE_SERIALIZER;
00430 };
00431 }
00432 }
00433
00434
00435 namespace ros
00436 {
00437 namespace serialization
00438 {
00439
00440 template<class ContainerAllocator> struct Serializer< ::kdl_arm_kinematics::GetWeightedIKResponse_<ContainerAllocator> >
00441 {
00442 template<typename Stream, typename T> inline static void allInOne(Stream& stream, T m)
00443 {
00444 stream.next(m.solution);
00445 stream.next(m.error_code);
00446 }
00447
00448 ROS_DECLARE_ALLINONE_SERIALIZER;
00449 };
00450 }
00451 }
00452
00453 namespace ros
00454 {
00455 namespace service_traits
00456 {
00457 template<>
00458 struct MD5Sum<kdl_arm_kinematics::GetWeightedIK> {
00459 static const char* value()
00460 {
00461 return "ae963d0cab638b025249ba90e48a170c";
00462 }
00463
00464 static const char* value(const kdl_arm_kinematics::GetWeightedIK&) { return value(); }
00465 };
00466
00467 template<>
00468 struct DataType<kdl_arm_kinematics::GetWeightedIK> {
00469 static const char* value()
00470 {
00471 return "kdl_arm_kinematics/GetWeightedIK";
00472 }
00473
00474 static const char* value(const kdl_arm_kinematics::GetWeightedIK&) { return value(); }
00475 };
00476
00477 template<class ContainerAllocator>
00478 struct MD5Sum<kdl_arm_kinematics::GetWeightedIKRequest_<ContainerAllocator> > {
00479 static const char* value()
00480 {
00481 return "ae963d0cab638b025249ba90e48a170c";
00482 }
00483
00484 static const char* value(const kdl_arm_kinematics::GetWeightedIKRequest_<ContainerAllocator> &) { return value(); }
00485 };
00486
00487 template<class ContainerAllocator>
00488 struct DataType<kdl_arm_kinematics::GetWeightedIKRequest_<ContainerAllocator> > {
00489 static const char* value()
00490 {
00491 return "kdl_arm_kinematics/GetWeightedIK";
00492 }
00493
00494 static const char* value(const kdl_arm_kinematics::GetWeightedIKRequest_<ContainerAllocator> &) { return value(); }
00495 };
00496
00497 template<class ContainerAllocator>
00498 struct MD5Sum<kdl_arm_kinematics::GetWeightedIKResponse_<ContainerAllocator> > {
00499 static const char* value()
00500 {
00501 return "ae963d0cab638b025249ba90e48a170c";
00502 }
00503
00504 static const char* value(const kdl_arm_kinematics::GetWeightedIKResponse_<ContainerAllocator> &) { return value(); }
00505 };
00506
00507 template<class ContainerAllocator>
00508 struct DataType<kdl_arm_kinematics::GetWeightedIKResponse_<ContainerAllocator> > {
00509 static const char* value()
00510 {
00511 return "kdl_arm_kinematics/GetWeightedIK";
00512 }
00513
00514 static const char* value(const kdl_arm_kinematics::GetWeightedIKResponse_<ContainerAllocator> &) { return value(); }
00515 };
00516
00517 }
00518 }
00519
00520 #endif // KDL_ARM_KINEMATICS_SERVICE_GETWEIGHTEDIK_H
00521