$search
00001 """autogenerated by genmsg_py from GetPositionFKRequest.msg. Do not edit.""" 00002 import roslib.message 00003 import struct 00004 00005 import arm_navigation_msgs.msg 00006 import geometry_msgs.msg 00007 import sensor_msgs.msg 00008 import roslib.rostime 00009 import std_msgs.msg 00010 00011 class GetPositionFKRequest(roslib.message.Message): 00012 _md5sum = "ddaa8b9932e60599795bcb983e28cf57" 00013 _type = "kinematics_msgs/GetPositionFKRequest" 00014 _has_header = True #flag to mark the presence of a Header object 00015 _full_text = """ 00016 00017 00018 Header header 00019 00020 00021 string[] fk_link_names 00022 00023 00024 arm_navigation_msgs/RobotState robot_state 00025 00026 ================================================================================ 00027 MSG: std_msgs/Header 00028 # Standard metadata for higher-level stamped data types. 00029 # This is generally used to communicate timestamped data 00030 # in a particular coordinate frame. 00031 # 00032 # sequence ID: consecutively increasing ID 00033 uint32 seq 00034 #Two-integer timestamp that is expressed as: 00035 # * stamp.secs: seconds (stamp_secs) since epoch 00036 # * stamp.nsecs: nanoseconds since stamp_secs 00037 # time-handling sugar is provided by the client library 00038 time stamp 00039 #Frame this data is associated with 00040 # 0: no frame 00041 # 1: global frame 00042 string frame_id 00043 00044 ================================================================================ 00045 MSG: arm_navigation_msgs/RobotState 00046 # This message contains information about the robot state, i.e. the positions of its joints and links 00047 sensor_msgs/JointState joint_state 00048 arm_navigation_msgs/MultiDOFJointState multi_dof_joint_state 00049 00050 ================================================================================ 00051 MSG: sensor_msgs/JointState 00052 # This is a message that holds data to describe the state of a set of torque controlled joints. 00053 # 00054 # The state of each joint (revolute or prismatic) is defined by: 00055 # * the position of the joint (rad or m), 00056 # * the velocity of the joint (rad/s or m/s) and 00057 # * the effort that is applied in the joint (Nm or N). 00058 # 00059 # Each joint is uniquely identified by its name 00060 # The header specifies the time at which the joint states were recorded. All the joint states 00061 # in one message have to be recorded at the same time. 00062 # 00063 # This message consists of a multiple arrays, one for each part of the joint state. 00064 # The goal is to make each of the fields optional. When e.g. your joints have no 00065 # effort associated with them, you can leave the effort array empty. 00066 # 00067 # All arrays in this message should have the same size, or be empty. 00068 # This is the only way to uniquely associate the joint name with the correct 00069 # states. 00070 00071 00072 Header header 00073 00074 string[] name 00075 float64[] position 00076 float64[] velocity 00077 float64[] effort 00078 00079 ================================================================================ 00080 MSG: arm_navigation_msgs/MultiDOFJointState 00081 #A representation of a multi-dof joint state 00082 time stamp 00083 string[] joint_names 00084 string[] frame_ids 00085 string[] child_frame_ids 00086 geometry_msgs/Pose[] poses 00087 00088 ================================================================================ 00089 MSG: geometry_msgs/Pose 00090 # A representation of pose in free space, composed of postion and orientation. 00091 Point position 00092 Quaternion orientation 00093 00094 ================================================================================ 00095 MSG: geometry_msgs/Point 00096 # This contains the position of a point in free space 00097 float64 x 00098 float64 y 00099 float64 z 00100 00101 ================================================================================ 00102 MSG: geometry_msgs/Quaternion 00103 # This represents an orientation in free space in quaternion form. 00104 00105 float64 x 00106 float64 y 00107 float64 z 00108 float64 w 00109 00110 """ 00111 __slots__ = ['header','fk_link_names','robot_state'] 00112 _slot_types = ['Header','string[]','arm_navigation_msgs/RobotState'] 00113 00114 def __init__(self, *args, **kwds): 00115 """ 00116 Constructor. Any message fields that are implicitly/explicitly 00117 set to None will be assigned a default value. The recommend 00118 use is keyword arguments as this is more robust to future message 00119 changes. You cannot mix in-order arguments and keyword arguments. 00120 00121 The available fields are: 00122 header,fk_link_names,robot_state 00123 00124 @param args: complete set of field values, in .msg order 00125 @param kwds: use keyword arguments corresponding to message field names 00126 to set specific fields. 00127 """ 00128 if args or kwds: 00129 super(GetPositionFKRequest, self).__init__(*args, **kwds) 00130 #message fields cannot be None, assign default values for those that are 00131 if self.header is None: 00132 self.header = std_msgs.msg._Header.Header() 00133 if self.fk_link_names is None: 00134 self.fk_link_names = [] 00135 if self.robot_state is None: 00136 self.robot_state = arm_navigation_msgs.msg.RobotState() 00137 else: 00138 self.header = std_msgs.msg._Header.Header() 00139 self.fk_link_names = [] 00140 self.robot_state = arm_navigation_msgs.msg.RobotState() 00141 00142 def _get_types(self): 00143 """ 00144 internal API method 00145 """ 00146 return self._slot_types 00147 00148 def serialize(self, buff): 00149 """ 00150 serialize message into buffer 00151 @param buff: buffer 00152 @type buff: StringIO 00153 """ 00154 try: 00155 _x = self 00156 buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs)) 00157 _x = self.header.frame_id 00158 length = len(_x) 00159 buff.write(struct.pack('<I%ss'%length, length, _x)) 00160 length = len(self.fk_link_names) 00161 buff.write(_struct_I.pack(length)) 00162 for val1 in self.fk_link_names: 00163 length = len(val1) 00164 buff.write(struct.pack('<I%ss'%length, length, val1)) 00165 _x = self 00166 buff.write(_struct_3I.pack(_x.robot_state.joint_state.header.seq, _x.robot_state.joint_state.header.stamp.secs, _x.robot_state.joint_state.header.stamp.nsecs)) 00167 _x = self.robot_state.joint_state.header.frame_id 00168 length = len(_x) 00169 buff.write(struct.pack('<I%ss'%length, length, _x)) 00170 length = len(self.robot_state.joint_state.name) 00171 buff.write(_struct_I.pack(length)) 00172 for val1 in self.robot_state.joint_state.name: 00173 length = len(val1) 00174 buff.write(struct.pack('<I%ss'%length, length, val1)) 00175 length = len(self.robot_state.joint_state.position) 00176 buff.write(_struct_I.pack(length)) 00177 pattern = '<%sd'%length 00178 buff.write(struct.pack(pattern, *self.robot_state.joint_state.position)) 00179 length = len(self.robot_state.joint_state.velocity) 00180 buff.write(_struct_I.pack(length)) 00181 pattern = '<%sd'%length 00182 buff.write(struct.pack(pattern, *self.robot_state.joint_state.velocity)) 00183 length = len(self.robot_state.joint_state.effort) 00184 buff.write(_struct_I.pack(length)) 00185 pattern = '<%sd'%length 00186 buff.write(struct.pack(pattern, *self.robot_state.joint_state.effort)) 00187 _x = self 00188 buff.write(_struct_2I.pack(_x.robot_state.multi_dof_joint_state.stamp.secs, _x.robot_state.multi_dof_joint_state.stamp.nsecs)) 00189 length = len(self.robot_state.multi_dof_joint_state.joint_names) 00190 buff.write(_struct_I.pack(length)) 00191 for val1 in self.robot_state.multi_dof_joint_state.joint_names: 00192 length = len(val1) 00193 buff.write(struct.pack('<I%ss'%length, length, val1)) 00194 length = len(self.robot_state.multi_dof_joint_state.frame_ids) 00195 buff.write(_struct_I.pack(length)) 00196 for val1 in self.robot_state.multi_dof_joint_state.frame_ids: 00197 length = len(val1) 00198 buff.write(struct.pack('<I%ss'%length, length, val1)) 00199 length = len(self.robot_state.multi_dof_joint_state.child_frame_ids) 00200 buff.write(_struct_I.pack(length)) 00201 for val1 in self.robot_state.multi_dof_joint_state.child_frame_ids: 00202 length = len(val1) 00203 buff.write(struct.pack('<I%ss'%length, length, val1)) 00204 length = len(self.robot_state.multi_dof_joint_state.poses) 00205 buff.write(_struct_I.pack(length)) 00206 for val1 in self.robot_state.multi_dof_joint_state.poses: 00207 _v1 = val1.position 00208 _x = _v1 00209 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00210 _v2 = val1.orientation 00211 _x = _v2 00212 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 00213 except struct.error as se: self._check_types(se) 00214 except TypeError as te: self._check_types(te) 00215 00216 def deserialize(self, str): 00217 """ 00218 unpack serialized message in str into this message instance 00219 @param str: byte array of serialized message 00220 @type str: str 00221 """ 00222 try: 00223 if self.header is None: 00224 self.header = std_msgs.msg._Header.Header() 00225 if self.robot_state is None: 00226 self.robot_state = arm_navigation_msgs.msg.RobotState() 00227 end = 0 00228 _x = self 00229 start = end 00230 end += 12 00231 (_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 00232 start = end 00233 end += 4 00234 (length,) = _struct_I.unpack(str[start:end]) 00235 start = end 00236 end += length 00237 self.header.frame_id = str[start:end] 00238 start = end 00239 end += 4 00240 (length,) = _struct_I.unpack(str[start:end]) 00241 self.fk_link_names = [] 00242 for i in range(0, length): 00243 start = end 00244 end += 4 00245 (length,) = _struct_I.unpack(str[start:end]) 00246 start = end 00247 end += length 00248 val1 = str[start:end] 00249 self.fk_link_names.append(val1) 00250 _x = self 00251 start = end 00252 end += 12 00253 (_x.robot_state.joint_state.header.seq, _x.robot_state.joint_state.header.stamp.secs, _x.robot_state.joint_state.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 00254 start = end 00255 end += 4 00256 (length,) = _struct_I.unpack(str[start:end]) 00257 start = end 00258 end += length 00259 self.robot_state.joint_state.header.frame_id = str[start:end] 00260 start = end 00261 end += 4 00262 (length,) = _struct_I.unpack(str[start:end]) 00263 self.robot_state.joint_state.name = [] 00264 for i in range(0, length): 00265 start = end 00266 end += 4 00267 (length,) = _struct_I.unpack(str[start:end]) 00268 start = end 00269 end += length 00270 val1 = str[start:end] 00271 self.robot_state.joint_state.name.append(val1) 00272 start = end 00273 end += 4 00274 (length,) = _struct_I.unpack(str[start:end]) 00275 pattern = '<%sd'%length 00276 start = end 00277 end += struct.calcsize(pattern) 00278 self.robot_state.joint_state.position = struct.unpack(pattern, str[start:end]) 00279 start = end 00280 end += 4 00281 (length,) = _struct_I.unpack(str[start:end]) 00282 pattern = '<%sd'%length 00283 start = end 00284 end += struct.calcsize(pattern) 00285 self.robot_state.joint_state.velocity = struct.unpack(pattern, str[start:end]) 00286 start = end 00287 end += 4 00288 (length,) = _struct_I.unpack(str[start:end]) 00289 pattern = '<%sd'%length 00290 start = end 00291 end += struct.calcsize(pattern) 00292 self.robot_state.joint_state.effort = struct.unpack(pattern, str[start:end]) 00293 _x = self 00294 start = end 00295 end += 8 00296 (_x.robot_state.multi_dof_joint_state.stamp.secs, _x.robot_state.multi_dof_joint_state.stamp.nsecs,) = _struct_2I.unpack(str[start:end]) 00297 start = end 00298 end += 4 00299 (length,) = _struct_I.unpack(str[start:end]) 00300 self.robot_state.multi_dof_joint_state.joint_names = [] 00301 for i in range(0, length): 00302 start = end 00303 end += 4 00304 (length,) = _struct_I.unpack(str[start:end]) 00305 start = end 00306 end += length 00307 val1 = str[start:end] 00308 self.robot_state.multi_dof_joint_state.joint_names.append(val1) 00309 start = end 00310 end += 4 00311 (length,) = _struct_I.unpack(str[start:end]) 00312 self.robot_state.multi_dof_joint_state.frame_ids = [] 00313 for i in range(0, length): 00314 start = end 00315 end += 4 00316 (length,) = _struct_I.unpack(str[start:end]) 00317 start = end 00318 end += length 00319 val1 = str[start:end] 00320 self.robot_state.multi_dof_joint_state.frame_ids.append(val1) 00321 start = end 00322 end += 4 00323 (length,) = _struct_I.unpack(str[start:end]) 00324 self.robot_state.multi_dof_joint_state.child_frame_ids = [] 00325 for i in range(0, length): 00326 start = end 00327 end += 4 00328 (length,) = _struct_I.unpack(str[start:end]) 00329 start = end 00330 end += length 00331 val1 = str[start:end] 00332 self.robot_state.multi_dof_joint_state.child_frame_ids.append(val1) 00333 start = end 00334 end += 4 00335 (length,) = _struct_I.unpack(str[start:end]) 00336 self.robot_state.multi_dof_joint_state.poses = [] 00337 for i in range(0, length): 00338 val1 = geometry_msgs.msg.Pose() 00339 _v3 = val1.position 00340 _x = _v3 00341 start = end 00342 end += 24 00343 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 00344 _v4 = val1.orientation 00345 _x = _v4 00346 start = end 00347 end += 32 00348 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 00349 self.robot_state.multi_dof_joint_state.poses.append(val1) 00350 return self 00351 except struct.error as e: 00352 raise roslib.message.DeserializationError(e) #most likely buffer underfill 00353 00354 00355 def serialize_numpy(self, buff, numpy): 00356 """ 00357 serialize message with numpy array types into buffer 00358 @param buff: buffer 00359 @type buff: StringIO 00360 @param numpy: numpy python module 00361 @type numpy module 00362 """ 00363 try: 00364 _x = self 00365 buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs)) 00366 _x = self.header.frame_id 00367 length = len(_x) 00368 buff.write(struct.pack('<I%ss'%length, length, _x)) 00369 length = len(self.fk_link_names) 00370 buff.write(_struct_I.pack(length)) 00371 for val1 in self.fk_link_names: 00372 length = len(val1) 00373 buff.write(struct.pack('<I%ss'%length, length, val1)) 00374 _x = self 00375 buff.write(_struct_3I.pack(_x.robot_state.joint_state.header.seq, _x.robot_state.joint_state.header.stamp.secs, _x.robot_state.joint_state.header.stamp.nsecs)) 00376 _x = self.robot_state.joint_state.header.frame_id 00377 length = len(_x) 00378 buff.write(struct.pack('<I%ss'%length, length, _x)) 00379 length = len(self.robot_state.joint_state.name) 00380 buff.write(_struct_I.pack(length)) 00381 for val1 in self.robot_state.joint_state.name: 00382 length = len(val1) 00383 buff.write(struct.pack('<I%ss'%length, length, val1)) 00384 length = len(self.robot_state.joint_state.position) 00385 buff.write(_struct_I.pack(length)) 00386 pattern = '<%sd'%length 00387 buff.write(self.robot_state.joint_state.position.tostring()) 00388 length = len(self.robot_state.joint_state.velocity) 00389 buff.write(_struct_I.pack(length)) 00390 pattern = '<%sd'%length 00391 buff.write(self.robot_state.joint_state.velocity.tostring()) 00392 length = len(self.robot_state.joint_state.effort) 00393 buff.write(_struct_I.pack(length)) 00394 pattern = '<%sd'%length 00395 buff.write(self.robot_state.joint_state.effort.tostring()) 00396 _x = self 00397 buff.write(_struct_2I.pack(_x.robot_state.multi_dof_joint_state.stamp.secs, _x.robot_state.multi_dof_joint_state.stamp.nsecs)) 00398 length = len(self.robot_state.multi_dof_joint_state.joint_names) 00399 buff.write(_struct_I.pack(length)) 00400 for val1 in self.robot_state.multi_dof_joint_state.joint_names: 00401 length = len(val1) 00402 buff.write(struct.pack('<I%ss'%length, length, val1)) 00403 length = len(self.robot_state.multi_dof_joint_state.frame_ids) 00404 buff.write(_struct_I.pack(length)) 00405 for val1 in self.robot_state.multi_dof_joint_state.frame_ids: 00406 length = len(val1) 00407 buff.write(struct.pack('<I%ss'%length, length, val1)) 00408 length = len(self.robot_state.multi_dof_joint_state.child_frame_ids) 00409 buff.write(_struct_I.pack(length)) 00410 for val1 in self.robot_state.multi_dof_joint_state.child_frame_ids: 00411 length = len(val1) 00412 buff.write(struct.pack('<I%ss'%length, length, val1)) 00413 length = len(self.robot_state.multi_dof_joint_state.poses) 00414 buff.write(_struct_I.pack(length)) 00415 for val1 in self.robot_state.multi_dof_joint_state.poses: 00416 _v5 = val1.position 00417 _x = _v5 00418 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00419 _v6 = val1.orientation 00420 _x = _v6 00421 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 00422 except struct.error as se: self._check_types(se) 00423 except TypeError as te: self._check_types(te) 00424 00425 def deserialize_numpy(self, str, numpy): 00426 """ 00427 unpack serialized message in str into this message instance using numpy for array types 00428 @param str: byte array of serialized message 00429 @type str: str 00430 @param numpy: numpy python module 00431 @type numpy: module 00432 """ 00433 try: 00434 if self.header is None: 00435 self.header = std_msgs.msg._Header.Header() 00436 if self.robot_state is None: 00437 self.robot_state = arm_navigation_msgs.msg.RobotState() 00438 end = 0 00439 _x = self 00440 start = end 00441 end += 12 00442 (_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 00443 start = end 00444 end += 4 00445 (length,) = _struct_I.unpack(str[start:end]) 00446 start = end 00447 end += length 00448 self.header.frame_id = str[start:end] 00449 start = end 00450 end += 4 00451 (length,) = _struct_I.unpack(str[start:end]) 00452 self.fk_link_names = [] 00453 for i in range(0, length): 00454 start = end 00455 end += 4 00456 (length,) = _struct_I.unpack(str[start:end]) 00457 start = end 00458 end += length 00459 val1 = str[start:end] 00460 self.fk_link_names.append(val1) 00461 _x = self 00462 start = end 00463 end += 12 00464 (_x.robot_state.joint_state.header.seq, _x.robot_state.joint_state.header.stamp.secs, _x.robot_state.joint_state.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 00465 start = end 00466 end += 4 00467 (length,) = _struct_I.unpack(str[start:end]) 00468 start = end 00469 end += length 00470 self.robot_state.joint_state.header.frame_id = str[start:end] 00471 start = end 00472 end += 4 00473 (length,) = _struct_I.unpack(str[start:end]) 00474 self.robot_state.joint_state.name = [] 00475 for i in range(0, length): 00476 start = end 00477 end += 4 00478 (length,) = _struct_I.unpack(str[start:end]) 00479 start = end 00480 end += length 00481 val1 = str[start:end] 00482 self.robot_state.joint_state.name.append(val1) 00483 start = end 00484 end += 4 00485 (length,) = _struct_I.unpack(str[start:end]) 00486 pattern = '<%sd'%length 00487 start = end 00488 end += struct.calcsize(pattern) 00489 self.robot_state.joint_state.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 00490 start = end 00491 end += 4 00492 (length,) = _struct_I.unpack(str[start:end]) 00493 pattern = '<%sd'%length 00494 start = end 00495 end += struct.calcsize(pattern) 00496 self.robot_state.joint_state.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 00497 start = end 00498 end += 4 00499 (length,) = _struct_I.unpack(str[start:end]) 00500 pattern = '<%sd'%length 00501 start = end 00502 end += struct.calcsize(pattern) 00503 self.robot_state.joint_state.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 00504 _x = self 00505 start = end 00506 end += 8 00507 (_x.robot_state.multi_dof_joint_state.stamp.secs, _x.robot_state.multi_dof_joint_state.stamp.nsecs,) = _struct_2I.unpack(str[start:end]) 00508 start = end 00509 end += 4 00510 (length,) = _struct_I.unpack(str[start:end]) 00511 self.robot_state.multi_dof_joint_state.joint_names = [] 00512 for i in range(0, length): 00513 start = end 00514 end += 4 00515 (length,) = _struct_I.unpack(str[start:end]) 00516 start = end 00517 end += length 00518 val1 = str[start:end] 00519 self.robot_state.multi_dof_joint_state.joint_names.append(val1) 00520 start = end 00521 end += 4 00522 (length,) = _struct_I.unpack(str[start:end]) 00523 self.robot_state.multi_dof_joint_state.frame_ids = [] 00524 for i in range(0, length): 00525 start = end 00526 end += 4 00527 (length,) = _struct_I.unpack(str[start:end]) 00528 start = end 00529 end += length 00530 val1 = str[start:end] 00531 self.robot_state.multi_dof_joint_state.frame_ids.append(val1) 00532 start = end 00533 end += 4 00534 (length,) = _struct_I.unpack(str[start:end]) 00535 self.robot_state.multi_dof_joint_state.child_frame_ids = [] 00536 for i in range(0, length): 00537 start = end 00538 end += 4 00539 (length,) = _struct_I.unpack(str[start:end]) 00540 start = end 00541 end += length 00542 val1 = str[start:end] 00543 self.robot_state.multi_dof_joint_state.child_frame_ids.append(val1) 00544 start = end 00545 end += 4 00546 (length,) = _struct_I.unpack(str[start:end]) 00547 self.robot_state.multi_dof_joint_state.poses = [] 00548 for i in range(0, length): 00549 val1 = geometry_msgs.msg.Pose() 00550 _v7 = val1.position 00551 _x = _v7 00552 start = end 00553 end += 24 00554 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 00555 _v8 = val1.orientation 00556 _x = _v8 00557 start = end 00558 end += 32 00559 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 00560 self.robot_state.multi_dof_joint_state.poses.append(val1) 00561 return self 00562 except struct.error as e: 00563 raise roslib.message.DeserializationError(e) #most likely buffer underfill 00564 00565 _struct_I = roslib.message.struct_I 00566 _struct_4d = struct.Struct("<4d") 00567 _struct_3I = struct.Struct("<3I") 00568 _struct_2I = struct.Struct("<2I") 00569 _struct_3d = struct.Struct("<3d") 00570 """autogenerated by genmsg_py from GetPositionFKResponse.msg. Do not edit.""" 00571 import roslib.message 00572 import struct 00573 00574 import arm_navigation_msgs.msg 00575 import geometry_msgs.msg 00576 import std_msgs.msg 00577 00578 class GetPositionFKResponse(roslib.message.Message): 00579 _md5sum = "4a3efc190f6ac6f248069cfa3cd94286" 00580 _type = "kinematics_msgs/GetPositionFKResponse" 00581 _has_header = False #flag to mark the presence of a Header object 00582 _full_text = """ 00583 geometry_msgs/PoseStamped[] pose_stamped 00584 00585 00586 string[] fk_link_names 00587 00588 arm_navigation_msgs/ArmNavigationErrorCodes error_code 00589 00590 00591 ================================================================================ 00592 MSG: geometry_msgs/PoseStamped 00593 # A Pose with reference coordinate frame and timestamp 00594 Header header 00595 Pose pose 00596 00597 ================================================================================ 00598 MSG: std_msgs/Header 00599 # Standard metadata for higher-level stamped data types. 00600 # This is generally used to communicate timestamped data 00601 # in a particular coordinate frame. 00602 # 00603 # sequence ID: consecutively increasing ID 00604 uint32 seq 00605 #Two-integer timestamp that is expressed as: 00606 # * stamp.secs: seconds (stamp_secs) since epoch 00607 # * stamp.nsecs: nanoseconds since stamp_secs 00608 # time-handling sugar is provided by the client library 00609 time stamp 00610 #Frame this data is associated with 00611 # 0: no frame 00612 # 1: global frame 00613 string frame_id 00614 00615 ================================================================================ 00616 MSG: geometry_msgs/Pose 00617 # A representation of pose in free space, composed of postion and orientation. 00618 Point position 00619 Quaternion orientation 00620 00621 ================================================================================ 00622 MSG: geometry_msgs/Point 00623 # This contains the position of a point in free space 00624 float64 x 00625 float64 y 00626 float64 z 00627 00628 ================================================================================ 00629 MSG: geometry_msgs/Quaternion 00630 # This represents an orientation in free space in quaternion form. 00631 00632 float64 x 00633 float64 y 00634 float64 z 00635 float64 w 00636 00637 ================================================================================ 00638 MSG: arm_navigation_msgs/ArmNavigationErrorCodes 00639 int32 val 00640 00641 # overall behavior 00642 int32 PLANNING_FAILED=-1 00643 int32 SUCCESS=1 00644 int32 TIMED_OUT=-2 00645 00646 # start state errors 00647 int32 START_STATE_IN_COLLISION=-3 00648 int32 START_STATE_VIOLATES_PATH_CONSTRAINTS=-4 00649 00650 # goal errors 00651 int32 GOAL_IN_COLLISION=-5 00652 int32 GOAL_VIOLATES_PATH_CONSTRAINTS=-6 00653 00654 # robot state 00655 int32 INVALID_ROBOT_STATE=-7 00656 int32 INCOMPLETE_ROBOT_STATE=-8 00657 00658 # planning request errors 00659 int32 INVALID_PLANNER_ID=-9 00660 int32 INVALID_NUM_PLANNING_ATTEMPTS=-10 00661 int32 INVALID_ALLOWED_PLANNING_TIME=-11 00662 int32 INVALID_GROUP_NAME=-12 00663 int32 INVALID_GOAL_JOINT_CONSTRAINTS=-13 00664 int32 INVALID_GOAL_POSITION_CONSTRAINTS=-14 00665 int32 INVALID_GOAL_ORIENTATION_CONSTRAINTS=-15 00666 int32 INVALID_PATH_JOINT_CONSTRAINTS=-16 00667 int32 INVALID_PATH_POSITION_CONSTRAINTS=-17 00668 int32 INVALID_PATH_ORIENTATION_CONSTRAINTS=-18 00669 00670 # state/trajectory monitor errors 00671 int32 INVALID_TRAJECTORY=-19 00672 int32 INVALID_INDEX=-20 00673 int32 JOINT_LIMITS_VIOLATED=-21 00674 int32 PATH_CONSTRAINTS_VIOLATED=-22 00675 int32 COLLISION_CONSTRAINTS_VIOLATED=-23 00676 int32 GOAL_CONSTRAINTS_VIOLATED=-24 00677 int32 JOINTS_NOT_MOVING=-25 00678 int32 TRAJECTORY_CONTROLLER_FAILED=-26 00679 00680 # system errors 00681 int32 FRAME_TRANSFORM_FAILURE=-27 00682 int32 COLLISION_CHECKING_UNAVAILABLE=-28 00683 int32 ROBOT_STATE_STALE=-29 00684 int32 SENSOR_INFO_STALE=-30 00685 00686 # kinematics errors 00687 int32 NO_IK_SOLUTION=-31 00688 int32 INVALID_LINK_NAME=-32 00689 int32 IK_LINK_IN_COLLISION=-33 00690 int32 NO_FK_SOLUTION=-34 00691 int32 KINEMATICS_STATE_IN_COLLISION=-35 00692 00693 # general errors 00694 int32 INVALID_TIMEOUT=-36 00695 00696 00697 """ 00698 __slots__ = ['pose_stamped','fk_link_names','error_code'] 00699 _slot_types = ['geometry_msgs/PoseStamped[]','string[]','arm_navigation_msgs/ArmNavigationErrorCodes'] 00700 00701 def __init__(self, *args, **kwds): 00702 """ 00703 Constructor. Any message fields that are implicitly/explicitly 00704 set to None will be assigned a default value. The recommend 00705 use is keyword arguments as this is more robust to future message 00706 changes. You cannot mix in-order arguments and keyword arguments. 00707 00708 The available fields are: 00709 pose_stamped,fk_link_names,error_code 00710 00711 @param args: complete set of field values, in .msg order 00712 @param kwds: use keyword arguments corresponding to message field names 00713 to set specific fields. 00714 """ 00715 if args or kwds: 00716 super(GetPositionFKResponse, self).__init__(*args, **kwds) 00717 #message fields cannot be None, assign default values for those that are 00718 if self.pose_stamped is None: 00719 self.pose_stamped = [] 00720 if self.fk_link_names is None: 00721 self.fk_link_names = [] 00722 if self.error_code is None: 00723 self.error_code = arm_navigation_msgs.msg.ArmNavigationErrorCodes() 00724 else: 00725 self.pose_stamped = [] 00726 self.fk_link_names = [] 00727 self.error_code = arm_navigation_msgs.msg.ArmNavigationErrorCodes() 00728 00729 def _get_types(self): 00730 """ 00731 internal API method 00732 """ 00733 return self._slot_types 00734 00735 def serialize(self, buff): 00736 """ 00737 serialize message into buffer 00738 @param buff: buffer 00739 @type buff: StringIO 00740 """ 00741 try: 00742 length = len(self.pose_stamped) 00743 buff.write(_struct_I.pack(length)) 00744 for val1 in self.pose_stamped: 00745 _v9 = val1.header 00746 buff.write(_struct_I.pack(_v9.seq)) 00747 _v10 = _v9.stamp 00748 _x = _v10 00749 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00750 _x = _v9.frame_id 00751 length = len(_x) 00752 buff.write(struct.pack('<I%ss'%length, length, _x)) 00753 _v11 = val1.pose 00754 _v12 = _v11.position 00755 _x = _v12 00756 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00757 _v13 = _v11.orientation 00758 _x = _v13 00759 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 00760 length = len(self.fk_link_names) 00761 buff.write(_struct_I.pack(length)) 00762 for val1 in self.fk_link_names: 00763 length = len(val1) 00764 buff.write(struct.pack('<I%ss'%length, length, val1)) 00765 buff.write(_struct_i.pack(self.error_code.val)) 00766 except struct.error as se: self._check_types(se) 00767 except TypeError as te: self._check_types(te) 00768 00769 def deserialize(self, str): 00770 """ 00771 unpack serialized message in str into this message instance 00772 @param str: byte array of serialized message 00773 @type str: str 00774 """ 00775 try: 00776 if self.error_code is None: 00777 self.error_code = arm_navigation_msgs.msg.ArmNavigationErrorCodes() 00778 end = 0 00779 start = end 00780 end += 4 00781 (length,) = _struct_I.unpack(str[start:end]) 00782 self.pose_stamped = [] 00783 for i in range(0, length): 00784 val1 = geometry_msgs.msg.PoseStamped() 00785 _v14 = val1.header 00786 start = end 00787 end += 4 00788 (_v14.seq,) = _struct_I.unpack(str[start:end]) 00789 _v15 = _v14.stamp 00790 _x = _v15 00791 start = end 00792 end += 8 00793 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 00794 start = end 00795 end += 4 00796 (length,) = _struct_I.unpack(str[start:end]) 00797 start = end 00798 end += length 00799 _v14.frame_id = str[start:end] 00800 _v16 = val1.pose 00801 _v17 = _v16.position 00802 _x = _v17 00803 start = end 00804 end += 24 00805 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 00806 _v18 = _v16.orientation 00807 _x = _v18 00808 start = end 00809 end += 32 00810 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 00811 self.pose_stamped.append(val1) 00812 start = end 00813 end += 4 00814 (length,) = _struct_I.unpack(str[start:end]) 00815 self.fk_link_names = [] 00816 for i in range(0, length): 00817 start = end 00818 end += 4 00819 (length,) = _struct_I.unpack(str[start:end]) 00820 start = end 00821 end += length 00822 val1 = str[start:end] 00823 self.fk_link_names.append(val1) 00824 start = end 00825 end += 4 00826 (self.error_code.val,) = _struct_i.unpack(str[start:end]) 00827 return self 00828 except struct.error as e: 00829 raise roslib.message.DeserializationError(e) #most likely buffer underfill 00830 00831 00832 def serialize_numpy(self, buff, numpy): 00833 """ 00834 serialize message with numpy array types into buffer 00835 @param buff: buffer 00836 @type buff: StringIO 00837 @param numpy: numpy python module 00838 @type numpy module 00839 """ 00840 try: 00841 length = len(self.pose_stamped) 00842 buff.write(_struct_I.pack(length)) 00843 for val1 in self.pose_stamped: 00844 _v19 = val1.header 00845 buff.write(_struct_I.pack(_v19.seq)) 00846 _v20 = _v19.stamp 00847 _x = _v20 00848 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00849 _x = _v19.frame_id 00850 length = len(_x) 00851 buff.write(struct.pack('<I%ss'%length, length, _x)) 00852 _v21 = val1.pose 00853 _v22 = _v21.position 00854 _x = _v22 00855 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00856 _v23 = _v21.orientation 00857 _x = _v23 00858 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 00859 length = len(self.fk_link_names) 00860 buff.write(_struct_I.pack(length)) 00861 for val1 in self.fk_link_names: 00862 length = len(val1) 00863 buff.write(struct.pack('<I%ss'%length, length, val1)) 00864 buff.write(_struct_i.pack(self.error_code.val)) 00865 except struct.error as se: self._check_types(se) 00866 except TypeError as te: self._check_types(te) 00867 00868 def deserialize_numpy(self, str, numpy): 00869 """ 00870 unpack serialized message in str into this message instance using numpy for array types 00871 @param str: byte array of serialized message 00872 @type str: str 00873 @param numpy: numpy python module 00874 @type numpy: module 00875 """ 00876 try: 00877 if self.error_code is None: 00878 self.error_code = arm_navigation_msgs.msg.ArmNavigationErrorCodes() 00879 end = 0 00880 start = end 00881 end += 4 00882 (length,) = _struct_I.unpack(str[start:end]) 00883 self.pose_stamped = [] 00884 for i in range(0, length): 00885 val1 = geometry_msgs.msg.PoseStamped() 00886 _v24 = val1.header 00887 start = end 00888 end += 4 00889 (_v24.seq,) = _struct_I.unpack(str[start:end]) 00890 _v25 = _v24.stamp 00891 _x = _v25 00892 start = end 00893 end += 8 00894 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 00895 start = end 00896 end += 4 00897 (length,) = _struct_I.unpack(str[start:end]) 00898 start = end 00899 end += length 00900 _v24.frame_id = str[start:end] 00901 _v26 = val1.pose 00902 _v27 = _v26.position 00903 _x = _v27 00904 start = end 00905 end += 24 00906 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 00907 _v28 = _v26.orientation 00908 _x = _v28 00909 start = end 00910 end += 32 00911 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 00912 self.pose_stamped.append(val1) 00913 start = end 00914 end += 4 00915 (length,) = _struct_I.unpack(str[start:end]) 00916 self.fk_link_names = [] 00917 for i in range(0, length): 00918 start = end 00919 end += 4 00920 (length,) = _struct_I.unpack(str[start:end]) 00921 start = end 00922 end += length 00923 val1 = str[start:end] 00924 self.fk_link_names.append(val1) 00925 start = end 00926 end += 4 00927 (self.error_code.val,) = _struct_i.unpack(str[start:end]) 00928 return self 00929 except struct.error as e: 00930 raise roslib.message.DeserializationError(e) #most likely buffer underfill 00931 00932 _struct_I = roslib.message.struct_I 00933 _struct_i = struct.Struct("<i") 00934 _struct_4d = struct.Struct("<4d") 00935 _struct_2I = struct.Struct("<2I") 00936 _struct_3d = struct.Struct("<3d") 00937 class GetPositionFK(roslib.message.ServiceDefinition): 00938 _type = 'kinematics_msgs/GetPositionFK' 00939 _md5sum = '2088007c8963e2252a67c872affa0985' 00940 _request_class = GetPositionFKRequest 00941 _response_class = GetPositionFKResponse