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