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