_GetRobotTrajectoryValidity.py
Go to the documentation of this file.
00001 """autogenerated by genpy from arm_navigation_msgs/GetRobotTrajectoryValidityRequest.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 trajectory_msgs.msg
00009 import geometry_msgs.msg
00010 import sensor_msgs.msg
00011 import genpy
00012 import std_msgs.msg
00013 
00014 class GetRobotTrajectoryValidityRequest(genpy.Message):
00015   _md5sum = "85259de614b32f9acd26c11efb8e55ca"
00016   _type = "arm_navigation_msgs/GetRobotTrajectoryValidityRequest"
00017   _has_header = False #flag to mark the presence of a Header object
00018   _full_text = """
00019 arm_navigation_msgs/RobotTrajectory trajectory
00020 
00021 
00022 
00023 
00024 
00025 
00026 
00027 arm_navigation_msgs/RobotState robot_state
00028 
00029 
00030 bool check_collisions
00031 
00032 
00033 bool check_path_constraints
00034 
00035 
00036 bool check_goal_constraints
00037 
00038 
00039 bool check_joint_limits
00040 
00041 
00042 
00043 
00044 bool check_full_trajectory
00045 
00046 
00047 
00048 
00049 arm_navigation_msgs/Constraints path_constraints
00050 
00051 
00052 
00053 
00054 arm_navigation_msgs/Constraints goal_constraints
00055 
00056 
00057 ================================================================================
00058 MSG: arm_navigation_msgs/RobotTrajectory
00059 trajectory_msgs/JointTrajectory joint_trajectory
00060 arm_navigation_msgs/MultiDOFJointTrajectory multi_dof_joint_trajectory
00061 
00062 ================================================================================
00063 MSG: trajectory_msgs/JointTrajectory
00064 Header header
00065 string[] joint_names
00066 JointTrajectoryPoint[] points
00067 ================================================================================
00068 MSG: std_msgs/Header
00069 # Standard metadata for higher-level stamped data types.
00070 # This is generally used to communicate timestamped data 
00071 # in a particular coordinate frame.
00072 # 
00073 # sequence ID: consecutively increasing ID 
00074 uint32 seq
00075 #Two-integer timestamp that is expressed as:
00076 # * stamp.secs: seconds (stamp_secs) since epoch
00077 # * stamp.nsecs: nanoseconds since stamp_secs
00078 # time-handling sugar is provided by the client library
00079 time stamp
00080 #Frame this data is associated with
00081 # 0: no frame
00082 # 1: global frame
00083 string frame_id
00084 
00085 ================================================================================
00086 MSG: trajectory_msgs/JointTrajectoryPoint
00087 float64[] positions
00088 float64[] velocities
00089 float64[] accelerations
00090 duration time_from_start
00091 ================================================================================
00092 MSG: arm_navigation_msgs/MultiDOFJointTrajectory
00093 #A representation of a multi-dof joint trajectory
00094 duration stamp
00095 string[] joint_names
00096 string[] frame_ids
00097 string[] child_frame_ids
00098 MultiDOFJointTrajectoryPoint[] points
00099 
00100 ================================================================================
00101 MSG: arm_navigation_msgs/MultiDOFJointTrajectoryPoint
00102 geometry_msgs/Pose[] poses
00103 duration time_from_start
00104 ================================================================================
00105 MSG: geometry_msgs/Pose
00106 # A representation of pose in free space, composed of postion and orientation. 
00107 Point position
00108 Quaternion orientation
00109 
00110 ================================================================================
00111 MSG: geometry_msgs/Point
00112 # This contains the position of a point in free space
00113 float64 x
00114 float64 y
00115 float64 z
00116 
00117 ================================================================================
00118 MSG: geometry_msgs/Quaternion
00119 # This represents an orientation in free space in quaternion form.
00120 
00121 float64 x
00122 float64 y
00123 float64 z
00124 float64 w
00125 
00126 ================================================================================
00127 MSG: arm_navigation_msgs/RobotState
00128 # This message contains information about the robot state, i.e. the positions of its joints and links
00129 sensor_msgs/JointState joint_state
00130 arm_navigation_msgs/MultiDOFJointState multi_dof_joint_state
00131 
00132 ================================================================================
00133 MSG: sensor_msgs/JointState
00134 # This is a message that holds data to describe the state of a set of torque controlled joints. 
00135 #
00136 # The state of each joint (revolute or prismatic) is defined by:
00137 #  * the position of the joint (rad or m),
00138 #  * the velocity of the joint (rad/s or m/s) and 
00139 #  * the effort that is applied in the joint (Nm or N).
00140 #
00141 # Each joint is uniquely identified by its name
00142 # The header specifies the time at which the joint states were recorded. All the joint states
00143 # in one message have to be recorded at the same time.
00144 #
00145 # This message consists of a multiple arrays, one for each part of the joint state. 
00146 # The goal is to make each of the fields optional. When e.g. your joints have no
00147 # effort associated with them, you can leave the effort array empty. 
00148 #
00149 # All arrays in this message should have the same size, or be empty.
00150 # This is the only way to uniquely associate the joint name with the correct
00151 # states.
00152 
00153 
00154 Header header
00155 
00156 string[] name
00157 float64[] position
00158 float64[] velocity
00159 float64[] effort
00160 
00161 ================================================================================
00162 MSG: arm_navigation_msgs/MultiDOFJointState
00163 #A representation of a multi-dof joint state
00164 time stamp
00165 string[] joint_names
00166 string[] frame_ids
00167 string[] child_frame_ids
00168 geometry_msgs/Pose[] poses
00169 
00170 ================================================================================
00171 MSG: arm_navigation_msgs/Constraints
00172 # This message contains a list of motion planning constraints.
00173 
00174 arm_navigation_msgs/JointConstraint[] joint_constraints
00175 arm_navigation_msgs/PositionConstraint[] position_constraints
00176 arm_navigation_msgs/OrientationConstraint[] orientation_constraints
00177 arm_navigation_msgs/VisibilityConstraint[] visibility_constraints
00178 
00179 ================================================================================
00180 MSG: arm_navigation_msgs/JointConstraint
00181 # Constrain the position of a joint to be within a certain bound
00182 string joint_name
00183 
00184 # the bound to be achieved is [position - tolerance_below, position + tolerance_above]
00185 float64 position
00186 float64 tolerance_above
00187 float64 tolerance_below
00188 
00189 # A weighting factor for this constraint
00190 float64 weight
00191 ================================================================================
00192 MSG: arm_navigation_msgs/PositionConstraint
00193 # This message contains the definition of a position constraint.
00194 Header header
00195 
00196 # The robot link this constraint refers to
00197 string link_name
00198 
00199 # The offset (in the link frame) for the target point on the link we are planning for
00200 geometry_msgs/Point target_point_offset
00201 
00202 # The nominal/target position for the point we are planning for
00203 geometry_msgs/Point position
00204 
00205 # The shape of the bounded region that constrains the position of the end-effector
00206 # This region is always centered at the position defined above
00207 arm_navigation_msgs/Shape constraint_region_shape
00208 
00209 # The orientation of the bounded region that constrains the position of the end-effector. 
00210 # This allows the specification of non-axis aligned constraints
00211 geometry_msgs/Quaternion constraint_region_orientation
00212 
00213 # Constraint weighting factor - a weight for this constraint
00214 float64 weight
00215 
00216 ================================================================================
00217 MSG: arm_navigation_msgs/Shape
00218 byte SPHERE=0
00219 byte BOX=1
00220 byte CYLINDER=2
00221 byte MESH=3
00222 
00223 byte type
00224 
00225 
00226 #### define sphere, box, cylinder ####
00227 # the origin of each shape is considered at the shape's center
00228 
00229 # for sphere
00230 # radius := dimensions[0]
00231 
00232 # for cylinder
00233 # radius := dimensions[0]
00234 # length := dimensions[1]
00235 # the length is along the Z axis
00236 
00237 # for box
00238 # size_x := dimensions[0]
00239 # size_y := dimensions[1]
00240 # size_z := dimensions[2]
00241 float64[] dimensions
00242 
00243 
00244 #### define mesh ####
00245 
00246 # list of triangles; triangle k is defined by tre vertices located
00247 # at indices triangles[3k], triangles[3k+1], triangles[3k+2]
00248 int32[] triangles
00249 geometry_msgs/Point[] vertices
00250 
00251 ================================================================================
00252 MSG: arm_navigation_msgs/OrientationConstraint
00253 # This message contains the definition of an orientation constraint.
00254 Header header
00255 
00256 # The robot link this constraint refers to
00257 string link_name
00258 
00259 # The type of the constraint
00260 int32 type
00261 int32 LINK_FRAME=0
00262 int32 HEADER_FRAME=1
00263 
00264 # The desired orientation of the robot link specified as a quaternion
00265 geometry_msgs/Quaternion orientation
00266 
00267 # optional RPY error tolerances specified if 
00268 float64 absolute_roll_tolerance
00269 float64 absolute_pitch_tolerance
00270 float64 absolute_yaw_tolerance
00271 
00272 # Constraint weighting factor - a weight for this constraint
00273 float64 weight
00274 
00275 ================================================================================
00276 MSG: arm_navigation_msgs/VisibilityConstraint
00277 # This message contains the definition of a visibility constraint.
00278 Header header
00279 
00280 # The point stamped target that needs to be kept within view of the sensor
00281 geometry_msgs/PointStamped target
00282 
00283 # The local pose of the frame in which visibility is to be maintained
00284 # The frame id should represent the robot link to which the sensor is attached
00285 # The visual axis of the sensor is assumed to be along the X axis of this frame
00286 geometry_msgs/PoseStamped sensor_pose
00287 
00288 # The deviation (in radians) that will be tolerated
00289 # Constraint error will be measured as the solid angle between the 
00290 # X axis of the frame defined above and the vector between the origin 
00291 # of the frame defined above and the target location
00292 float64 absolute_tolerance
00293 
00294 
00295 ================================================================================
00296 MSG: geometry_msgs/PointStamped
00297 # This represents a Point with reference coordinate frame and timestamp
00298 Header header
00299 Point point
00300 
00301 ================================================================================
00302 MSG: geometry_msgs/PoseStamped
00303 # A Pose with reference coordinate frame and timestamp
00304 Header header
00305 Pose pose
00306 
00307 """
00308   __slots__ = ['trajectory','robot_state','check_collisions','check_path_constraints','check_goal_constraints','check_joint_limits','check_full_trajectory','path_constraints','goal_constraints']
00309   _slot_types = ['arm_navigation_msgs/RobotTrajectory','arm_navigation_msgs/RobotState','bool','bool','bool','bool','bool','arm_navigation_msgs/Constraints','arm_navigation_msgs/Constraints']
00310 
00311   def __init__(self, *args, **kwds):
00312     """
00313     Constructor. Any message fields that are implicitly/explicitly
00314     set to None will be assigned a default value. The recommend
00315     use is keyword arguments as this is more robust to future message
00316     changes.  You cannot mix in-order arguments and keyword arguments.
00317 
00318     The available fields are:
00319        trajectory,robot_state,check_collisions,check_path_constraints,check_goal_constraints,check_joint_limits,check_full_trajectory,path_constraints,goal_constraints
00320 
00321     :param args: complete set of field values, in .msg order
00322     :param kwds: use keyword arguments corresponding to message field names
00323     to set specific fields.
00324     """
00325     if args or kwds:
00326       super(GetRobotTrajectoryValidityRequest, self).__init__(*args, **kwds)
00327       #message fields cannot be None, assign default values for those that are
00328       if self.trajectory is None:
00329         self.trajectory = arm_navigation_msgs.msg.RobotTrajectory()
00330       if self.robot_state is None:
00331         self.robot_state = arm_navigation_msgs.msg.RobotState()
00332       if self.check_collisions is None:
00333         self.check_collisions = False
00334       if self.check_path_constraints is None:
00335         self.check_path_constraints = False
00336       if self.check_goal_constraints is None:
00337         self.check_goal_constraints = False
00338       if self.check_joint_limits is None:
00339         self.check_joint_limits = False
00340       if self.check_full_trajectory is None:
00341         self.check_full_trajectory = False
00342       if self.path_constraints is None:
00343         self.path_constraints = arm_navigation_msgs.msg.Constraints()
00344       if self.goal_constraints is None:
00345         self.goal_constraints = arm_navigation_msgs.msg.Constraints()
00346     else:
00347       self.trajectory = arm_navigation_msgs.msg.RobotTrajectory()
00348       self.robot_state = arm_navigation_msgs.msg.RobotState()
00349       self.check_collisions = False
00350       self.check_path_constraints = False
00351       self.check_goal_constraints = False
00352       self.check_joint_limits = False
00353       self.check_full_trajectory = False
00354       self.path_constraints = arm_navigation_msgs.msg.Constraints()
00355       self.goal_constraints = arm_navigation_msgs.msg.Constraints()
00356 
00357   def _get_types(self):
00358     """
00359     internal API method
00360     """
00361     return self._slot_types
00362 
00363   def serialize(self, buff):
00364     """
00365     serialize message into buffer
00366     :param buff: buffer, ``StringIO``
00367     """
00368     try:
00369       _x = self
00370       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))
00371       _x = self.trajectory.joint_trajectory.header.frame_id
00372       length = len(_x)
00373       if python3 or type(_x) == unicode:
00374         _x = _x.encode('utf-8')
00375         length = len(_x)
00376       buff.write(struct.pack('<I%ss'%length, length, _x))
00377       length = len(self.trajectory.joint_trajectory.joint_names)
00378       buff.write(_struct_I.pack(length))
00379       for val1 in self.trajectory.joint_trajectory.joint_names:
00380         length = len(val1)
00381         if python3 or type(val1) == unicode:
00382           val1 = val1.encode('utf-8')
00383           length = len(val1)
00384         buff.write(struct.pack('<I%ss'%length, length, val1))
00385       length = len(self.trajectory.joint_trajectory.points)
00386       buff.write(_struct_I.pack(length))
00387       for val1 in self.trajectory.joint_trajectory.points:
00388         length = len(val1.positions)
00389         buff.write(_struct_I.pack(length))
00390         pattern = '<%sd'%length
00391         buff.write(struct.pack(pattern, *val1.positions))
00392         length = len(val1.velocities)
00393         buff.write(_struct_I.pack(length))
00394         pattern = '<%sd'%length
00395         buff.write(struct.pack(pattern, *val1.velocities))
00396         length = len(val1.accelerations)
00397         buff.write(_struct_I.pack(length))
00398         pattern = '<%sd'%length
00399         buff.write(struct.pack(pattern, *val1.accelerations))
00400         _v1 = val1.time_from_start
00401         _x = _v1
00402         buff.write(_struct_2i.pack(_x.secs, _x.nsecs))
00403       _x = self
00404       buff.write(_struct_2i.pack(_x.trajectory.multi_dof_joint_trajectory.stamp.secs, _x.trajectory.multi_dof_joint_trajectory.stamp.nsecs))
00405       length = len(self.trajectory.multi_dof_joint_trajectory.joint_names)
00406       buff.write(_struct_I.pack(length))
00407       for val1 in self.trajectory.multi_dof_joint_trajectory.joint_names:
00408         length = len(val1)
00409         if python3 or type(val1) == unicode:
00410           val1 = val1.encode('utf-8')
00411           length = len(val1)
00412         buff.write(struct.pack('<I%ss'%length, length, val1))
00413       length = len(self.trajectory.multi_dof_joint_trajectory.frame_ids)
00414       buff.write(_struct_I.pack(length))
00415       for val1 in self.trajectory.multi_dof_joint_trajectory.frame_ids:
00416         length = len(val1)
00417         if python3 or type(val1) == unicode:
00418           val1 = val1.encode('utf-8')
00419           length = len(val1)
00420         buff.write(struct.pack('<I%ss'%length, length, val1))
00421       length = len(self.trajectory.multi_dof_joint_trajectory.child_frame_ids)
00422       buff.write(_struct_I.pack(length))
00423       for val1 in self.trajectory.multi_dof_joint_trajectory.child_frame_ids:
00424         length = len(val1)
00425         if python3 or type(val1) == unicode:
00426           val1 = val1.encode('utf-8')
00427           length = len(val1)
00428         buff.write(struct.pack('<I%ss'%length, length, val1))
00429       length = len(self.trajectory.multi_dof_joint_trajectory.points)
00430       buff.write(_struct_I.pack(length))
00431       for val1 in self.trajectory.multi_dof_joint_trajectory.points:
00432         length = len(val1.poses)
00433         buff.write(_struct_I.pack(length))
00434         for val2 in val1.poses:
00435           _v2 = val2.position
00436           _x = _v2
00437           buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00438           _v3 = val2.orientation
00439           _x = _v3
00440           buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00441         _v4 = val1.time_from_start
00442         _x = _v4
00443         buff.write(_struct_2i.pack(_x.secs, _x.nsecs))
00444       _x = self
00445       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))
00446       _x = self.robot_state.joint_state.header.frame_id
00447       length = len(_x)
00448       if python3 or type(_x) == unicode:
00449         _x = _x.encode('utf-8')
00450         length = len(_x)
00451       buff.write(struct.pack('<I%ss'%length, length, _x))
00452       length = len(self.robot_state.joint_state.name)
00453       buff.write(_struct_I.pack(length))
00454       for val1 in self.robot_state.joint_state.name:
00455         length = len(val1)
00456         if python3 or type(val1) == unicode:
00457           val1 = val1.encode('utf-8')
00458           length = len(val1)
00459         buff.write(struct.pack('<I%ss'%length, length, val1))
00460       length = len(self.robot_state.joint_state.position)
00461       buff.write(_struct_I.pack(length))
00462       pattern = '<%sd'%length
00463       buff.write(struct.pack(pattern, *self.robot_state.joint_state.position))
00464       length = len(self.robot_state.joint_state.velocity)
00465       buff.write(_struct_I.pack(length))
00466       pattern = '<%sd'%length
00467       buff.write(struct.pack(pattern, *self.robot_state.joint_state.velocity))
00468       length = len(self.robot_state.joint_state.effort)
00469       buff.write(_struct_I.pack(length))
00470       pattern = '<%sd'%length
00471       buff.write(struct.pack(pattern, *self.robot_state.joint_state.effort))
00472       _x = self
00473       buff.write(_struct_2I.pack(_x.robot_state.multi_dof_joint_state.stamp.secs, _x.robot_state.multi_dof_joint_state.stamp.nsecs))
00474       length = len(self.robot_state.multi_dof_joint_state.joint_names)
00475       buff.write(_struct_I.pack(length))
00476       for val1 in self.robot_state.multi_dof_joint_state.joint_names:
00477         length = len(val1)
00478         if python3 or type(val1) == unicode:
00479           val1 = val1.encode('utf-8')
00480           length = len(val1)
00481         buff.write(struct.pack('<I%ss'%length, length, val1))
00482       length = len(self.robot_state.multi_dof_joint_state.frame_ids)
00483       buff.write(_struct_I.pack(length))
00484       for val1 in self.robot_state.multi_dof_joint_state.frame_ids:
00485         length = len(val1)
00486         if python3 or type(val1) == unicode:
00487           val1 = val1.encode('utf-8')
00488           length = len(val1)
00489         buff.write(struct.pack('<I%ss'%length, length, val1))
00490       length = len(self.robot_state.multi_dof_joint_state.child_frame_ids)
00491       buff.write(_struct_I.pack(length))
00492       for val1 in self.robot_state.multi_dof_joint_state.child_frame_ids:
00493         length = len(val1)
00494         if python3 or type(val1) == unicode:
00495           val1 = val1.encode('utf-8')
00496           length = len(val1)
00497         buff.write(struct.pack('<I%ss'%length, length, val1))
00498       length = len(self.robot_state.multi_dof_joint_state.poses)
00499       buff.write(_struct_I.pack(length))
00500       for val1 in self.robot_state.multi_dof_joint_state.poses:
00501         _v5 = val1.position
00502         _x = _v5
00503         buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00504         _v6 = val1.orientation
00505         _x = _v6
00506         buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00507       _x = self
00508       buff.write(_struct_5B.pack(_x.check_collisions, _x.check_path_constraints, _x.check_goal_constraints, _x.check_joint_limits, _x.check_full_trajectory))
00509       length = len(self.path_constraints.joint_constraints)
00510       buff.write(_struct_I.pack(length))
00511       for val1 in self.path_constraints.joint_constraints:
00512         _x = val1.joint_name
00513         length = len(_x)
00514         if python3 or type(_x) == unicode:
00515           _x = _x.encode('utf-8')
00516           length = len(_x)
00517         buff.write(struct.pack('<I%ss'%length, length, _x))
00518         _x = val1
00519         buff.write(_struct_4d.pack(_x.position, _x.tolerance_above, _x.tolerance_below, _x.weight))
00520       length = len(self.path_constraints.position_constraints)
00521       buff.write(_struct_I.pack(length))
00522       for val1 in self.path_constraints.position_constraints:
00523         _v7 = val1.header
00524         buff.write(_struct_I.pack(_v7.seq))
00525         _v8 = _v7.stamp
00526         _x = _v8
00527         buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00528         _x = _v7.frame_id
00529         length = len(_x)
00530         if python3 or type(_x) == unicode:
00531           _x = _x.encode('utf-8')
00532           length = len(_x)
00533         buff.write(struct.pack('<I%ss'%length, length, _x))
00534         _x = val1.link_name
00535         length = len(_x)
00536         if python3 or type(_x) == unicode:
00537           _x = _x.encode('utf-8')
00538           length = len(_x)
00539         buff.write(struct.pack('<I%ss'%length, length, _x))
00540         _v9 = val1.target_point_offset
00541         _x = _v9
00542         buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00543         _v10 = val1.position
00544         _x = _v10
00545         buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00546         _v11 = val1.constraint_region_shape
00547         buff.write(_struct_b.pack(_v11.type))
00548         length = len(_v11.dimensions)
00549         buff.write(_struct_I.pack(length))
00550         pattern = '<%sd'%length
00551         buff.write(struct.pack(pattern, *_v11.dimensions))
00552         length = len(_v11.triangles)
00553         buff.write(_struct_I.pack(length))
00554         pattern = '<%si'%length
00555         buff.write(struct.pack(pattern, *_v11.triangles))
00556         length = len(_v11.vertices)
00557         buff.write(_struct_I.pack(length))
00558         for val3 in _v11.vertices:
00559           _x = val3
00560           buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00561         _v12 = val1.constraint_region_orientation
00562         _x = _v12
00563         buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00564         buff.write(_struct_d.pack(val1.weight))
00565       length = len(self.path_constraints.orientation_constraints)
00566       buff.write(_struct_I.pack(length))
00567       for val1 in self.path_constraints.orientation_constraints:
00568         _v13 = val1.header
00569         buff.write(_struct_I.pack(_v13.seq))
00570         _v14 = _v13.stamp
00571         _x = _v14
00572         buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00573         _x = _v13.frame_id
00574         length = len(_x)
00575         if python3 or type(_x) == unicode:
00576           _x = _x.encode('utf-8')
00577           length = len(_x)
00578         buff.write(struct.pack('<I%ss'%length, length, _x))
00579         _x = val1.link_name
00580         length = len(_x)
00581         if python3 or type(_x) == unicode:
00582           _x = _x.encode('utf-8')
00583           length = len(_x)
00584         buff.write(struct.pack('<I%ss'%length, length, _x))
00585         buff.write(_struct_i.pack(val1.type))
00586         _v15 = val1.orientation
00587         _x = _v15
00588         buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00589         _x = val1
00590         buff.write(_struct_4d.pack(_x.absolute_roll_tolerance, _x.absolute_pitch_tolerance, _x.absolute_yaw_tolerance, _x.weight))
00591       length = len(self.path_constraints.visibility_constraints)
00592       buff.write(_struct_I.pack(length))
00593       for val1 in self.path_constraints.visibility_constraints:
00594         _v16 = val1.header
00595         buff.write(_struct_I.pack(_v16.seq))
00596         _v17 = _v16.stamp
00597         _x = _v17
00598         buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00599         _x = _v16.frame_id
00600         length = len(_x)
00601         if python3 or type(_x) == unicode:
00602           _x = _x.encode('utf-8')
00603           length = len(_x)
00604         buff.write(struct.pack('<I%ss'%length, length, _x))
00605         _v18 = val1.target
00606         _v19 = _v18.header
00607         buff.write(_struct_I.pack(_v19.seq))
00608         _v20 = _v19.stamp
00609         _x = _v20
00610         buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00611         _x = _v19.frame_id
00612         length = len(_x)
00613         if python3 or type(_x) == unicode:
00614           _x = _x.encode('utf-8')
00615           length = len(_x)
00616         buff.write(struct.pack('<I%ss'%length, length, _x))
00617         _v21 = _v18.point
00618         _x = _v21
00619         buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00620         _v22 = val1.sensor_pose
00621         _v23 = _v22.header
00622         buff.write(_struct_I.pack(_v23.seq))
00623         _v24 = _v23.stamp
00624         _x = _v24
00625         buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00626         _x = _v23.frame_id
00627         length = len(_x)
00628         if python3 or type(_x) == unicode:
00629           _x = _x.encode('utf-8')
00630           length = len(_x)
00631         buff.write(struct.pack('<I%ss'%length, length, _x))
00632         _v25 = _v22.pose
00633         _v26 = _v25.position
00634         _x = _v26
00635         buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00636         _v27 = _v25.orientation
00637         _x = _v27
00638         buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00639         buff.write(_struct_d.pack(val1.absolute_tolerance))
00640       length = len(self.goal_constraints.joint_constraints)
00641       buff.write(_struct_I.pack(length))
00642       for val1 in self.goal_constraints.joint_constraints:
00643         _x = val1.joint_name
00644         length = len(_x)
00645         if python3 or type(_x) == unicode:
00646           _x = _x.encode('utf-8')
00647           length = len(_x)
00648         buff.write(struct.pack('<I%ss'%length, length, _x))
00649         _x = val1
00650         buff.write(_struct_4d.pack(_x.position, _x.tolerance_above, _x.tolerance_below, _x.weight))
00651       length = len(self.goal_constraints.position_constraints)
00652       buff.write(_struct_I.pack(length))
00653       for val1 in self.goal_constraints.position_constraints:
00654         _v28 = val1.header
00655         buff.write(_struct_I.pack(_v28.seq))
00656         _v29 = _v28.stamp
00657         _x = _v29
00658         buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00659         _x = _v28.frame_id
00660         length = len(_x)
00661         if python3 or type(_x) == unicode:
00662           _x = _x.encode('utf-8')
00663           length = len(_x)
00664         buff.write(struct.pack('<I%ss'%length, length, _x))
00665         _x = val1.link_name
00666         length = len(_x)
00667         if python3 or type(_x) == unicode:
00668           _x = _x.encode('utf-8')
00669           length = len(_x)
00670         buff.write(struct.pack('<I%ss'%length, length, _x))
00671         _v30 = val1.target_point_offset
00672         _x = _v30
00673         buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00674         _v31 = val1.position
00675         _x = _v31
00676         buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00677         _v32 = val1.constraint_region_shape
00678         buff.write(_struct_b.pack(_v32.type))
00679         length = len(_v32.dimensions)
00680         buff.write(_struct_I.pack(length))
00681         pattern = '<%sd'%length
00682         buff.write(struct.pack(pattern, *_v32.dimensions))
00683         length = len(_v32.triangles)
00684         buff.write(_struct_I.pack(length))
00685         pattern = '<%si'%length
00686         buff.write(struct.pack(pattern, *_v32.triangles))
00687         length = len(_v32.vertices)
00688         buff.write(_struct_I.pack(length))
00689         for val3 in _v32.vertices:
00690           _x = val3
00691           buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00692         _v33 = val1.constraint_region_orientation
00693         _x = _v33
00694         buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00695         buff.write(_struct_d.pack(val1.weight))
00696       length = len(self.goal_constraints.orientation_constraints)
00697       buff.write(_struct_I.pack(length))
00698       for val1 in self.goal_constraints.orientation_constraints:
00699         _v34 = val1.header
00700         buff.write(_struct_I.pack(_v34.seq))
00701         _v35 = _v34.stamp
00702         _x = _v35
00703         buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00704         _x = _v34.frame_id
00705         length = len(_x)
00706         if python3 or type(_x) == unicode:
00707           _x = _x.encode('utf-8')
00708           length = len(_x)
00709         buff.write(struct.pack('<I%ss'%length, length, _x))
00710         _x = val1.link_name
00711         length = len(_x)
00712         if python3 or type(_x) == unicode:
00713           _x = _x.encode('utf-8')
00714           length = len(_x)
00715         buff.write(struct.pack('<I%ss'%length, length, _x))
00716         buff.write(_struct_i.pack(val1.type))
00717         _v36 = val1.orientation
00718         _x = _v36
00719         buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00720         _x = val1
00721         buff.write(_struct_4d.pack(_x.absolute_roll_tolerance, _x.absolute_pitch_tolerance, _x.absolute_yaw_tolerance, _x.weight))
00722       length = len(self.goal_constraints.visibility_constraints)
00723       buff.write(_struct_I.pack(length))
00724       for val1 in self.goal_constraints.visibility_constraints:
00725         _v37 = val1.header
00726         buff.write(_struct_I.pack(_v37.seq))
00727         _v38 = _v37.stamp
00728         _x = _v38
00729         buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00730         _x = _v37.frame_id
00731         length = len(_x)
00732         if python3 or type(_x) == unicode:
00733           _x = _x.encode('utf-8')
00734           length = len(_x)
00735         buff.write(struct.pack('<I%ss'%length, length, _x))
00736         _v39 = val1.target
00737         _v40 = _v39.header
00738         buff.write(_struct_I.pack(_v40.seq))
00739         _v41 = _v40.stamp
00740         _x = _v41
00741         buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00742         _x = _v40.frame_id
00743         length = len(_x)
00744         if python3 or type(_x) == unicode:
00745           _x = _x.encode('utf-8')
00746           length = len(_x)
00747         buff.write(struct.pack('<I%ss'%length, length, _x))
00748         _v42 = _v39.point
00749         _x = _v42
00750         buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00751         _v43 = val1.sensor_pose
00752         _v44 = _v43.header
00753         buff.write(_struct_I.pack(_v44.seq))
00754         _v45 = _v44.stamp
00755         _x = _v45
00756         buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00757         _x = _v44.frame_id
00758         length = len(_x)
00759         if python3 or type(_x) == unicode:
00760           _x = _x.encode('utf-8')
00761           length = len(_x)
00762         buff.write(struct.pack('<I%ss'%length, length, _x))
00763         _v46 = _v43.pose
00764         _v47 = _v46.position
00765         _x = _v47
00766         buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00767         _v48 = _v46.orientation
00768         _x = _v48
00769         buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00770         buff.write(_struct_d.pack(val1.absolute_tolerance))
00771     except struct.error as se: self._check_types(se)
00772     except TypeError as te: self._check_types(te)
00773 
00774   def deserialize(self, str):
00775     """
00776     unpack serialized message in str into this message instance
00777     :param str: byte array of serialized message, ``str``
00778     """
00779     try:
00780       if self.trajectory is None:
00781         self.trajectory = arm_navigation_msgs.msg.RobotTrajectory()
00782       if self.robot_state is None:
00783         self.robot_state = arm_navigation_msgs.msg.RobotState()
00784       if self.path_constraints is None:
00785         self.path_constraints = arm_navigation_msgs.msg.Constraints()
00786       if self.goal_constraints is None:
00787         self.goal_constraints = arm_navigation_msgs.msg.Constraints()
00788       end = 0
00789       _x = self
00790       start = end
00791       end += 12
00792       (_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])
00793       start = end
00794       end += 4
00795       (length,) = _struct_I.unpack(str[start:end])
00796       start = end
00797       end += length
00798       if python3:
00799         self.trajectory.joint_trajectory.header.frame_id = str[start:end].decode('utf-8')
00800       else:
00801         self.trajectory.joint_trajectory.header.frame_id = str[start:end]
00802       start = end
00803       end += 4
00804       (length,) = _struct_I.unpack(str[start:end])
00805       self.trajectory.joint_trajectory.joint_names = []
00806       for i in range(0, length):
00807         start = end
00808         end += 4
00809         (length,) = _struct_I.unpack(str[start:end])
00810         start = end
00811         end += length
00812         if python3:
00813           val1 = str[start:end].decode('utf-8')
00814         else:
00815           val1 = str[start:end]
00816         self.trajectory.joint_trajectory.joint_names.append(val1)
00817       start = end
00818       end += 4
00819       (length,) = _struct_I.unpack(str[start:end])
00820       self.trajectory.joint_trajectory.points = []
00821       for i in range(0, length):
00822         val1 = trajectory_msgs.msg.JointTrajectoryPoint()
00823         start = end
00824         end += 4
00825         (length,) = _struct_I.unpack(str[start:end])
00826         pattern = '<%sd'%length
00827         start = end
00828         end += struct.calcsize(pattern)
00829         val1.positions = struct.unpack(pattern, str[start:end])
00830         start = end
00831         end += 4
00832         (length,) = _struct_I.unpack(str[start:end])
00833         pattern = '<%sd'%length
00834         start = end
00835         end += struct.calcsize(pattern)
00836         val1.velocities = struct.unpack(pattern, str[start:end])
00837         start = end
00838         end += 4
00839         (length,) = _struct_I.unpack(str[start:end])
00840         pattern = '<%sd'%length
00841         start = end
00842         end += struct.calcsize(pattern)
00843         val1.accelerations = struct.unpack(pattern, str[start:end])
00844         _v49 = val1.time_from_start
00845         _x = _v49
00846         start = end
00847         end += 8
00848         (_x.secs, _x.nsecs,) = _struct_2i.unpack(str[start:end])
00849         self.trajectory.joint_trajectory.points.append(val1)
00850       _x = self
00851       start = end
00852       end += 8
00853       (_x.trajectory.multi_dof_joint_trajectory.stamp.secs, _x.trajectory.multi_dof_joint_trajectory.stamp.nsecs,) = _struct_2i.unpack(str[start:end])
00854       start = end
00855       end += 4
00856       (length,) = _struct_I.unpack(str[start:end])
00857       self.trajectory.multi_dof_joint_trajectory.joint_names = []
00858       for i in range(0, length):
00859         start = end
00860         end += 4
00861         (length,) = _struct_I.unpack(str[start:end])
00862         start = end
00863         end += length
00864         if python3:
00865           val1 = str[start:end].decode('utf-8')
00866         else:
00867           val1 = str[start:end]
00868         self.trajectory.multi_dof_joint_trajectory.joint_names.append(val1)
00869       start = end
00870       end += 4
00871       (length,) = _struct_I.unpack(str[start:end])
00872       self.trajectory.multi_dof_joint_trajectory.frame_ids = []
00873       for i in range(0, length):
00874         start = end
00875         end += 4
00876         (length,) = _struct_I.unpack(str[start:end])
00877         start = end
00878         end += length
00879         if python3:
00880           val1 = str[start:end].decode('utf-8')
00881         else:
00882           val1 = str[start:end]
00883         self.trajectory.multi_dof_joint_trajectory.frame_ids.append(val1)
00884       start = end
00885       end += 4
00886       (length,) = _struct_I.unpack(str[start:end])
00887       self.trajectory.multi_dof_joint_trajectory.child_frame_ids = []
00888       for i in range(0, length):
00889         start = end
00890         end += 4
00891         (length,) = _struct_I.unpack(str[start:end])
00892         start = end
00893         end += length
00894         if python3:
00895           val1 = str[start:end].decode('utf-8')
00896         else:
00897           val1 = str[start:end]
00898         self.trajectory.multi_dof_joint_trajectory.child_frame_ids.append(val1)
00899       start = end
00900       end += 4
00901       (length,) = _struct_I.unpack(str[start:end])
00902       self.trajectory.multi_dof_joint_trajectory.points = []
00903       for i in range(0, length):
00904         val1 = arm_navigation_msgs.msg.MultiDOFJointTrajectoryPoint()
00905         start = end
00906         end += 4
00907         (length,) = _struct_I.unpack(str[start:end])
00908         val1.poses = []
00909         for i in range(0, length):
00910           val2 = geometry_msgs.msg.Pose()
00911           _v50 = val2.position
00912           _x = _v50
00913           start = end
00914           end += 24
00915           (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00916           _v51 = val2.orientation
00917           _x = _v51
00918           start = end
00919           end += 32
00920           (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
00921           val1.poses.append(val2)
00922         _v52 = val1.time_from_start
00923         _x = _v52
00924         start = end
00925         end += 8
00926         (_x.secs, _x.nsecs,) = _struct_2i.unpack(str[start:end])
00927         self.trajectory.multi_dof_joint_trajectory.points.append(val1)
00928       _x = self
00929       start = end
00930       end += 12
00931       (_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])
00932       start = end
00933       end += 4
00934       (length,) = _struct_I.unpack(str[start:end])
00935       start = end
00936       end += length
00937       if python3:
00938         self.robot_state.joint_state.header.frame_id = str[start:end].decode('utf-8')
00939       else:
00940         self.robot_state.joint_state.header.frame_id = str[start:end]
00941       start = end
00942       end += 4
00943       (length,) = _struct_I.unpack(str[start:end])
00944       self.robot_state.joint_state.name = []
00945       for i in range(0, length):
00946         start = end
00947         end += 4
00948         (length,) = _struct_I.unpack(str[start:end])
00949         start = end
00950         end += length
00951         if python3:
00952           val1 = str[start:end].decode('utf-8')
00953         else:
00954           val1 = str[start:end]
00955         self.robot_state.joint_state.name.append(val1)
00956       start = end
00957       end += 4
00958       (length,) = _struct_I.unpack(str[start:end])
00959       pattern = '<%sd'%length
00960       start = end
00961       end += struct.calcsize(pattern)
00962       self.robot_state.joint_state.position = struct.unpack(pattern, str[start:end])
00963       start = end
00964       end += 4
00965       (length,) = _struct_I.unpack(str[start:end])
00966       pattern = '<%sd'%length
00967       start = end
00968       end += struct.calcsize(pattern)
00969       self.robot_state.joint_state.velocity = struct.unpack(pattern, str[start:end])
00970       start = end
00971       end += 4
00972       (length,) = _struct_I.unpack(str[start:end])
00973       pattern = '<%sd'%length
00974       start = end
00975       end += struct.calcsize(pattern)
00976       self.robot_state.joint_state.effort = struct.unpack(pattern, str[start:end])
00977       _x = self
00978       start = end
00979       end += 8
00980       (_x.robot_state.multi_dof_joint_state.stamp.secs, _x.robot_state.multi_dof_joint_state.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
00981       start = end
00982       end += 4
00983       (length,) = _struct_I.unpack(str[start:end])
00984       self.robot_state.multi_dof_joint_state.joint_names = []
00985       for i in range(0, length):
00986         start = end
00987         end += 4
00988         (length,) = _struct_I.unpack(str[start:end])
00989         start = end
00990         end += length
00991         if python3:
00992           val1 = str[start:end].decode('utf-8')
00993         else:
00994           val1 = str[start:end]
00995         self.robot_state.multi_dof_joint_state.joint_names.append(val1)
00996       start = end
00997       end += 4
00998       (length,) = _struct_I.unpack(str[start:end])
00999       self.robot_state.multi_dof_joint_state.frame_ids = []
01000       for i in range(0, length):
01001         start = end
01002         end += 4
01003         (length,) = _struct_I.unpack(str[start:end])
01004         start = end
01005         end += length
01006         if python3:
01007           val1 = str[start:end].decode('utf-8')
01008         else:
01009           val1 = str[start:end]
01010         self.robot_state.multi_dof_joint_state.frame_ids.append(val1)
01011       start = end
01012       end += 4
01013       (length,) = _struct_I.unpack(str[start:end])
01014       self.robot_state.multi_dof_joint_state.child_frame_ids = []
01015       for i in range(0, length):
01016         start = end
01017         end += 4
01018         (length,) = _struct_I.unpack(str[start:end])
01019         start = end
01020         end += length
01021         if python3:
01022           val1 = str[start:end].decode('utf-8')
01023         else:
01024           val1 = str[start:end]
01025         self.robot_state.multi_dof_joint_state.child_frame_ids.append(val1)
01026       start = end
01027       end += 4
01028       (length,) = _struct_I.unpack(str[start:end])
01029       self.robot_state.multi_dof_joint_state.poses = []
01030       for i in range(0, length):
01031         val1 = geometry_msgs.msg.Pose()
01032         _v53 = val1.position
01033         _x = _v53
01034         start = end
01035         end += 24
01036         (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01037         _v54 = val1.orientation
01038         _x = _v54
01039         start = end
01040         end += 32
01041         (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01042         self.robot_state.multi_dof_joint_state.poses.append(val1)
01043       _x = self
01044       start = end
01045       end += 5
01046       (_x.check_collisions, _x.check_path_constraints, _x.check_goal_constraints, _x.check_joint_limits, _x.check_full_trajectory,) = _struct_5B.unpack(str[start:end])
01047       self.check_collisions = bool(self.check_collisions)
01048       self.check_path_constraints = bool(self.check_path_constraints)
01049       self.check_goal_constraints = bool(self.check_goal_constraints)
01050       self.check_joint_limits = bool(self.check_joint_limits)
01051       self.check_full_trajectory = bool(self.check_full_trajectory)
01052       start = end
01053       end += 4
01054       (length,) = _struct_I.unpack(str[start:end])
01055       self.path_constraints.joint_constraints = []
01056       for i in range(0, length):
01057         val1 = arm_navigation_msgs.msg.JointConstraint()
01058         start = end
01059         end += 4
01060         (length,) = _struct_I.unpack(str[start:end])
01061         start = end
01062         end += length
01063         if python3:
01064           val1.joint_name = str[start:end].decode('utf-8')
01065         else:
01066           val1.joint_name = str[start:end]
01067         _x = val1
01068         start = end
01069         end += 32
01070         (_x.position, _x.tolerance_above, _x.tolerance_below, _x.weight,) = _struct_4d.unpack(str[start:end])
01071         self.path_constraints.joint_constraints.append(val1)
01072       start = end
01073       end += 4
01074       (length,) = _struct_I.unpack(str[start:end])
01075       self.path_constraints.position_constraints = []
01076       for i in range(0, length):
01077         val1 = arm_navigation_msgs.msg.PositionConstraint()
01078         _v55 = val1.header
01079         start = end
01080         end += 4
01081         (_v55.seq,) = _struct_I.unpack(str[start:end])
01082         _v56 = _v55.stamp
01083         _x = _v56
01084         start = end
01085         end += 8
01086         (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01087         start = end
01088         end += 4
01089         (length,) = _struct_I.unpack(str[start:end])
01090         start = end
01091         end += length
01092         if python3:
01093           _v55.frame_id = str[start:end].decode('utf-8')
01094         else:
01095           _v55.frame_id = str[start:end]
01096         start = end
01097         end += 4
01098         (length,) = _struct_I.unpack(str[start:end])
01099         start = end
01100         end += length
01101         if python3:
01102           val1.link_name = str[start:end].decode('utf-8')
01103         else:
01104           val1.link_name = str[start:end]
01105         _v57 = val1.target_point_offset
01106         _x = _v57
01107         start = end
01108         end += 24
01109         (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01110         _v58 = val1.position
01111         _x = _v58
01112         start = end
01113         end += 24
01114         (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01115         _v59 = val1.constraint_region_shape
01116         start = end
01117         end += 1
01118         (_v59.type,) = _struct_b.unpack(str[start:end])
01119         start = end
01120         end += 4
01121         (length,) = _struct_I.unpack(str[start:end])
01122         pattern = '<%sd'%length
01123         start = end
01124         end += struct.calcsize(pattern)
01125         _v59.dimensions = struct.unpack(pattern, str[start:end])
01126         start = end
01127         end += 4
01128         (length,) = _struct_I.unpack(str[start:end])
01129         pattern = '<%si'%length
01130         start = end
01131         end += struct.calcsize(pattern)
01132         _v59.triangles = struct.unpack(pattern, str[start:end])
01133         start = end
01134         end += 4
01135         (length,) = _struct_I.unpack(str[start:end])
01136         _v59.vertices = []
01137         for i in range(0, length):
01138           val3 = geometry_msgs.msg.Point()
01139           _x = val3
01140           start = end
01141           end += 24
01142           (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01143           _v59.vertices.append(val3)
01144         _v60 = val1.constraint_region_orientation
01145         _x = _v60
01146         start = end
01147         end += 32
01148         (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01149         start = end
01150         end += 8
01151         (val1.weight,) = _struct_d.unpack(str[start:end])
01152         self.path_constraints.position_constraints.append(val1)
01153       start = end
01154       end += 4
01155       (length,) = _struct_I.unpack(str[start:end])
01156       self.path_constraints.orientation_constraints = []
01157       for i in range(0, length):
01158         val1 = arm_navigation_msgs.msg.OrientationConstraint()
01159         _v61 = val1.header
01160         start = end
01161         end += 4
01162         (_v61.seq,) = _struct_I.unpack(str[start:end])
01163         _v62 = _v61.stamp
01164         _x = _v62
01165         start = end
01166         end += 8
01167         (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01168         start = end
01169         end += 4
01170         (length,) = _struct_I.unpack(str[start:end])
01171         start = end
01172         end += length
01173         if python3:
01174           _v61.frame_id = str[start:end].decode('utf-8')
01175         else:
01176           _v61.frame_id = str[start:end]
01177         start = end
01178         end += 4
01179         (length,) = _struct_I.unpack(str[start:end])
01180         start = end
01181         end += length
01182         if python3:
01183           val1.link_name = str[start:end].decode('utf-8')
01184         else:
01185           val1.link_name = str[start:end]
01186         start = end
01187         end += 4
01188         (val1.type,) = _struct_i.unpack(str[start:end])
01189         _v63 = val1.orientation
01190         _x = _v63
01191         start = end
01192         end += 32
01193         (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01194         _x = val1
01195         start = end
01196         end += 32
01197         (_x.absolute_roll_tolerance, _x.absolute_pitch_tolerance, _x.absolute_yaw_tolerance, _x.weight,) = _struct_4d.unpack(str[start:end])
01198         self.path_constraints.orientation_constraints.append(val1)
01199       start = end
01200       end += 4
01201       (length,) = _struct_I.unpack(str[start:end])
01202       self.path_constraints.visibility_constraints = []
01203       for i in range(0, length):
01204         val1 = arm_navigation_msgs.msg.VisibilityConstraint()
01205         _v64 = val1.header
01206         start = end
01207         end += 4
01208         (_v64.seq,) = _struct_I.unpack(str[start:end])
01209         _v65 = _v64.stamp
01210         _x = _v65
01211         start = end
01212         end += 8
01213         (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01214         start = end
01215         end += 4
01216         (length,) = _struct_I.unpack(str[start:end])
01217         start = end
01218         end += length
01219         if python3:
01220           _v64.frame_id = str[start:end].decode('utf-8')
01221         else:
01222           _v64.frame_id = str[start:end]
01223         _v66 = val1.target
01224         _v67 = _v66.header
01225         start = end
01226         end += 4
01227         (_v67.seq,) = _struct_I.unpack(str[start:end])
01228         _v68 = _v67.stamp
01229         _x = _v68
01230         start = end
01231         end += 8
01232         (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01233         start = end
01234         end += 4
01235         (length,) = _struct_I.unpack(str[start:end])
01236         start = end
01237         end += length
01238         if python3:
01239           _v67.frame_id = str[start:end].decode('utf-8')
01240         else:
01241           _v67.frame_id = str[start:end]
01242         _v69 = _v66.point
01243         _x = _v69
01244         start = end
01245         end += 24
01246         (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01247         _v70 = val1.sensor_pose
01248         _v71 = _v70.header
01249         start = end
01250         end += 4
01251         (_v71.seq,) = _struct_I.unpack(str[start:end])
01252         _v72 = _v71.stamp
01253         _x = _v72
01254         start = end
01255         end += 8
01256         (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01257         start = end
01258         end += 4
01259         (length,) = _struct_I.unpack(str[start:end])
01260         start = end
01261         end += length
01262         if python3:
01263           _v71.frame_id = str[start:end].decode('utf-8')
01264         else:
01265           _v71.frame_id = str[start:end]
01266         _v73 = _v70.pose
01267         _v74 = _v73.position
01268         _x = _v74
01269         start = end
01270         end += 24
01271         (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01272         _v75 = _v73.orientation
01273         _x = _v75
01274         start = end
01275         end += 32
01276         (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01277         start = end
01278         end += 8
01279         (val1.absolute_tolerance,) = _struct_d.unpack(str[start:end])
01280         self.path_constraints.visibility_constraints.append(val1)
01281       start = end
01282       end += 4
01283       (length,) = _struct_I.unpack(str[start:end])
01284       self.goal_constraints.joint_constraints = []
01285       for i in range(0, length):
01286         val1 = arm_navigation_msgs.msg.JointConstraint()
01287         start = end
01288         end += 4
01289         (length,) = _struct_I.unpack(str[start:end])
01290         start = end
01291         end += length
01292         if python3:
01293           val1.joint_name = str[start:end].decode('utf-8')
01294         else:
01295           val1.joint_name = str[start:end]
01296         _x = val1
01297         start = end
01298         end += 32
01299         (_x.position, _x.tolerance_above, _x.tolerance_below, _x.weight,) = _struct_4d.unpack(str[start:end])
01300         self.goal_constraints.joint_constraints.append(val1)
01301       start = end
01302       end += 4
01303       (length,) = _struct_I.unpack(str[start:end])
01304       self.goal_constraints.position_constraints = []
01305       for i in range(0, length):
01306         val1 = arm_navigation_msgs.msg.PositionConstraint()
01307         _v76 = val1.header
01308         start = end
01309         end += 4
01310         (_v76.seq,) = _struct_I.unpack(str[start:end])
01311         _v77 = _v76.stamp
01312         _x = _v77
01313         start = end
01314         end += 8
01315         (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01316         start = end
01317         end += 4
01318         (length,) = _struct_I.unpack(str[start:end])
01319         start = end
01320         end += length
01321         if python3:
01322           _v76.frame_id = str[start:end].decode('utf-8')
01323         else:
01324           _v76.frame_id = str[start:end]
01325         start = end
01326         end += 4
01327         (length,) = _struct_I.unpack(str[start:end])
01328         start = end
01329         end += length
01330         if python3:
01331           val1.link_name = str[start:end].decode('utf-8')
01332         else:
01333           val1.link_name = str[start:end]
01334         _v78 = val1.target_point_offset
01335         _x = _v78
01336         start = end
01337         end += 24
01338         (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01339         _v79 = val1.position
01340         _x = _v79
01341         start = end
01342         end += 24
01343         (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01344         _v80 = val1.constraint_region_shape
01345         start = end
01346         end += 1
01347         (_v80.type,) = _struct_b.unpack(str[start:end])
01348         start = end
01349         end += 4
01350         (length,) = _struct_I.unpack(str[start:end])
01351         pattern = '<%sd'%length
01352         start = end
01353         end += struct.calcsize(pattern)
01354         _v80.dimensions = struct.unpack(pattern, str[start:end])
01355         start = end
01356         end += 4
01357         (length,) = _struct_I.unpack(str[start:end])
01358         pattern = '<%si'%length
01359         start = end
01360         end += struct.calcsize(pattern)
01361         _v80.triangles = struct.unpack(pattern, str[start:end])
01362         start = end
01363         end += 4
01364         (length,) = _struct_I.unpack(str[start:end])
01365         _v80.vertices = []
01366         for i in range(0, length):
01367           val3 = geometry_msgs.msg.Point()
01368           _x = val3
01369           start = end
01370           end += 24
01371           (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01372           _v80.vertices.append(val3)
01373         _v81 = val1.constraint_region_orientation
01374         _x = _v81
01375         start = end
01376         end += 32
01377         (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01378         start = end
01379         end += 8
01380         (val1.weight,) = _struct_d.unpack(str[start:end])
01381         self.goal_constraints.position_constraints.append(val1)
01382       start = end
01383       end += 4
01384       (length,) = _struct_I.unpack(str[start:end])
01385       self.goal_constraints.orientation_constraints = []
01386       for i in range(0, length):
01387         val1 = arm_navigation_msgs.msg.OrientationConstraint()
01388         _v82 = val1.header
01389         start = end
01390         end += 4
01391         (_v82.seq,) = _struct_I.unpack(str[start:end])
01392         _v83 = _v82.stamp
01393         _x = _v83
01394         start = end
01395         end += 8
01396         (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01397         start = end
01398         end += 4
01399         (length,) = _struct_I.unpack(str[start:end])
01400         start = end
01401         end += length
01402         if python3:
01403           _v82.frame_id = str[start:end].decode('utf-8')
01404         else:
01405           _v82.frame_id = str[start:end]
01406         start = end
01407         end += 4
01408         (length,) = _struct_I.unpack(str[start:end])
01409         start = end
01410         end += length
01411         if python3:
01412           val1.link_name = str[start:end].decode('utf-8')
01413         else:
01414           val1.link_name = str[start:end]
01415         start = end
01416         end += 4
01417         (val1.type,) = _struct_i.unpack(str[start:end])
01418         _v84 = val1.orientation
01419         _x = _v84
01420         start = end
01421         end += 32
01422         (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01423         _x = val1
01424         start = end
01425         end += 32
01426         (_x.absolute_roll_tolerance, _x.absolute_pitch_tolerance, _x.absolute_yaw_tolerance, _x.weight,) = _struct_4d.unpack(str[start:end])
01427         self.goal_constraints.orientation_constraints.append(val1)
01428       start = end
01429       end += 4
01430       (length,) = _struct_I.unpack(str[start:end])
01431       self.goal_constraints.visibility_constraints = []
01432       for i in range(0, length):
01433         val1 = arm_navigation_msgs.msg.VisibilityConstraint()
01434         _v85 = val1.header
01435         start = end
01436         end += 4
01437         (_v85.seq,) = _struct_I.unpack(str[start:end])
01438         _v86 = _v85.stamp
01439         _x = _v86
01440         start = end
01441         end += 8
01442         (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01443         start = end
01444         end += 4
01445         (length,) = _struct_I.unpack(str[start:end])
01446         start = end
01447         end += length
01448         if python3:
01449           _v85.frame_id = str[start:end].decode('utf-8')
01450         else:
01451           _v85.frame_id = str[start:end]
01452         _v87 = val1.target
01453         _v88 = _v87.header
01454         start = end
01455         end += 4
01456         (_v88.seq,) = _struct_I.unpack(str[start:end])
01457         _v89 = _v88.stamp
01458         _x = _v89
01459         start = end
01460         end += 8
01461         (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01462         start = end
01463         end += 4
01464         (length,) = _struct_I.unpack(str[start:end])
01465         start = end
01466         end += length
01467         if python3:
01468           _v88.frame_id = str[start:end].decode('utf-8')
01469         else:
01470           _v88.frame_id = str[start:end]
01471         _v90 = _v87.point
01472         _x = _v90
01473         start = end
01474         end += 24
01475         (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01476         _v91 = val1.sensor_pose
01477         _v92 = _v91.header
01478         start = end
01479         end += 4
01480         (_v92.seq,) = _struct_I.unpack(str[start:end])
01481         _v93 = _v92.stamp
01482         _x = _v93
01483         start = end
01484         end += 8
01485         (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01486         start = end
01487         end += 4
01488         (length,) = _struct_I.unpack(str[start:end])
01489         start = end
01490         end += length
01491         if python3:
01492           _v92.frame_id = str[start:end].decode('utf-8')
01493         else:
01494           _v92.frame_id = str[start:end]
01495         _v94 = _v91.pose
01496         _v95 = _v94.position
01497         _x = _v95
01498         start = end
01499         end += 24
01500         (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01501         _v96 = _v94.orientation
01502         _x = _v96
01503         start = end
01504         end += 32
01505         (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01506         start = end
01507         end += 8
01508         (val1.absolute_tolerance,) = _struct_d.unpack(str[start:end])
01509         self.goal_constraints.visibility_constraints.append(val1)
01510       return self
01511     except struct.error as e:
01512       raise genpy.DeserializationError(e) #most likely buffer underfill
01513 
01514 
01515   def serialize_numpy(self, buff, numpy):
01516     """
01517     serialize message with numpy array types into buffer
01518     :param buff: buffer, ``StringIO``
01519     :param numpy: numpy python module
01520     """
01521     try:
01522       _x = self
01523       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))
01524       _x = self.trajectory.joint_trajectory.header.frame_id
01525       length = len(_x)
01526       if python3 or type(_x) == unicode:
01527         _x = _x.encode('utf-8')
01528         length = len(_x)
01529       buff.write(struct.pack('<I%ss'%length, length, _x))
01530       length = len(self.trajectory.joint_trajectory.joint_names)
01531       buff.write(_struct_I.pack(length))
01532       for val1 in self.trajectory.joint_trajectory.joint_names:
01533         length = len(val1)
01534         if python3 or type(val1) == unicode:
01535           val1 = val1.encode('utf-8')
01536           length = len(val1)
01537         buff.write(struct.pack('<I%ss'%length, length, val1))
01538       length = len(self.trajectory.joint_trajectory.points)
01539       buff.write(_struct_I.pack(length))
01540       for val1 in self.trajectory.joint_trajectory.points:
01541         length = len(val1.positions)
01542         buff.write(_struct_I.pack(length))
01543         pattern = '<%sd'%length
01544         buff.write(val1.positions.tostring())
01545         length = len(val1.velocities)
01546         buff.write(_struct_I.pack(length))
01547         pattern = '<%sd'%length
01548         buff.write(val1.velocities.tostring())
01549         length = len(val1.accelerations)
01550         buff.write(_struct_I.pack(length))
01551         pattern = '<%sd'%length
01552         buff.write(val1.accelerations.tostring())
01553         _v97 = val1.time_from_start
01554         _x = _v97
01555         buff.write(_struct_2i.pack(_x.secs, _x.nsecs))
01556       _x = self
01557       buff.write(_struct_2i.pack(_x.trajectory.multi_dof_joint_trajectory.stamp.secs, _x.trajectory.multi_dof_joint_trajectory.stamp.nsecs))
01558       length = len(self.trajectory.multi_dof_joint_trajectory.joint_names)
01559       buff.write(_struct_I.pack(length))
01560       for val1 in self.trajectory.multi_dof_joint_trajectory.joint_names:
01561         length = len(val1)
01562         if python3 or type(val1) == unicode:
01563           val1 = val1.encode('utf-8')
01564           length = len(val1)
01565         buff.write(struct.pack('<I%ss'%length, length, val1))
01566       length = len(self.trajectory.multi_dof_joint_trajectory.frame_ids)
01567       buff.write(_struct_I.pack(length))
01568       for val1 in self.trajectory.multi_dof_joint_trajectory.frame_ids:
01569         length = len(val1)
01570         if python3 or type(val1) == unicode:
01571           val1 = val1.encode('utf-8')
01572           length = len(val1)
01573         buff.write(struct.pack('<I%ss'%length, length, val1))
01574       length = len(self.trajectory.multi_dof_joint_trajectory.child_frame_ids)
01575       buff.write(_struct_I.pack(length))
01576       for val1 in self.trajectory.multi_dof_joint_trajectory.child_frame_ids:
01577         length = len(val1)
01578         if python3 or type(val1) == unicode:
01579           val1 = val1.encode('utf-8')
01580           length = len(val1)
01581         buff.write(struct.pack('<I%ss'%length, length, val1))
01582       length = len(self.trajectory.multi_dof_joint_trajectory.points)
01583       buff.write(_struct_I.pack(length))
01584       for val1 in self.trajectory.multi_dof_joint_trajectory.points:
01585         length = len(val1.poses)
01586         buff.write(_struct_I.pack(length))
01587         for val2 in val1.poses:
01588           _v98 = val2.position
01589           _x = _v98
01590           buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01591           _v99 = val2.orientation
01592           _x = _v99
01593           buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01594         _v100 = val1.time_from_start
01595         _x = _v100
01596         buff.write(_struct_2i.pack(_x.secs, _x.nsecs))
01597       _x = self
01598       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))
01599       _x = self.robot_state.joint_state.header.frame_id
01600       length = len(_x)
01601       if python3 or type(_x) == unicode:
01602         _x = _x.encode('utf-8')
01603         length = len(_x)
01604       buff.write(struct.pack('<I%ss'%length, length, _x))
01605       length = len(self.robot_state.joint_state.name)
01606       buff.write(_struct_I.pack(length))
01607       for val1 in self.robot_state.joint_state.name:
01608         length = len(val1)
01609         if python3 or type(val1) == unicode:
01610           val1 = val1.encode('utf-8')
01611           length = len(val1)
01612         buff.write(struct.pack('<I%ss'%length, length, val1))
01613       length = len(self.robot_state.joint_state.position)
01614       buff.write(_struct_I.pack(length))
01615       pattern = '<%sd'%length
01616       buff.write(self.robot_state.joint_state.position.tostring())
01617       length = len(self.robot_state.joint_state.velocity)
01618       buff.write(_struct_I.pack(length))
01619       pattern = '<%sd'%length
01620       buff.write(self.robot_state.joint_state.velocity.tostring())
01621       length = len(self.robot_state.joint_state.effort)
01622       buff.write(_struct_I.pack(length))
01623       pattern = '<%sd'%length
01624       buff.write(self.robot_state.joint_state.effort.tostring())
01625       _x = self
01626       buff.write(_struct_2I.pack(_x.robot_state.multi_dof_joint_state.stamp.secs, _x.robot_state.multi_dof_joint_state.stamp.nsecs))
01627       length = len(self.robot_state.multi_dof_joint_state.joint_names)
01628       buff.write(_struct_I.pack(length))
01629       for val1 in self.robot_state.multi_dof_joint_state.joint_names:
01630         length = len(val1)
01631         if python3 or type(val1) == unicode:
01632           val1 = val1.encode('utf-8')
01633           length = len(val1)
01634         buff.write(struct.pack('<I%ss'%length, length, val1))
01635       length = len(self.robot_state.multi_dof_joint_state.frame_ids)
01636       buff.write(_struct_I.pack(length))
01637       for val1 in self.robot_state.multi_dof_joint_state.frame_ids:
01638         length = len(val1)
01639         if python3 or type(val1) == unicode:
01640           val1 = val1.encode('utf-8')
01641           length = len(val1)
01642         buff.write(struct.pack('<I%ss'%length, length, val1))
01643       length = len(self.robot_state.multi_dof_joint_state.child_frame_ids)
01644       buff.write(_struct_I.pack(length))
01645       for val1 in self.robot_state.multi_dof_joint_state.child_frame_ids:
01646         length = len(val1)
01647         if python3 or type(val1) == unicode:
01648           val1 = val1.encode('utf-8')
01649           length = len(val1)
01650         buff.write(struct.pack('<I%ss'%length, length, val1))
01651       length = len(self.robot_state.multi_dof_joint_state.poses)
01652       buff.write(_struct_I.pack(length))
01653       for val1 in self.robot_state.multi_dof_joint_state.poses:
01654         _v101 = val1.position
01655         _x = _v101
01656         buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01657         _v102 = val1.orientation
01658         _x = _v102
01659         buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01660       _x = self
01661       buff.write(_struct_5B.pack(_x.check_collisions, _x.check_path_constraints, _x.check_goal_constraints, _x.check_joint_limits, _x.check_full_trajectory))
01662       length = len(self.path_constraints.joint_constraints)
01663       buff.write(_struct_I.pack(length))
01664       for val1 in self.path_constraints.joint_constraints:
01665         _x = val1.joint_name
01666         length = len(_x)
01667         if python3 or type(_x) == unicode:
01668           _x = _x.encode('utf-8')
01669           length = len(_x)
01670         buff.write(struct.pack('<I%ss'%length, length, _x))
01671         _x = val1
01672         buff.write(_struct_4d.pack(_x.position, _x.tolerance_above, _x.tolerance_below, _x.weight))
01673       length = len(self.path_constraints.position_constraints)
01674       buff.write(_struct_I.pack(length))
01675       for val1 in self.path_constraints.position_constraints:
01676         _v103 = val1.header
01677         buff.write(_struct_I.pack(_v103.seq))
01678         _v104 = _v103.stamp
01679         _x = _v104
01680         buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01681         _x = _v103.frame_id
01682         length = len(_x)
01683         if python3 or type(_x) == unicode:
01684           _x = _x.encode('utf-8')
01685           length = len(_x)
01686         buff.write(struct.pack('<I%ss'%length, length, _x))
01687         _x = val1.link_name
01688         length = len(_x)
01689         if python3 or type(_x) == unicode:
01690           _x = _x.encode('utf-8')
01691           length = len(_x)
01692         buff.write(struct.pack('<I%ss'%length, length, _x))
01693         _v105 = val1.target_point_offset
01694         _x = _v105
01695         buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01696         _v106 = val1.position
01697         _x = _v106
01698         buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01699         _v107 = val1.constraint_region_shape
01700         buff.write(_struct_b.pack(_v107.type))
01701         length = len(_v107.dimensions)
01702         buff.write(_struct_I.pack(length))
01703         pattern = '<%sd'%length
01704         buff.write(_v107.dimensions.tostring())
01705         length = len(_v107.triangles)
01706         buff.write(_struct_I.pack(length))
01707         pattern = '<%si'%length
01708         buff.write(_v107.triangles.tostring())
01709         length = len(_v107.vertices)
01710         buff.write(_struct_I.pack(length))
01711         for val3 in _v107.vertices:
01712           _x = val3
01713           buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01714         _v108 = val1.constraint_region_orientation
01715         _x = _v108
01716         buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01717         buff.write(_struct_d.pack(val1.weight))
01718       length = len(self.path_constraints.orientation_constraints)
01719       buff.write(_struct_I.pack(length))
01720       for val1 in self.path_constraints.orientation_constraints:
01721         _v109 = val1.header
01722         buff.write(_struct_I.pack(_v109.seq))
01723         _v110 = _v109.stamp
01724         _x = _v110
01725         buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01726         _x = _v109.frame_id
01727         length = len(_x)
01728         if python3 or type(_x) == unicode:
01729           _x = _x.encode('utf-8')
01730           length = len(_x)
01731         buff.write(struct.pack('<I%ss'%length, length, _x))
01732         _x = val1.link_name
01733         length = len(_x)
01734         if python3 or type(_x) == unicode:
01735           _x = _x.encode('utf-8')
01736           length = len(_x)
01737         buff.write(struct.pack('<I%ss'%length, length, _x))
01738         buff.write(_struct_i.pack(val1.type))
01739         _v111 = val1.orientation
01740         _x = _v111
01741         buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01742         _x = val1
01743         buff.write(_struct_4d.pack(_x.absolute_roll_tolerance, _x.absolute_pitch_tolerance, _x.absolute_yaw_tolerance, _x.weight))
01744       length = len(self.path_constraints.visibility_constraints)
01745       buff.write(_struct_I.pack(length))
01746       for val1 in self.path_constraints.visibility_constraints:
01747         _v112 = val1.header
01748         buff.write(_struct_I.pack(_v112.seq))
01749         _v113 = _v112.stamp
01750         _x = _v113
01751         buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01752         _x = _v112.frame_id
01753         length = len(_x)
01754         if python3 or type(_x) == unicode:
01755           _x = _x.encode('utf-8')
01756           length = len(_x)
01757         buff.write(struct.pack('<I%ss'%length, length, _x))
01758         _v114 = val1.target
01759         _v115 = _v114.header
01760         buff.write(_struct_I.pack(_v115.seq))
01761         _v116 = _v115.stamp
01762         _x = _v116
01763         buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01764         _x = _v115.frame_id
01765         length = len(_x)
01766         if python3 or type(_x) == unicode:
01767           _x = _x.encode('utf-8')
01768           length = len(_x)
01769         buff.write(struct.pack('<I%ss'%length, length, _x))
01770         _v117 = _v114.point
01771         _x = _v117
01772         buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01773         _v118 = val1.sensor_pose
01774         _v119 = _v118.header
01775         buff.write(_struct_I.pack(_v119.seq))
01776         _v120 = _v119.stamp
01777         _x = _v120
01778         buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01779         _x = _v119.frame_id
01780         length = len(_x)
01781         if python3 or type(_x) == unicode:
01782           _x = _x.encode('utf-8')
01783           length = len(_x)
01784         buff.write(struct.pack('<I%ss'%length, length, _x))
01785         _v121 = _v118.pose
01786         _v122 = _v121.position
01787         _x = _v122
01788         buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01789         _v123 = _v121.orientation
01790         _x = _v123
01791         buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01792         buff.write(_struct_d.pack(val1.absolute_tolerance))
01793       length = len(self.goal_constraints.joint_constraints)
01794       buff.write(_struct_I.pack(length))
01795       for val1 in self.goal_constraints.joint_constraints:
01796         _x = val1.joint_name
01797         length = len(_x)
01798         if python3 or type(_x) == unicode:
01799           _x = _x.encode('utf-8')
01800           length = len(_x)
01801         buff.write(struct.pack('<I%ss'%length, length, _x))
01802         _x = val1
01803         buff.write(_struct_4d.pack(_x.position, _x.tolerance_above, _x.tolerance_below, _x.weight))
01804       length = len(self.goal_constraints.position_constraints)
01805       buff.write(_struct_I.pack(length))
01806       for val1 in self.goal_constraints.position_constraints:
01807         _v124 = val1.header
01808         buff.write(_struct_I.pack(_v124.seq))
01809         _v125 = _v124.stamp
01810         _x = _v125
01811         buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01812         _x = _v124.frame_id
01813         length = len(_x)
01814         if python3 or type(_x) == unicode:
01815           _x = _x.encode('utf-8')
01816           length = len(_x)
01817         buff.write(struct.pack('<I%ss'%length, length, _x))
01818         _x = val1.link_name
01819         length = len(_x)
01820         if python3 or type(_x) == unicode:
01821           _x = _x.encode('utf-8')
01822           length = len(_x)
01823         buff.write(struct.pack('<I%ss'%length, length, _x))
01824         _v126 = val1.target_point_offset
01825         _x = _v126
01826         buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01827         _v127 = val1.position
01828         _x = _v127
01829         buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01830         _v128 = val1.constraint_region_shape
01831         buff.write(_struct_b.pack(_v128.type))
01832         length = len(_v128.dimensions)
01833         buff.write(_struct_I.pack(length))
01834         pattern = '<%sd'%length
01835         buff.write(_v128.dimensions.tostring())
01836         length = len(_v128.triangles)
01837         buff.write(_struct_I.pack(length))
01838         pattern = '<%si'%length
01839         buff.write(_v128.triangles.tostring())
01840         length = len(_v128.vertices)
01841         buff.write(_struct_I.pack(length))
01842         for val3 in _v128.vertices:
01843           _x = val3
01844           buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01845         _v129 = val1.constraint_region_orientation
01846         _x = _v129
01847         buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01848         buff.write(_struct_d.pack(val1.weight))
01849       length = len(self.goal_constraints.orientation_constraints)
01850       buff.write(_struct_I.pack(length))
01851       for val1 in self.goal_constraints.orientation_constraints:
01852         _v130 = val1.header
01853         buff.write(_struct_I.pack(_v130.seq))
01854         _v131 = _v130.stamp
01855         _x = _v131
01856         buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01857         _x = _v130.frame_id
01858         length = len(_x)
01859         if python3 or type(_x) == unicode:
01860           _x = _x.encode('utf-8')
01861           length = len(_x)
01862         buff.write(struct.pack('<I%ss'%length, length, _x))
01863         _x = val1.link_name
01864         length = len(_x)
01865         if python3 or type(_x) == unicode:
01866           _x = _x.encode('utf-8')
01867           length = len(_x)
01868         buff.write(struct.pack('<I%ss'%length, length, _x))
01869         buff.write(_struct_i.pack(val1.type))
01870         _v132 = val1.orientation
01871         _x = _v132
01872         buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01873         _x = val1
01874         buff.write(_struct_4d.pack(_x.absolute_roll_tolerance, _x.absolute_pitch_tolerance, _x.absolute_yaw_tolerance, _x.weight))
01875       length = len(self.goal_constraints.visibility_constraints)
01876       buff.write(_struct_I.pack(length))
01877       for val1 in self.goal_constraints.visibility_constraints:
01878         _v133 = val1.header
01879         buff.write(_struct_I.pack(_v133.seq))
01880         _v134 = _v133.stamp
01881         _x = _v134
01882         buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01883         _x = _v133.frame_id
01884         length = len(_x)
01885         if python3 or type(_x) == unicode:
01886           _x = _x.encode('utf-8')
01887           length = len(_x)
01888         buff.write(struct.pack('<I%ss'%length, length, _x))
01889         _v135 = val1.target
01890         _v136 = _v135.header
01891         buff.write(_struct_I.pack(_v136.seq))
01892         _v137 = _v136.stamp
01893         _x = _v137
01894         buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01895         _x = _v136.frame_id
01896         length = len(_x)
01897         if python3 or type(_x) == unicode:
01898           _x = _x.encode('utf-8')
01899           length = len(_x)
01900         buff.write(struct.pack('<I%ss'%length, length, _x))
01901         _v138 = _v135.point
01902         _x = _v138
01903         buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01904         _v139 = val1.sensor_pose
01905         _v140 = _v139.header
01906         buff.write(_struct_I.pack(_v140.seq))
01907         _v141 = _v140.stamp
01908         _x = _v141
01909         buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01910         _x = _v140.frame_id
01911         length = len(_x)
01912         if python3 or type(_x) == unicode:
01913           _x = _x.encode('utf-8')
01914           length = len(_x)
01915         buff.write(struct.pack('<I%ss'%length, length, _x))
01916         _v142 = _v139.pose
01917         _v143 = _v142.position
01918         _x = _v143
01919         buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01920         _v144 = _v142.orientation
01921         _x = _v144
01922         buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01923         buff.write(_struct_d.pack(val1.absolute_tolerance))
01924     except struct.error as se: self._check_types(se)
01925     except TypeError as te: self._check_types(te)
01926 
01927   def deserialize_numpy(self, str, numpy):
01928     """
01929     unpack serialized message in str into this message instance using numpy for array types
01930     :param str: byte array of serialized message, ``str``
01931     :param numpy: numpy python module
01932     """
01933     try:
01934       if self.trajectory is None:
01935         self.trajectory = arm_navigation_msgs.msg.RobotTrajectory()
01936       if self.robot_state is None:
01937         self.robot_state = arm_navigation_msgs.msg.RobotState()
01938       if self.path_constraints is None:
01939         self.path_constraints = arm_navigation_msgs.msg.Constraints()
01940       if self.goal_constraints is None:
01941         self.goal_constraints = arm_navigation_msgs.msg.Constraints()
01942       end = 0
01943       _x = self
01944       start = end
01945       end += 12
01946       (_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])
01947       start = end
01948       end += 4
01949       (length,) = _struct_I.unpack(str[start:end])
01950       start = end
01951       end += length
01952       if python3:
01953         self.trajectory.joint_trajectory.header.frame_id = str[start:end].decode('utf-8')
01954       else:
01955         self.trajectory.joint_trajectory.header.frame_id = str[start:end]
01956       start = end
01957       end += 4
01958       (length,) = _struct_I.unpack(str[start:end])
01959       self.trajectory.joint_trajectory.joint_names = []
01960       for i in range(0, length):
01961         start = end
01962         end += 4
01963         (length,) = _struct_I.unpack(str[start:end])
01964         start = end
01965         end += length
01966         if python3:
01967           val1 = str[start:end].decode('utf-8')
01968         else:
01969           val1 = str[start:end]
01970         self.trajectory.joint_trajectory.joint_names.append(val1)
01971       start = end
01972       end += 4
01973       (length,) = _struct_I.unpack(str[start:end])
01974       self.trajectory.joint_trajectory.points = []
01975       for i in range(0, length):
01976         val1 = trajectory_msgs.msg.JointTrajectoryPoint()
01977         start = end
01978         end += 4
01979         (length,) = _struct_I.unpack(str[start:end])
01980         pattern = '<%sd'%length
01981         start = end
01982         end += struct.calcsize(pattern)
01983         val1.positions = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01984         start = end
01985         end += 4
01986         (length,) = _struct_I.unpack(str[start:end])
01987         pattern = '<%sd'%length
01988         start = end
01989         end += struct.calcsize(pattern)
01990         val1.velocities = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01991         start = end
01992         end += 4
01993         (length,) = _struct_I.unpack(str[start:end])
01994         pattern = '<%sd'%length
01995         start = end
01996         end += struct.calcsize(pattern)
01997         val1.accelerations = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01998         _v145 = val1.time_from_start
01999         _x = _v145
02000         start = end
02001         end += 8
02002         (_x.secs, _x.nsecs,) = _struct_2i.unpack(str[start:end])
02003         self.trajectory.joint_trajectory.points.append(val1)
02004       _x = self
02005       start = end
02006       end += 8
02007       (_x.trajectory.multi_dof_joint_trajectory.stamp.secs, _x.trajectory.multi_dof_joint_trajectory.stamp.nsecs,) = _struct_2i.unpack(str[start:end])
02008       start = end
02009       end += 4
02010       (length,) = _struct_I.unpack(str[start:end])
02011       self.trajectory.multi_dof_joint_trajectory.joint_names = []
02012       for i in range(0, length):
02013         start = end
02014         end += 4
02015         (length,) = _struct_I.unpack(str[start:end])
02016         start = end
02017         end += length
02018         if python3:
02019           val1 = str[start:end].decode('utf-8')
02020         else:
02021           val1 = str[start:end]
02022         self.trajectory.multi_dof_joint_trajectory.joint_names.append(val1)
02023       start = end
02024       end += 4
02025       (length,) = _struct_I.unpack(str[start:end])
02026       self.trajectory.multi_dof_joint_trajectory.frame_ids = []
02027       for i in range(0, length):
02028         start = end
02029         end += 4
02030         (length,) = _struct_I.unpack(str[start:end])
02031         start = end
02032         end += length
02033         if python3:
02034           val1 = str[start:end].decode('utf-8')
02035         else:
02036           val1 = str[start:end]
02037         self.trajectory.multi_dof_joint_trajectory.frame_ids.append(val1)
02038       start = end
02039       end += 4
02040       (length,) = _struct_I.unpack(str[start:end])
02041       self.trajectory.multi_dof_joint_trajectory.child_frame_ids = []
02042       for i in range(0, length):
02043         start = end
02044         end += 4
02045         (length,) = _struct_I.unpack(str[start:end])
02046         start = end
02047         end += length
02048         if python3:
02049           val1 = str[start:end].decode('utf-8')
02050         else:
02051           val1 = str[start:end]
02052         self.trajectory.multi_dof_joint_trajectory.child_frame_ids.append(val1)
02053       start = end
02054       end += 4
02055       (length,) = _struct_I.unpack(str[start:end])
02056       self.trajectory.multi_dof_joint_trajectory.points = []
02057       for i in range(0, length):
02058         val1 = arm_navigation_msgs.msg.MultiDOFJointTrajectoryPoint()
02059         start = end
02060         end += 4
02061         (length,) = _struct_I.unpack(str[start:end])
02062         val1.poses = []
02063         for i in range(0, length):
02064           val2 = geometry_msgs.msg.Pose()
02065           _v146 = val2.position
02066           _x = _v146
02067           start = end
02068           end += 24
02069           (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02070           _v147 = val2.orientation
02071           _x = _v147
02072           start = end
02073           end += 32
02074           (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
02075           val1.poses.append(val2)
02076         _v148 = val1.time_from_start
02077         _x = _v148
02078         start = end
02079         end += 8
02080         (_x.secs, _x.nsecs,) = _struct_2i.unpack(str[start:end])
02081         self.trajectory.multi_dof_joint_trajectory.points.append(val1)
02082       _x = self
02083       start = end
02084       end += 12
02085       (_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])
02086       start = end
02087       end += 4
02088       (length,) = _struct_I.unpack(str[start:end])
02089       start = end
02090       end += length
02091       if python3:
02092         self.robot_state.joint_state.header.frame_id = str[start:end].decode('utf-8')
02093       else:
02094         self.robot_state.joint_state.header.frame_id = str[start:end]
02095       start = end
02096       end += 4
02097       (length,) = _struct_I.unpack(str[start:end])
02098       self.robot_state.joint_state.name = []
02099       for i in range(0, length):
02100         start = end
02101         end += 4
02102         (length,) = _struct_I.unpack(str[start:end])
02103         start = end
02104         end += length
02105         if python3:
02106           val1 = str[start:end].decode('utf-8')
02107         else:
02108           val1 = str[start:end]
02109         self.robot_state.joint_state.name.append(val1)
02110       start = end
02111       end += 4
02112       (length,) = _struct_I.unpack(str[start:end])
02113       pattern = '<%sd'%length
02114       start = end
02115       end += struct.calcsize(pattern)
02116       self.robot_state.joint_state.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
02117       start = end
02118       end += 4
02119       (length,) = _struct_I.unpack(str[start:end])
02120       pattern = '<%sd'%length
02121       start = end
02122       end += struct.calcsize(pattern)
02123       self.robot_state.joint_state.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
02124       start = end
02125       end += 4
02126       (length,) = _struct_I.unpack(str[start:end])
02127       pattern = '<%sd'%length
02128       start = end
02129       end += struct.calcsize(pattern)
02130       self.robot_state.joint_state.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
02131       _x = self
02132       start = end
02133       end += 8
02134       (_x.robot_state.multi_dof_joint_state.stamp.secs, _x.robot_state.multi_dof_joint_state.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
02135       start = end
02136       end += 4
02137       (length,) = _struct_I.unpack(str[start:end])
02138       self.robot_state.multi_dof_joint_state.joint_names = []
02139       for i in range(0, length):
02140         start = end
02141         end += 4
02142         (length,) = _struct_I.unpack(str[start:end])
02143         start = end
02144         end += length
02145         if python3:
02146           val1 = str[start:end].decode('utf-8')
02147         else:
02148           val1 = str[start:end]
02149         self.robot_state.multi_dof_joint_state.joint_names.append(val1)
02150       start = end
02151       end += 4
02152       (length,) = _struct_I.unpack(str[start:end])
02153       self.robot_state.multi_dof_joint_state.frame_ids = []
02154       for i in range(0, length):
02155         start = end
02156         end += 4
02157         (length,) = _struct_I.unpack(str[start:end])
02158         start = end
02159         end += length
02160         if python3:
02161           val1 = str[start:end].decode('utf-8')
02162         else:
02163           val1 = str[start:end]
02164         self.robot_state.multi_dof_joint_state.frame_ids.append(val1)
02165       start = end
02166       end += 4
02167       (length,) = _struct_I.unpack(str[start:end])
02168       self.robot_state.multi_dof_joint_state.child_frame_ids = []
02169       for i in range(0, length):
02170         start = end
02171         end += 4
02172         (length,) = _struct_I.unpack(str[start:end])
02173         start = end
02174         end += length
02175         if python3:
02176           val1 = str[start:end].decode('utf-8')
02177         else:
02178           val1 = str[start:end]
02179         self.robot_state.multi_dof_joint_state.child_frame_ids.append(val1)
02180       start = end
02181       end += 4
02182       (length,) = _struct_I.unpack(str[start:end])
02183       self.robot_state.multi_dof_joint_state.poses = []
02184       for i in range(0, length):
02185         val1 = geometry_msgs.msg.Pose()
02186         _v149 = val1.position
02187         _x = _v149
02188         start = end
02189         end += 24
02190         (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02191         _v150 = val1.orientation
02192         _x = _v150
02193         start = end
02194         end += 32
02195         (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
02196         self.robot_state.multi_dof_joint_state.poses.append(val1)
02197       _x = self
02198       start = end
02199       end += 5
02200       (_x.check_collisions, _x.check_path_constraints, _x.check_goal_constraints, _x.check_joint_limits, _x.check_full_trajectory,) = _struct_5B.unpack(str[start:end])
02201       self.check_collisions = bool(self.check_collisions)
02202       self.check_path_constraints = bool(self.check_path_constraints)
02203       self.check_goal_constraints = bool(self.check_goal_constraints)
02204       self.check_joint_limits = bool(self.check_joint_limits)
02205       self.check_full_trajectory = bool(self.check_full_trajectory)
02206       start = end
02207       end += 4
02208       (length,) = _struct_I.unpack(str[start:end])
02209       self.path_constraints.joint_constraints = []
02210       for i in range(0, length):
02211         val1 = arm_navigation_msgs.msg.JointConstraint()
02212         start = end
02213         end += 4
02214         (length,) = _struct_I.unpack(str[start:end])
02215         start = end
02216         end += length
02217         if python3:
02218           val1.joint_name = str[start:end].decode('utf-8')
02219         else:
02220           val1.joint_name = str[start:end]
02221         _x = val1
02222         start = end
02223         end += 32
02224         (_x.position, _x.tolerance_above, _x.tolerance_below, _x.weight,) = _struct_4d.unpack(str[start:end])
02225         self.path_constraints.joint_constraints.append(val1)
02226       start = end
02227       end += 4
02228       (length,) = _struct_I.unpack(str[start:end])
02229       self.path_constraints.position_constraints = []
02230       for i in range(0, length):
02231         val1 = arm_navigation_msgs.msg.PositionConstraint()
02232         _v151 = val1.header
02233         start = end
02234         end += 4
02235         (_v151.seq,) = _struct_I.unpack(str[start:end])
02236         _v152 = _v151.stamp
02237         _x = _v152
02238         start = end
02239         end += 8
02240         (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02241         start = end
02242         end += 4
02243         (length,) = _struct_I.unpack(str[start:end])
02244         start = end
02245         end += length
02246         if python3:
02247           _v151.frame_id = str[start:end].decode('utf-8')
02248         else:
02249           _v151.frame_id = str[start:end]
02250         start = end
02251         end += 4
02252         (length,) = _struct_I.unpack(str[start:end])
02253         start = end
02254         end += length
02255         if python3:
02256           val1.link_name = str[start:end].decode('utf-8')
02257         else:
02258           val1.link_name = str[start:end]
02259         _v153 = val1.target_point_offset
02260         _x = _v153
02261         start = end
02262         end += 24
02263         (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02264         _v154 = val1.position
02265         _x = _v154
02266         start = end
02267         end += 24
02268         (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02269         _v155 = val1.constraint_region_shape
02270         start = end
02271         end += 1
02272         (_v155.type,) = _struct_b.unpack(str[start:end])
02273         start = end
02274         end += 4
02275         (length,) = _struct_I.unpack(str[start:end])
02276         pattern = '<%sd'%length
02277         start = end
02278         end += struct.calcsize(pattern)
02279         _v155.dimensions = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
02280         start = end
02281         end += 4
02282         (length,) = _struct_I.unpack(str[start:end])
02283         pattern = '<%si'%length
02284         start = end
02285         end += struct.calcsize(pattern)
02286         _v155.triangles = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
02287         start = end
02288         end += 4
02289         (length,) = _struct_I.unpack(str[start:end])
02290         _v155.vertices = []
02291         for i in range(0, length):
02292           val3 = geometry_msgs.msg.Point()
02293           _x = val3
02294           start = end
02295           end += 24
02296           (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02297           _v155.vertices.append(val3)
02298         _v156 = val1.constraint_region_orientation
02299         _x = _v156
02300         start = end
02301         end += 32
02302         (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
02303         start = end
02304         end += 8
02305         (val1.weight,) = _struct_d.unpack(str[start:end])
02306         self.path_constraints.position_constraints.append(val1)
02307       start = end
02308       end += 4
02309       (length,) = _struct_I.unpack(str[start:end])
02310       self.path_constraints.orientation_constraints = []
02311       for i in range(0, length):
02312         val1 = arm_navigation_msgs.msg.OrientationConstraint()
02313         _v157 = val1.header
02314         start = end
02315         end += 4
02316         (_v157.seq,) = _struct_I.unpack(str[start:end])
02317         _v158 = _v157.stamp
02318         _x = _v158
02319         start = end
02320         end += 8
02321         (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02322         start = end
02323         end += 4
02324         (length,) = _struct_I.unpack(str[start:end])
02325         start = end
02326         end += length
02327         if python3:
02328           _v157.frame_id = str[start:end].decode('utf-8')
02329         else:
02330           _v157.frame_id = str[start:end]
02331         start = end
02332         end += 4
02333         (length,) = _struct_I.unpack(str[start:end])
02334         start = end
02335         end += length
02336         if python3:
02337           val1.link_name = str[start:end].decode('utf-8')
02338         else:
02339           val1.link_name = str[start:end]
02340         start = end
02341         end += 4
02342         (val1.type,) = _struct_i.unpack(str[start:end])
02343         _v159 = val1.orientation
02344         _x = _v159
02345         start = end
02346         end += 32
02347         (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
02348         _x = val1
02349         start = end
02350         end += 32
02351         (_x.absolute_roll_tolerance, _x.absolute_pitch_tolerance, _x.absolute_yaw_tolerance, _x.weight,) = _struct_4d.unpack(str[start:end])
02352         self.path_constraints.orientation_constraints.append(val1)
02353       start = end
02354       end += 4
02355       (length,) = _struct_I.unpack(str[start:end])
02356       self.path_constraints.visibility_constraints = []
02357       for i in range(0, length):
02358         val1 = arm_navigation_msgs.msg.VisibilityConstraint()
02359         _v160 = val1.header
02360         start = end
02361         end += 4
02362         (_v160.seq,) = _struct_I.unpack(str[start:end])
02363         _v161 = _v160.stamp
02364         _x = _v161
02365         start = end
02366         end += 8
02367         (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02368         start = end
02369         end += 4
02370         (length,) = _struct_I.unpack(str[start:end])
02371         start = end
02372         end += length
02373         if python3:
02374           _v160.frame_id = str[start:end].decode('utf-8')
02375         else:
02376           _v160.frame_id = str[start:end]
02377         _v162 = val1.target
02378         _v163 = _v162.header
02379         start = end
02380         end += 4
02381         (_v163.seq,) = _struct_I.unpack(str[start:end])
02382         _v164 = _v163.stamp
02383         _x = _v164
02384         start = end
02385         end += 8
02386         (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02387         start = end
02388         end += 4
02389         (length,) = _struct_I.unpack(str[start:end])
02390         start = end
02391         end += length
02392         if python3:
02393           _v163.frame_id = str[start:end].decode('utf-8')
02394         else:
02395           _v163.frame_id = str[start:end]
02396         _v165 = _v162.point
02397         _x = _v165
02398         start = end
02399         end += 24
02400         (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02401         _v166 = val1.sensor_pose
02402         _v167 = _v166.header
02403         start = end
02404         end += 4
02405         (_v167.seq,) = _struct_I.unpack(str[start:end])
02406         _v168 = _v167.stamp
02407         _x = _v168
02408         start = end
02409         end += 8
02410         (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02411         start = end
02412         end += 4
02413         (length,) = _struct_I.unpack(str[start:end])
02414         start = end
02415         end += length
02416         if python3:
02417           _v167.frame_id = str[start:end].decode('utf-8')
02418         else:
02419           _v167.frame_id = str[start:end]
02420         _v169 = _v166.pose
02421         _v170 = _v169.position
02422         _x = _v170
02423         start = end
02424         end += 24
02425         (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02426         _v171 = _v169.orientation
02427         _x = _v171
02428         start = end
02429         end += 32
02430         (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
02431         start = end
02432         end += 8
02433         (val1.absolute_tolerance,) = _struct_d.unpack(str[start:end])
02434         self.path_constraints.visibility_constraints.append(val1)
02435       start = end
02436       end += 4
02437       (length,) = _struct_I.unpack(str[start:end])
02438       self.goal_constraints.joint_constraints = []
02439       for i in range(0, length):
02440         val1 = arm_navigation_msgs.msg.JointConstraint()
02441         start = end
02442         end += 4
02443         (length,) = _struct_I.unpack(str[start:end])
02444         start = end
02445         end += length
02446         if python3:
02447           val1.joint_name = str[start:end].decode('utf-8')
02448         else:
02449           val1.joint_name = str[start:end]
02450         _x = val1
02451         start = end
02452         end += 32
02453         (_x.position, _x.tolerance_above, _x.tolerance_below, _x.weight,) = _struct_4d.unpack(str[start:end])
02454         self.goal_constraints.joint_constraints.append(val1)
02455       start = end
02456       end += 4
02457       (length,) = _struct_I.unpack(str[start:end])
02458       self.goal_constraints.position_constraints = []
02459       for i in range(0, length):
02460         val1 = arm_navigation_msgs.msg.PositionConstraint()
02461         _v172 = val1.header
02462         start = end
02463         end += 4
02464         (_v172.seq,) = _struct_I.unpack(str[start:end])
02465         _v173 = _v172.stamp
02466         _x = _v173
02467         start = end
02468         end += 8
02469         (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02470         start = end
02471         end += 4
02472         (length,) = _struct_I.unpack(str[start:end])
02473         start = end
02474         end += length
02475         if python3:
02476           _v172.frame_id = str[start:end].decode('utf-8')
02477         else:
02478           _v172.frame_id = str[start:end]
02479         start = end
02480         end += 4
02481         (length,) = _struct_I.unpack(str[start:end])
02482         start = end
02483         end += length
02484         if python3:
02485           val1.link_name = str[start:end].decode('utf-8')
02486         else:
02487           val1.link_name = str[start:end]
02488         _v174 = val1.target_point_offset
02489         _x = _v174
02490         start = end
02491         end += 24
02492         (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02493         _v175 = val1.position
02494         _x = _v175
02495         start = end
02496         end += 24
02497         (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02498         _v176 = val1.constraint_region_shape
02499         start = end
02500         end += 1
02501         (_v176.type,) = _struct_b.unpack(str[start:end])
02502         start = end
02503         end += 4
02504         (length,) = _struct_I.unpack(str[start:end])
02505         pattern = '<%sd'%length
02506         start = end
02507         end += struct.calcsize(pattern)
02508         _v176.dimensions = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
02509         start = end
02510         end += 4
02511         (length,) = _struct_I.unpack(str[start:end])
02512         pattern = '<%si'%length
02513         start = end
02514         end += struct.calcsize(pattern)
02515         _v176.triangles = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
02516         start = end
02517         end += 4
02518         (length,) = _struct_I.unpack(str[start:end])
02519         _v176.vertices = []
02520         for i in range(0, length):
02521           val3 = geometry_msgs.msg.Point()
02522           _x = val3
02523           start = end
02524           end += 24
02525           (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02526           _v176.vertices.append(val3)
02527         _v177 = val1.constraint_region_orientation
02528         _x = _v177
02529         start = end
02530         end += 32
02531         (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
02532         start = end
02533         end += 8
02534         (val1.weight,) = _struct_d.unpack(str[start:end])
02535         self.goal_constraints.position_constraints.append(val1)
02536       start = end
02537       end += 4
02538       (length,) = _struct_I.unpack(str[start:end])
02539       self.goal_constraints.orientation_constraints = []
02540       for i in range(0, length):
02541         val1 = arm_navigation_msgs.msg.OrientationConstraint()
02542         _v178 = val1.header
02543         start = end
02544         end += 4
02545         (_v178.seq,) = _struct_I.unpack(str[start:end])
02546         _v179 = _v178.stamp
02547         _x = _v179
02548         start = end
02549         end += 8
02550         (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02551         start = end
02552         end += 4
02553         (length,) = _struct_I.unpack(str[start:end])
02554         start = end
02555         end += length
02556         if python3:
02557           _v178.frame_id = str[start:end].decode('utf-8')
02558         else:
02559           _v178.frame_id = str[start:end]
02560         start = end
02561         end += 4
02562         (length,) = _struct_I.unpack(str[start:end])
02563         start = end
02564         end += length
02565         if python3:
02566           val1.link_name = str[start:end].decode('utf-8')
02567         else:
02568           val1.link_name = str[start:end]
02569         start = end
02570         end += 4
02571         (val1.type,) = _struct_i.unpack(str[start:end])
02572         _v180 = val1.orientation
02573         _x = _v180
02574         start = end
02575         end += 32
02576         (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
02577         _x = val1
02578         start = end
02579         end += 32
02580         (_x.absolute_roll_tolerance, _x.absolute_pitch_tolerance, _x.absolute_yaw_tolerance, _x.weight,) = _struct_4d.unpack(str[start:end])
02581         self.goal_constraints.orientation_constraints.append(val1)
02582       start = end
02583       end += 4
02584       (length,) = _struct_I.unpack(str[start:end])
02585       self.goal_constraints.visibility_constraints = []
02586       for i in range(0, length):
02587         val1 = arm_navigation_msgs.msg.VisibilityConstraint()
02588         _v181 = val1.header
02589         start = end
02590         end += 4
02591         (_v181.seq,) = _struct_I.unpack(str[start:end])
02592         _v182 = _v181.stamp
02593         _x = _v182
02594         start = end
02595         end += 8
02596         (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02597         start = end
02598         end += 4
02599         (length,) = _struct_I.unpack(str[start:end])
02600         start = end
02601         end += length
02602         if python3:
02603           _v181.frame_id = str[start:end].decode('utf-8')
02604         else:
02605           _v181.frame_id = str[start:end]
02606         _v183 = val1.target
02607         _v184 = _v183.header
02608         start = end
02609         end += 4
02610         (_v184.seq,) = _struct_I.unpack(str[start:end])
02611         _v185 = _v184.stamp
02612         _x = _v185
02613         start = end
02614         end += 8
02615         (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02616         start = end
02617         end += 4
02618         (length,) = _struct_I.unpack(str[start:end])
02619         start = end
02620         end += length
02621         if python3:
02622           _v184.frame_id = str[start:end].decode('utf-8')
02623         else:
02624           _v184.frame_id = str[start:end]
02625         _v186 = _v183.point
02626         _x = _v186
02627         start = end
02628         end += 24
02629         (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02630         _v187 = val1.sensor_pose
02631         _v188 = _v187.header
02632         start = end
02633         end += 4
02634         (_v188.seq,) = _struct_I.unpack(str[start:end])
02635         _v189 = _v188.stamp
02636         _x = _v189
02637         start = end
02638         end += 8
02639         (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02640         start = end
02641         end += 4
02642         (length,) = _struct_I.unpack(str[start:end])
02643         start = end
02644         end += length
02645         if python3:
02646           _v188.frame_id = str[start:end].decode('utf-8')
02647         else:
02648           _v188.frame_id = str[start:end]
02649         _v190 = _v187.pose
02650         _v191 = _v190.position
02651         _x = _v191
02652         start = end
02653         end += 24
02654         (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02655         _v192 = _v190.orientation
02656         _x = _v192
02657         start = end
02658         end += 32
02659         (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
02660         start = end
02661         end += 8
02662         (val1.absolute_tolerance,) = _struct_d.unpack(str[start:end])
02663         self.goal_constraints.visibility_constraints.append(val1)
02664       return self
02665     except struct.error as e:
02666       raise genpy.DeserializationError(e) #most likely buffer underfill
02667 
02668 _struct_I = genpy.struct_I
02669 _struct_b = struct.Struct("<b")
02670 _struct_d = struct.Struct("<d")
02671 _struct_2I = struct.Struct("<2I")
02672 _struct_i = struct.Struct("<i")
02673 _struct_3I = struct.Struct("<3I")
02674 _struct_4d = struct.Struct("<4d")
02675 _struct_3d = struct.Struct("<3d")
02676 _struct_2i = struct.Struct("<2i")
02677 _struct_5B = struct.Struct("<5B")
02678 """autogenerated by genpy from arm_navigation_msgs/GetRobotTrajectoryValidityResponse.msg. Do not edit."""
02679 import sys
02680 python3 = True if sys.hexversion > 0x03000000 else False
02681 import genpy
02682 import struct
02683 
02684 import arm_navigation_msgs.msg
02685 
02686 class GetRobotTrajectoryValidityResponse(genpy.Message):
02687   _md5sum = "6316c11e993d068a87f32bc2109e7658"
02688   _type = "arm_navigation_msgs/GetRobotTrajectoryValidityResponse"
02689   _has_header = False #flag to mark the presence of a Header object
02690   _full_text = """
02691 
02692 
02693 
02694 arm_navigation_msgs/ArmNavigationErrorCodes error_code
02695 
02696 
02697 
02698 
02699 arm_navigation_msgs/ArmNavigationErrorCodes[] trajectory_error_codes
02700 
02701 
02702 ================================================================================
02703 MSG: arm_navigation_msgs/ArmNavigationErrorCodes
02704 int32 val
02705 
02706 # overall behavior
02707 int32 PLANNING_FAILED=-1
02708 int32 SUCCESS=1
02709 int32 TIMED_OUT=-2
02710 
02711 # start state errors
02712 int32 START_STATE_IN_COLLISION=-3
02713 int32 START_STATE_VIOLATES_PATH_CONSTRAINTS=-4
02714 
02715 # goal errors
02716 int32 GOAL_IN_COLLISION=-5
02717 int32 GOAL_VIOLATES_PATH_CONSTRAINTS=-6
02718 
02719 # robot state
02720 int32 INVALID_ROBOT_STATE=-7
02721 int32 INCOMPLETE_ROBOT_STATE=-8
02722 
02723 # planning request errors
02724 int32 INVALID_PLANNER_ID=-9
02725 int32 INVALID_NUM_PLANNING_ATTEMPTS=-10
02726 int32 INVALID_ALLOWED_PLANNING_TIME=-11
02727 int32 INVALID_GROUP_NAME=-12
02728 int32 INVALID_GOAL_JOINT_CONSTRAINTS=-13
02729 int32 INVALID_GOAL_POSITION_CONSTRAINTS=-14
02730 int32 INVALID_GOAL_ORIENTATION_CONSTRAINTS=-15
02731 int32 INVALID_PATH_JOINT_CONSTRAINTS=-16
02732 int32 INVALID_PATH_POSITION_CONSTRAINTS=-17
02733 int32 INVALID_PATH_ORIENTATION_CONSTRAINTS=-18
02734 
02735 # state/trajectory monitor errors
02736 int32 INVALID_TRAJECTORY=-19
02737 int32 INVALID_INDEX=-20
02738 int32 JOINT_LIMITS_VIOLATED=-21
02739 int32 PATH_CONSTRAINTS_VIOLATED=-22
02740 int32 COLLISION_CONSTRAINTS_VIOLATED=-23
02741 int32 GOAL_CONSTRAINTS_VIOLATED=-24
02742 int32 JOINTS_NOT_MOVING=-25
02743 int32 TRAJECTORY_CONTROLLER_FAILED=-26
02744 
02745 # system errors
02746 int32 FRAME_TRANSFORM_FAILURE=-27
02747 int32 COLLISION_CHECKING_UNAVAILABLE=-28
02748 int32 ROBOT_STATE_STALE=-29
02749 int32 SENSOR_INFO_STALE=-30
02750 
02751 # kinematics errors
02752 int32 NO_IK_SOLUTION=-31
02753 int32 INVALID_LINK_NAME=-32
02754 int32 IK_LINK_IN_COLLISION=-33
02755 int32 NO_FK_SOLUTION=-34
02756 int32 KINEMATICS_STATE_IN_COLLISION=-35
02757 
02758 # general errors
02759 int32 INVALID_TIMEOUT=-36
02760 
02761 
02762 """
02763   __slots__ = ['error_code','trajectory_error_codes']
02764   _slot_types = ['arm_navigation_msgs/ArmNavigationErrorCodes','arm_navigation_msgs/ArmNavigationErrorCodes[]']
02765 
02766   def __init__(self, *args, **kwds):
02767     """
02768     Constructor. Any message fields that are implicitly/explicitly
02769     set to None will be assigned a default value. The recommend
02770     use is keyword arguments as this is more robust to future message
02771     changes.  You cannot mix in-order arguments and keyword arguments.
02772 
02773     The available fields are:
02774        error_code,trajectory_error_codes
02775 
02776     :param args: complete set of field values, in .msg order
02777     :param kwds: use keyword arguments corresponding to message field names
02778     to set specific fields.
02779     """
02780     if args or kwds:
02781       super(GetRobotTrajectoryValidityResponse, self).__init__(*args, **kwds)
02782       #message fields cannot be None, assign default values for those that are
02783       if self.error_code is None:
02784         self.error_code = arm_navigation_msgs.msg.ArmNavigationErrorCodes()
02785       if self.trajectory_error_codes is None:
02786         self.trajectory_error_codes = []
02787     else:
02788       self.error_code = arm_navigation_msgs.msg.ArmNavigationErrorCodes()
02789       self.trajectory_error_codes = []
02790 
02791   def _get_types(self):
02792     """
02793     internal API method
02794     """
02795     return self._slot_types
02796 
02797   def serialize(self, buff):
02798     """
02799     serialize message into buffer
02800     :param buff: buffer, ``StringIO``
02801     """
02802     try:
02803       buff.write(_struct_i.pack(self.error_code.val))
02804       length = len(self.trajectory_error_codes)
02805       buff.write(_struct_I.pack(length))
02806       for val1 in self.trajectory_error_codes:
02807         buff.write(_struct_i.pack(val1.val))
02808     except struct.error as se: self._check_types(se)
02809     except TypeError as te: self._check_types(te)
02810 
02811   def deserialize(self, str):
02812     """
02813     unpack serialized message in str into this message instance
02814     :param str: byte array of serialized message, ``str``
02815     """
02816     try:
02817       if self.error_code is None:
02818         self.error_code = arm_navigation_msgs.msg.ArmNavigationErrorCodes()
02819       if self.trajectory_error_codes is None:
02820         self.trajectory_error_codes = None
02821       end = 0
02822       start = end
02823       end += 4
02824       (self.error_code.val,) = _struct_i.unpack(str[start:end])
02825       start = end
02826       end += 4
02827       (length,) = _struct_I.unpack(str[start:end])
02828       self.trajectory_error_codes = []
02829       for i in range(0, length):
02830         val1 = arm_navigation_msgs.msg.ArmNavigationErrorCodes()
02831         start = end
02832         end += 4
02833         (val1.val,) = _struct_i.unpack(str[start:end])
02834         self.trajectory_error_codes.append(val1)
02835       return self
02836     except struct.error as e:
02837       raise genpy.DeserializationError(e) #most likely buffer underfill
02838 
02839 
02840   def serialize_numpy(self, buff, numpy):
02841     """
02842     serialize message with numpy array types into buffer
02843     :param buff: buffer, ``StringIO``
02844     :param numpy: numpy python module
02845     """
02846     try:
02847       buff.write(_struct_i.pack(self.error_code.val))
02848       length = len(self.trajectory_error_codes)
02849       buff.write(_struct_I.pack(length))
02850       for val1 in self.trajectory_error_codes:
02851         buff.write(_struct_i.pack(val1.val))
02852     except struct.error as se: self._check_types(se)
02853     except TypeError as te: self._check_types(te)
02854 
02855   def deserialize_numpy(self, str, numpy):
02856     """
02857     unpack serialized message in str into this message instance using numpy for array types
02858     :param str: byte array of serialized message, ``str``
02859     :param numpy: numpy python module
02860     """
02861     try:
02862       if self.error_code is None:
02863         self.error_code = arm_navigation_msgs.msg.ArmNavigationErrorCodes()
02864       if self.trajectory_error_codes is None:
02865         self.trajectory_error_codes = None
02866       end = 0
02867       start = end
02868       end += 4
02869       (self.error_code.val,) = _struct_i.unpack(str[start:end])
02870       start = end
02871       end += 4
02872       (length,) = _struct_I.unpack(str[start:end])
02873       self.trajectory_error_codes = []
02874       for i in range(0, length):
02875         val1 = arm_navigation_msgs.msg.ArmNavigationErrorCodes()
02876         start = end
02877         end += 4
02878         (val1.val,) = _struct_i.unpack(str[start:end])
02879         self.trajectory_error_codes.append(val1)
02880       return self
02881     except struct.error as e:
02882       raise genpy.DeserializationError(e) #most likely buffer underfill
02883 
02884 _struct_I = genpy.struct_I
02885 _struct_i = struct.Struct("<i")
02886 class GetRobotTrajectoryValidity(object):
02887   _type          = 'arm_navigation_msgs/GetRobotTrajectoryValidity'
02888   _md5sum = '2b18e8353390aab164a69813e3458fdb'
02889   _request_class  = GetRobotTrajectoryValidityRequest
02890   _response_class = GetRobotTrajectoryValidityResponse


arm_navigation_msgs
Author(s): Gil Jones
autogenerated on Mon Dec 2 2013 12:31:50