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