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