00001 """autogenerated by genpy from arm_navigation_msgs/Constraints.msg. Do not edit."""
00002 import sys
00003 python3 = True if sys.hexversion > 0x03000000 else False
00004 import genpy
00005 import struct
00006
00007 import arm_navigation_msgs.msg
00008 import geometry_msgs.msg
00009 import std_msgs.msg
00010
00011 class Constraints(genpy.Message):
00012 _md5sum = "fe6b6f09c687fd46c05a2de4ca18378a"
00013 _type = "arm_navigation_msgs/Constraints"
00014 _has_header = False
00015 _full_text = """# This message contains a list of motion planning constraints.
00016
00017 arm_navigation_msgs/JointConstraint[] joint_constraints
00018 arm_navigation_msgs/PositionConstraint[] position_constraints
00019 arm_navigation_msgs/OrientationConstraint[] orientation_constraints
00020 arm_navigation_msgs/VisibilityConstraint[] visibility_constraints
00021
00022 ================================================================================
00023 MSG: arm_navigation_msgs/JointConstraint
00024 # Constrain the position of a joint to be within a certain bound
00025 string joint_name
00026
00027 # the bound to be achieved is [position - tolerance_below, position + tolerance_above]
00028 float64 position
00029 float64 tolerance_above
00030 float64 tolerance_below
00031
00032 # A weighting factor for this constraint
00033 float64 weight
00034 ================================================================================
00035 MSG: arm_navigation_msgs/PositionConstraint
00036 # This message contains the definition of a position constraint.
00037 Header header
00038
00039 # The robot link this constraint refers to
00040 string link_name
00041
00042 # The offset (in the link frame) for the target point on the link we are planning for
00043 geometry_msgs/Point target_point_offset
00044
00045 # The nominal/target position for the point we are planning for
00046 geometry_msgs/Point position
00047
00048 # The shape of the bounded region that constrains the position of the end-effector
00049 # This region is always centered at the position defined above
00050 arm_navigation_msgs/Shape constraint_region_shape
00051
00052 # The orientation of the bounded region that constrains the position of the end-effector.
00053 # This allows the specification of non-axis aligned constraints
00054 geometry_msgs/Quaternion constraint_region_orientation
00055
00056 # Constraint weighting factor - a weight for this constraint
00057 float64 weight
00058
00059 ================================================================================
00060 MSG: std_msgs/Header
00061 # Standard metadata for higher-level stamped data types.
00062 # This is generally used to communicate timestamped data
00063 # in a particular coordinate frame.
00064 #
00065 # sequence ID: consecutively increasing ID
00066 uint32 seq
00067 #Two-integer timestamp that is expressed as:
00068 # * stamp.secs: seconds (stamp_secs) since epoch
00069 # * stamp.nsecs: nanoseconds since stamp_secs
00070 # time-handling sugar is provided by the client library
00071 time stamp
00072 #Frame this data is associated with
00073 # 0: no frame
00074 # 1: global frame
00075 string frame_id
00076
00077 ================================================================================
00078 MSG: geometry_msgs/Point
00079 # This contains the position of a point in free space
00080 float64 x
00081 float64 y
00082 float64 z
00083
00084 ================================================================================
00085 MSG: arm_navigation_msgs/Shape
00086 byte SPHERE=0
00087 byte BOX=1
00088 byte CYLINDER=2
00089 byte MESH=3
00090
00091 byte type
00092
00093
00094 #### define sphere, box, cylinder ####
00095 # the origin of each shape is considered at the shape's center
00096
00097 # for sphere
00098 # radius := dimensions[0]
00099
00100 # for cylinder
00101 # radius := dimensions[0]
00102 # length := dimensions[1]
00103 # the length is along the Z axis
00104
00105 # for box
00106 # size_x := dimensions[0]
00107 # size_y := dimensions[1]
00108 # size_z := dimensions[2]
00109 float64[] dimensions
00110
00111
00112 #### define mesh ####
00113
00114 # list of triangles; triangle k is defined by tre vertices located
00115 # at indices triangles[3k], triangles[3k+1], triangles[3k+2]
00116 int32[] triangles
00117 geometry_msgs/Point[] vertices
00118
00119 ================================================================================
00120 MSG: geometry_msgs/Quaternion
00121 # This represents an orientation in free space in quaternion form.
00122
00123 float64 x
00124 float64 y
00125 float64 z
00126 float64 w
00127
00128 ================================================================================
00129 MSG: arm_navigation_msgs/OrientationConstraint
00130 # This message contains the definition of an orientation constraint.
00131 Header header
00132
00133 # The robot link this constraint refers to
00134 string link_name
00135
00136 # The type of the constraint
00137 int32 type
00138 int32 LINK_FRAME=0
00139 int32 HEADER_FRAME=1
00140
00141 # The desired orientation of the robot link specified as a quaternion
00142 geometry_msgs/Quaternion orientation
00143
00144 # optional RPY error tolerances specified if
00145 float64 absolute_roll_tolerance
00146 float64 absolute_pitch_tolerance
00147 float64 absolute_yaw_tolerance
00148
00149 # Constraint weighting factor - a weight for this constraint
00150 float64 weight
00151
00152 ================================================================================
00153 MSG: arm_navigation_msgs/VisibilityConstraint
00154 # This message contains the definition of a visibility constraint.
00155 Header header
00156
00157 # The point stamped target that needs to be kept within view of the sensor
00158 geometry_msgs/PointStamped target
00159
00160 # The local pose of the frame in which visibility is to be maintained
00161 # The frame id should represent the robot link to which the sensor is attached
00162 # The visual axis of the sensor is assumed to be along the X axis of this frame
00163 geometry_msgs/PoseStamped sensor_pose
00164
00165 # The deviation (in radians) that will be tolerated
00166 # Constraint error will be measured as the solid angle between the
00167 # X axis of the frame defined above and the vector between the origin
00168 # of the frame defined above and the target location
00169 float64 absolute_tolerance
00170
00171
00172 ================================================================================
00173 MSG: geometry_msgs/PointStamped
00174 # This represents a Point with reference coordinate frame and timestamp
00175 Header header
00176 Point point
00177
00178 ================================================================================
00179 MSG: geometry_msgs/PoseStamped
00180 # A Pose with reference coordinate frame and timestamp
00181 Header header
00182 Pose pose
00183
00184 ================================================================================
00185 MSG: geometry_msgs/Pose
00186 # A representation of pose in free space, composed of postion and orientation.
00187 Point position
00188 Quaternion orientation
00189
00190 """
00191 __slots__ = ['joint_constraints','position_constraints','orientation_constraints','visibility_constraints']
00192 _slot_types = ['arm_navigation_msgs/JointConstraint[]','arm_navigation_msgs/PositionConstraint[]','arm_navigation_msgs/OrientationConstraint[]','arm_navigation_msgs/VisibilityConstraint[]']
00193
00194 def __init__(self, *args, **kwds):
00195 """
00196 Constructor. Any message fields that are implicitly/explicitly
00197 set to None will be assigned a default value. The recommend
00198 use is keyword arguments as this is more robust to future message
00199 changes. You cannot mix in-order arguments and keyword arguments.
00200
00201 The available fields are:
00202 joint_constraints,position_constraints,orientation_constraints,visibility_constraints
00203
00204 :param args: complete set of field values, in .msg order
00205 :param kwds: use keyword arguments corresponding to message field names
00206 to set specific fields.
00207 """
00208 if args or kwds:
00209 super(Constraints, self).__init__(*args, **kwds)
00210
00211 if self.joint_constraints is None:
00212 self.joint_constraints = []
00213 if self.position_constraints is None:
00214 self.position_constraints = []
00215 if self.orientation_constraints is None:
00216 self.orientation_constraints = []
00217 if self.visibility_constraints is None:
00218 self.visibility_constraints = []
00219 else:
00220 self.joint_constraints = []
00221 self.position_constraints = []
00222 self.orientation_constraints = []
00223 self.visibility_constraints = []
00224
00225 def _get_types(self):
00226 """
00227 internal API method
00228 """
00229 return self._slot_types
00230
00231 def serialize(self, buff):
00232 """
00233 serialize message into buffer
00234 :param buff: buffer, ``StringIO``
00235 """
00236 try:
00237 length = len(self.joint_constraints)
00238 buff.write(_struct_I.pack(length))
00239 for val1 in self.joint_constraints:
00240 _x = val1.joint_name
00241 length = len(_x)
00242 if python3 or type(_x) == unicode:
00243 _x = _x.encode('utf-8')
00244 length = len(_x)
00245 buff.write(struct.pack('<I%ss'%length, length, _x))
00246 _x = val1
00247 buff.write(_struct_4d.pack(_x.position, _x.tolerance_above, _x.tolerance_below, _x.weight))
00248 length = len(self.position_constraints)
00249 buff.write(_struct_I.pack(length))
00250 for val1 in self.position_constraints:
00251 _v1 = val1.header
00252 buff.write(_struct_I.pack(_v1.seq))
00253 _v2 = _v1.stamp
00254 _x = _v2
00255 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00256 _x = _v1.frame_id
00257 length = len(_x)
00258 if python3 or type(_x) == unicode:
00259 _x = _x.encode('utf-8')
00260 length = len(_x)
00261 buff.write(struct.pack('<I%ss'%length, length, _x))
00262 _x = val1.link_name
00263 length = len(_x)
00264 if python3 or type(_x) == unicode:
00265 _x = _x.encode('utf-8')
00266 length = len(_x)
00267 buff.write(struct.pack('<I%ss'%length, length, _x))
00268 _v3 = val1.target_point_offset
00269 _x = _v3
00270 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00271 _v4 = val1.position
00272 _x = _v4
00273 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00274 _v5 = val1.constraint_region_shape
00275 buff.write(_struct_b.pack(_v5.type))
00276 length = len(_v5.dimensions)
00277 buff.write(_struct_I.pack(length))
00278 pattern = '<%sd'%length
00279 buff.write(struct.pack(pattern, *_v5.dimensions))
00280 length = len(_v5.triangles)
00281 buff.write(_struct_I.pack(length))
00282 pattern = '<%si'%length
00283 buff.write(struct.pack(pattern, *_v5.triangles))
00284 length = len(_v5.vertices)
00285 buff.write(_struct_I.pack(length))
00286 for val3 in _v5.vertices:
00287 _x = val3
00288 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00289 _v6 = val1.constraint_region_orientation
00290 _x = _v6
00291 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00292 buff.write(_struct_d.pack(val1.weight))
00293 length = len(self.orientation_constraints)
00294 buff.write(_struct_I.pack(length))
00295 for val1 in self.orientation_constraints:
00296 _v7 = val1.header
00297 buff.write(_struct_I.pack(_v7.seq))
00298 _v8 = _v7.stamp
00299 _x = _v8
00300 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00301 _x = _v7.frame_id
00302 length = len(_x)
00303 if python3 or type(_x) == unicode:
00304 _x = _x.encode('utf-8')
00305 length = len(_x)
00306 buff.write(struct.pack('<I%ss'%length, length, _x))
00307 _x = val1.link_name
00308 length = len(_x)
00309 if python3 or type(_x) == unicode:
00310 _x = _x.encode('utf-8')
00311 length = len(_x)
00312 buff.write(struct.pack('<I%ss'%length, length, _x))
00313 buff.write(_struct_i.pack(val1.type))
00314 _v9 = val1.orientation
00315 _x = _v9
00316 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00317 _x = val1
00318 buff.write(_struct_4d.pack(_x.absolute_roll_tolerance, _x.absolute_pitch_tolerance, _x.absolute_yaw_tolerance, _x.weight))
00319 length = len(self.visibility_constraints)
00320 buff.write(_struct_I.pack(length))
00321 for val1 in self.visibility_constraints:
00322 _v10 = val1.header
00323 buff.write(_struct_I.pack(_v10.seq))
00324 _v11 = _v10.stamp
00325 _x = _v11
00326 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00327 _x = _v10.frame_id
00328 length = len(_x)
00329 if python3 or type(_x) == unicode:
00330 _x = _x.encode('utf-8')
00331 length = len(_x)
00332 buff.write(struct.pack('<I%ss'%length, length, _x))
00333 _v12 = val1.target
00334 _v13 = _v12.header
00335 buff.write(_struct_I.pack(_v13.seq))
00336 _v14 = _v13.stamp
00337 _x = _v14
00338 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00339 _x = _v13.frame_id
00340 length = len(_x)
00341 if python3 or type(_x) == unicode:
00342 _x = _x.encode('utf-8')
00343 length = len(_x)
00344 buff.write(struct.pack('<I%ss'%length, length, _x))
00345 _v15 = _v12.point
00346 _x = _v15
00347 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00348 _v16 = val1.sensor_pose
00349 _v17 = _v16.header
00350 buff.write(_struct_I.pack(_v17.seq))
00351 _v18 = _v17.stamp
00352 _x = _v18
00353 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00354 _x = _v17.frame_id
00355 length = len(_x)
00356 if python3 or type(_x) == unicode:
00357 _x = _x.encode('utf-8')
00358 length = len(_x)
00359 buff.write(struct.pack('<I%ss'%length, length, _x))
00360 _v19 = _v16.pose
00361 _v20 = _v19.position
00362 _x = _v20
00363 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00364 _v21 = _v19.orientation
00365 _x = _v21
00366 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00367 buff.write(_struct_d.pack(val1.absolute_tolerance))
00368 except struct.error as se: self._check_types(se)
00369 except TypeError as te: self._check_types(te)
00370
00371 def deserialize(self, str):
00372 """
00373 unpack serialized message in str into this message instance
00374 :param str: byte array of serialized message, ``str``
00375 """
00376 try:
00377 if self.joint_constraints is None:
00378 self.joint_constraints = None
00379 if self.position_constraints is None:
00380 self.position_constraints = None
00381 if self.orientation_constraints is None:
00382 self.orientation_constraints = None
00383 if self.visibility_constraints is None:
00384 self.visibility_constraints = None
00385 end = 0
00386 start = end
00387 end += 4
00388 (length,) = _struct_I.unpack(str[start:end])
00389 self.joint_constraints = []
00390 for i in range(0, length):
00391 val1 = arm_navigation_msgs.msg.JointConstraint()
00392 start = end
00393 end += 4
00394 (length,) = _struct_I.unpack(str[start:end])
00395 start = end
00396 end += length
00397 if python3:
00398 val1.joint_name = str[start:end].decode('utf-8')
00399 else:
00400 val1.joint_name = str[start:end]
00401 _x = val1
00402 start = end
00403 end += 32
00404 (_x.position, _x.tolerance_above, _x.tolerance_below, _x.weight,) = _struct_4d.unpack(str[start:end])
00405 self.joint_constraints.append(val1)
00406 start = end
00407 end += 4
00408 (length,) = _struct_I.unpack(str[start:end])
00409 self.position_constraints = []
00410 for i in range(0, length):
00411 val1 = arm_navigation_msgs.msg.PositionConstraint()
00412 _v22 = val1.header
00413 start = end
00414 end += 4
00415 (_v22.seq,) = _struct_I.unpack(str[start:end])
00416 _v23 = _v22.stamp
00417 _x = _v23
00418 start = end
00419 end += 8
00420 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00421 start = end
00422 end += 4
00423 (length,) = _struct_I.unpack(str[start:end])
00424 start = end
00425 end += length
00426 if python3:
00427 _v22.frame_id = str[start:end].decode('utf-8')
00428 else:
00429 _v22.frame_id = str[start:end]
00430 start = end
00431 end += 4
00432 (length,) = _struct_I.unpack(str[start:end])
00433 start = end
00434 end += length
00435 if python3:
00436 val1.link_name = str[start:end].decode('utf-8')
00437 else:
00438 val1.link_name = str[start:end]
00439 _v24 = val1.target_point_offset
00440 _x = _v24
00441 start = end
00442 end += 24
00443 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00444 _v25 = val1.position
00445 _x = _v25
00446 start = end
00447 end += 24
00448 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00449 _v26 = val1.constraint_region_shape
00450 start = end
00451 end += 1
00452 (_v26.type,) = _struct_b.unpack(str[start:end])
00453 start = end
00454 end += 4
00455 (length,) = _struct_I.unpack(str[start:end])
00456 pattern = '<%sd'%length
00457 start = end
00458 end += struct.calcsize(pattern)
00459 _v26.dimensions = struct.unpack(pattern, str[start:end])
00460 start = end
00461 end += 4
00462 (length,) = _struct_I.unpack(str[start:end])
00463 pattern = '<%si'%length
00464 start = end
00465 end += struct.calcsize(pattern)
00466 _v26.triangles = struct.unpack(pattern, str[start:end])
00467 start = end
00468 end += 4
00469 (length,) = _struct_I.unpack(str[start:end])
00470 _v26.vertices = []
00471 for i in range(0, length):
00472 val3 = geometry_msgs.msg.Point()
00473 _x = val3
00474 start = end
00475 end += 24
00476 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00477 _v26.vertices.append(val3)
00478 _v27 = val1.constraint_region_orientation
00479 _x = _v27
00480 start = end
00481 end += 32
00482 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
00483 start = end
00484 end += 8
00485 (val1.weight,) = _struct_d.unpack(str[start:end])
00486 self.position_constraints.append(val1)
00487 start = end
00488 end += 4
00489 (length,) = _struct_I.unpack(str[start:end])
00490 self.orientation_constraints = []
00491 for i in range(0, length):
00492 val1 = arm_navigation_msgs.msg.OrientationConstraint()
00493 _v28 = val1.header
00494 start = end
00495 end += 4
00496 (_v28.seq,) = _struct_I.unpack(str[start:end])
00497 _v29 = _v28.stamp
00498 _x = _v29
00499 start = end
00500 end += 8
00501 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00502 start = end
00503 end += 4
00504 (length,) = _struct_I.unpack(str[start:end])
00505 start = end
00506 end += length
00507 if python3:
00508 _v28.frame_id = str[start:end].decode('utf-8')
00509 else:
00510 _v28.frame_id = str[start:end]
00511 start = end
00512 end += 4
00513 (length,) = _struct_I.unpack(str[start:end])
00514 start = end
00515 end += length
00516 if python3:
00517 val1.link_name = str[start:end].decode('utf-8')
00518 else:
00519 val1.link_name = str[start:end]
00520 start = end
00521 end += 4
00522 (val1.type,) = _struct_i.unpack(str[start:end])
00523 _v30 = val1.orientation
00524 _x = _v30
00525 start = end
00526 end += 32
00527 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
00528 _x = val1
00529 start = end
00530 end += 32
00531 (_x.absolute_roll_tolerance, _x.absolute_pitch_tolerance, _x.absolute_yaw_tolerance, _x.weight,) = _struct_4d.unpack(str[start:end])
00532 self.orientation_constraints.append(val1)
00533 start = end
00534 end += 4
00535 (length,) = _struct_I.unpack(str[start:end])
00536 self.visibility_constraints = []
00537 for i in range(0, length):
00538 val1 = arm_navigation_msgs.msg.VisibilityConstraint()
00539 _v31 = val1.header
00540 start = end
00541 end += 4
00542 (_v31.seq,) = _struct_I.unpack(str[start:end])
00543 _v32 = _v31.stamp
00544 _x = _v32
00545 start = end
00546 end += 8
00547 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00548 start = end
00549 end += 4
00550 (length,) = _struct_I.unpack(str[start:end])
00551 start = end
00552 end += length
00553 if python3:
00554 _v31.frame_id = str[start:end].decode('utf-8')
00555 else:
00556 _v31.frame_id = str[start:end]
00557 _v33 = val1.target
00558 _v34 = _v33.header
00559 start = end
00560 end += 4
00561 (_v34.seq,) = _struct_I.unpack(str[start:end])
00562 _v35 = _v34.stamp
00563 _x = _v35
00564 start = end
00565 end += 8
00566 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00567 start = end
00568 end += 4
00569 (length,) = _struct_I.unpack(str[start:end])
00570 start = end
00571 end += length
00572 if python3:
00573 _v34.frame_id = str[start:end].decode('utf-8')
00574 else:
00575 _v34.frame_id = str[start:end]
00576 _v36 = _v33.point
00577 _x = _v36
00578 start = end
00579 end += 24
00580 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00581 _v37 = val1.sensor_pose
00582 _v38 = _v37.header
00583 start = end
00584 end += 4
00585 (_v38.seq,) = _struct_I.unpack(str[start:end])
00586 _v39 = _v38.stamp
00587 _x = _v39
00588 start = end
00589 end += 8
00590 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00591 start = end
00592 end += 4
00593 (length,) = _struct_I.unpack(str[start:end])
00594 start = end
00595 end += length
00596 if python3:
00597 _v38.frame_id = str[start:end].decode('utf-8')
00598 else:
00599 _v38.frame_id = str[start:end]
00600 _v40 = _v37.pose
00601 _v41 = _v40.position
00602 _x = _v41
00603 start = end
00604 end += 24
00605 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00606 _v42 = _v40.orientation
00607 _x = _v42
00608 start = end
00609 end += 32
00610 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
00611 start = end
00612 end += 8
00613 (val1.absolute_tolerance,) = _struct_d.unpack(str[start:end])
00614 self.visibility_constraints.append(val1)
00615 return self
00616 except struct.error as e:
00617 raise genpy.DeserializationError(e)
00618
00619
00620 def serialize_numpy(self, buff, numpy):
00621 """
00622 serialize message with numpy array types into buffer
00623 :param buff: buffer, ``StringIO``
00624 :param numpy: numpy python module
00625 """
00626 try:
00627 length = len(self.joint_constraints)
00628 buff.write(_struct_I.pack(length))
00629 for val1 in self.joint_constraints:
00630 _x = val1.joint_name
00631 length = len(_x)
00632 if python3 or type(_x) == unicode:
00633 _x = _x.encode('utf-8')
00634 length = len(_x)
00635 buff.write(struct.pack('<I%ss'%length, length, _x))
00636 _x = val1
00637 buff.write(_struct_4d.pack(_x.position, _x.tolerance_above, _x.tolerance_below, _x.weight))
00638 length = len(self.position_constraints)
00639 buff.write(_struct_I.pack(length))
00640 for val1 in self.position_constraints:
00641 _v43 = val1.header
00642 buff.write(_struct_I.pack(_v43.seq))
00643 _v44 = _v43.stamp
00644 _x = _v44
00645 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00646 _x = _v43.frame_id
00647 length = len(_x)
00648 if python3 or type(_x) == unicode:
00649 _x = _x.encode('utf-8')
00650 length = len(_x)
00651 buff.write(struct.pack('<I%ss'%length, length, _x))
00652 _x = val1.link_name
00653 length = len(_x)
00654 if python3 or type(_x) == unicode:
00655 _x = _x.encode('utf-8')
00656 length = len(_x)
00657 buff.write(struct.pack('<I%ss'%length, length, _x))
00658 _v45 = val1.target_point_offset
00659 _x = _v45
00660 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00661 _v46 = val1.position
00662 _x = _v46
00663 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00664 _v47 = val1.constraint_region_shape
00665 buff.write(_struct_b.pack(_v47.type))
00666 length = len(_v47.dimensions)
00667 buff.write(_struct_I.pack(length))
00668 pattern = '<%sd'%length
00669 buff.write(_v47.dimensions.tostring())
00670 length = len(_v47.triangles)
00671 buff.write(_struct_I.pack(length))
00672 pattern = '<%si'%length
00673 buff.write(_v47.triangles.tostring())
00674 length = len(_v47.vertices)
00675 buff.write(_struct_I.pack(length))
00676 for val3 in _v47.vertices:
00677 _x = val3
00678 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00679 _v48 = val1.constraint_region_orientation
00680 _x = _v48
00681 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00682 buff.write(_struct_d.pack(val1.weight))
00683 length = len(self.orientation_constraints)
00684 buff.write(_struct_I.pack(length))
00685 for val1 in self.orientation_constraints:
00686 _v49 = val1.header
00687 buff.write(_struct_I.pack(_v49.seq))
00688 _v50 = _v49.stamp
00689 _x = _v50
00690 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00691 _x = _v49.frame_id
00692 length = len(_x)
00693 if python3 or type(_x) == unicode:
00694 _x = _x.encode('utf-8')
00695 length = len(_x)
00696 buff.write(struct.pack('<I%ss'%length, length, _x))
00697 _x = val1.link_name
00698 length = len(_x)
00699 if python3 or type(_x) == unicode:
00700 _x = _x.encode('utf-8')
00701 length = len(_x)
00702 buff.write(struct.pack('<I%ss'%length, length, _x))
00703 buff.write(_struct_i.pack(val1.type))
00704 _v51 = val1.orientation
00705 _x = _v51
00706 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00707 _x = val1
00708 buff.write(_struct_4d.pack(_x.absolute_roll_tolerance, _x.absolute_pitch_tolerance, _x.absolute_yaw_tolerance, _x.weight))
00709 length = len(self.visibility_constraints)
00710 buff.write(_struct_I.pack(length))
00711 for val1 in self.visibility_constraints:
00712 _v52 = val1.header
00713 buff.write(_struct_I.pack(_v52.seq))
00714 _v53 = _v52.stamp
00715 _x = _v53
00716 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00717 _x = _v52.frame_id
00718 length = len(_x)
00719 if python3 or type(_x) == unicode:
00720 _x = _x.encode('utf-8')
00721 length = len(_x)
00722 buff.write(struct.pack('<I%ss'%length, length, _x))
00723 _v54 = val1.target
00724 _v55 = _v54.header
00725 buff.write(_struct_I.pack(_v55.seq))
00726 _v56 = _v55.stamp
00727 _x = _v56
00728 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00729 _x = _v55.frame_id
00730 length = len(_x)
00731 if python3 or type(_x) == unicode:
00732 _x = _x.encode('utf-8')
00733 length = len(_x)
00734 buff.write(struct.pack('<I%ss'%length, length, _x))
00735 _v57 = _v54.point
00736 _x = _v57
00737 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00738 _v58 = val1.sensor_pose
00739 _v59 = _v58.header
00740 buff.write(_struct_I.pack(_v59.seq))
00741 _v60 = _v59.stamp
00742 _x = _v60
00743 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00744 _x = _v59.frame_id
00745 length = len(_x)
00746 if python3 or type(_x) == unicode:
00747 _x = _x.encode('utf-8')
00748 length = len(_x)
00749 buff.write(struct.pack('<I%ss'%length, length, _x))
00750 _v61 = _v58.pose
00751 _v62 = _v61.position
00752 _x = _v62
00753 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00754 _v63 = _v61.orientation
00755 _x = _v63
00756 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00757 buff.write(_struct_d.pack(val1.absolute_tolerance))
00758 except struct.error as se: self._check_types(se)
00759 except TypeError as te: self._check_types(te)
00760
00761 def deserialize_numpy(self, str, numpy):
00762 """
00763 unpack serialized message in str into this message instance using numpy for array types
00764 :param str: byte array of serialized message, ``str``
00765 :param numpy: numpy python module
00766 """
00767 try:
00768 if self.joint_constraints is None:
00769 self.joint_constraints = None
00770 if self.position_constraints is None:
00771 self.position_constraints = None
00772 if self.orientation_constraints is None:
00773 self.orientation_constraints = None
00774 if self.visibility_constraints is None:
00775 self.visibility_constraints = None
00776 end = 0
00777 start = end
00778 end += 4
00779 (length,) = _struct_I.unpack(str[start:end])
00780 self.joint_constraints = []
00781 for i in range(0, length):
00782 val1 = arm_navigation_msgs.msg.JointConstraint()
00783 start = end
00784 end += 4
00785 (length,) = _struct_I.unpack(str[start:end])
00786 start = end
00787 end += length
00788 if python3:
00789 val1.joint_name = str[start:end].decode('utf-8')
00790 else:
00791 val1.joint_name = str[start:end]
00792 _x = val1
00793 start = end
00794 end += 32
00795 (_x.position, _x.tolerance_above, _x.tolerance_below, _x.weight,) = _struct_4d.unpack(str[start:end])
00796 self.joint_constraints.append(val1)
00797 start = end
00798 end += 4
00799 (length,) = _struct_I.unpack(str[start:end])
00800 self.position_constraints = []
00801 for i in range(0, length):
00802 val1 = arm_navigation_msgs.msg.PositionConstraint()
00803 _v64 = val1.header
00804 start = end
00805 end += 4
00806 (_v64.seq,) = _struct_I.unpack(str[start:end])
00807 _v65 = _v64.stamp
00808 _x = _v65
00809 start = end
00810 end += 8
00811 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00812 start = end
00813 end += 4
00814 (length,) = _struct_I.unpack(str[start:end])
00815 start = end
00816 end += length
00817 if python3:
00818 _v64.frame_id = str[start:end].decode('utf-8')
00819 else:
00820 _v64.frame_id = str[start:end]
00821 start = end
00822 end += 4
00823 (length,) = _struct_I.unpack(str[start:end])
00824 start = end
00825 end += length
00826 if python3:
00827 val1.link_name = str[start:end].decode('utf-8')
00828 else:
00829 val1.link_name = str[start:end]
00830 _v66 = val1.target_point_offset
00831 _x = _v66
00832 start = end
00833 end += 24
00834 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00835 _v67 = val1.position
00836 _x = _v67
00837 start = end
00838 end += 24
00839 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00840 _v68 = val1.constraint_region_shape
00841 start = end
00842 end += 1
00843 (_v68.type,) = _struct_b.unpack(str[start:end])
00844 start = end
00845 end += 4
00846 (length,) = _struct_I.unpack(str[start:end])
00847 pattern = '<%sd'%length
00848 start = end
00849 end += struct.calcsize(pattern)
00850 _v68.dimensions = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
00851 start = end
00852 end += 4
00853 (length,) = _struct_I.unpack(str[start:end])
00854 pattern = '<%si'%length
00855 start = end
00856 end += struct.calcsize(pattern)
00857 _v68.triangles = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
00858 start = end
00859 end += 4
00860 (length,) = _struct_I.unpack(str[start:end])
00861 _v68.vertices = []
00862 for i in range(0, length):
00863 val3 = geometry_msgs.msg.Point()
00864 _x = val3
00865 start = end
00866 end += 24
00867 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00868 _v68.vertices.append(val3)
00869 _v69 = val1.constraint_region_orientation
00870 _x = _v69
00871 start = end
00872 end += 32
00873 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
00874 start = end
00875 end += 8
00876 (val1.weight,) = _struct_d.unpack(str[start:end])
00877 self.position_constraints.append(val1)
00878 start = end
00879 end += 4
00880 (length,) = _struct_I.unpack(str[start:end])
00881 self.orientation_constraints = []
00882 for i in range(0, length):
00883 val1 = arm_navigation_msgs.msg.OrientationConstraint()
00884 _v70 = val1.header
00885 start = end
00886 end += 4
00887 (_v70.seq,) = _struct_I.unpack(str[start:end])
00888 _v71 = _v70.stamp
00889 _x = _v71
00890 start = end
00891 end += 8
00892 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00893 start = end
00894 end += 4
00895 (length,) = _struct_I.unpack(str[start:end])
00896 start = end
00897 end += length
00898 if python3:
00899 _v70.frame_id = str[start:end].decode('utf-8')
00900 else:
00901 _v70.frame_id = str[start:end]
00902 start = end
00903 end += 4
00904 (length,) = _struct_I.unpack(str[start:end])
00905 start = end
00906 end += length
00907 if python3:
00908 val1.link_name = str[start:end].decode('utf-8')
00909 else:
00910 val1.link_name = str[start:end]
00911 start = end
00912 end += 4
00913 (val1.type,) = _struct_i.unpack(str[start:end])
00914 _v72 = val1.orientation
00915 _x = _v72
00916 start = end
00917 end += 32
00918 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
00919 _x = val1
00920 start = end
00921 end += 32
00922 (_x.absolute_roll_tolerance, _x.absolute_pitch_tolerance, _x.absolute_yaw_tolerance, _x.weight,) = _struct_4d.unpack(str[start:end])
00923 self.orientation_constraints.append(val1)
00924 start = end
00925 end += 4
00926 (length,) = _struct_I.unpack(str[start:end])
00927 self.visibility_constraints = []
00928 for i in range(0, length):
00929 val1 = arm_navigation_msgs.msg.VisibilityConstraint()
00930 _v73 = val1.header
00931 start = end
00932 end += 4
00933 (_v73.seq,) = _struct_I.unpack(str[start:end])
00934 _v74 = _v73.stamp
00935 _x = _v74
00936 start = end
00937 end += 8
00938 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00939 start = end
00940 end += 4
00941 (length,) = _struct_I.unpack(str[start:end])
00942 start = end
00943 end += length
00944 if python3:
00945 _v73.frame_id = str[start:end].decode('utf-8')
00946 else:
00947 _v73.frame_id = str[start:end]
00948 _v75 = val1.target
00949 _v76 = _v75.header
00950 start = end
00951 end += 4
00952 (_v76.seq,) = _struct_I.unpack(str[start:end])
00953 _v77 = _v76.stamp
00954 _x = _v77
00955 start = end
00956 end += 8
00957 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00958 start = end
00959 end += 4
00960 (length,) = _struct_I.unpack(str[start:end])
00961 start = end
00962 end += length
00963 if python3:
00964 _v76.frame_id = str[start:end].decode('utf-8')
00965 else:
00966 _v76.frame_id = str[start:end]
00967 _v78 = _v75.point
00968 _x = _v78
00969 start = end
00970 end += 24
00971 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00972 _v79 = val1.sensor_pose
00973 _v80 = _v79.header
00974 start = end
00975 end += 4
00976 (_v80.seq,) = _struct_I.unpack(str[start:end])
00977 _v81 = _v80.stamp
00978 _x = _v81
00979 start = end
00980 end += 8
00981 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00982 start = end
00983 end += 4
00984 (length,) = _struct_I.unpack(str[start:end])
00985 start = end
00986 end += length
00987 if python3:
00988 _v80.frame_id = str[start:end].decode('utf-8')
00989 else:
00990 _v80.frame_id = str[start:end]
00991 _v82 = _v79.pose
00992 _v83 = _v82.position
00993 _x = _v83
00994 start = end
00995 end += 24
00996 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00997 _v84 = _v82.orientation
00998 _x = _v84
00999 start = end
01000 end += 32
01001 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01002 start = end
01003 end += 8
01004 (val1.absolute_tolerance,) = _struct_d.unpack(str[start:end])
01005 self.visibility_constraints.append(val1)
01006 return self
01007 except struct.error as e:
01008 raise genpy.DeserializationError(e)
01009
01010 _struct_I = genpy.struct_I
01011 _struct_b = struct.Struct("<b")
01012 _struct_d = struct.Struct("<d")
01013 _struct_i = struct.Struct("<i")
01014 _struct_4d = struct.Struct("<4d")
01015 _struct_2I = struct.Struct("<2I")
01016 _struct_3d = struct.Struct("<3d")