$search
00001 """autogenerated by genmsg_py from MoveArmActionGoal.msg. Do not edit.""" 00002 import roslib.message 00003 import struct 00004 00005 import arm_navigation_msgs.msg 00006 import roslib.rostime 00007 import actionlib_msgs.msg 00008 import geometry_msgs.msg 00009 import sensor_msgs.msg 00010 import std_msgs.msg 00011 00012 class MoveArmActionGoal(roslib.message.Message): 00013 _md5sum = "d560cc046d6b5e8bf3f70a960054d6c6" 00014 _type = "arm_navigation_msgs/MoveArmActionGoal" 00015 _has_header = True #flag to mark the presence of a Header object 00016 _full_text = """# ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ====== 00017 00018 Header header 00019 actionlib_msgs/GoalID goal_id 00020 MoveArmGoal goal 00021 00022 ================================================================================ 00023 MSG: std_msgs/Header 00024 # Standard metadata for higher-level stamped data types. 00025 # This is generally used to communicate timestamped data 00026 # in a particular coordinate frame. 00027 # 00028 # sequence ID: consecutively increasing ID 00029 uint32 seq 00030 #Two-integer timestamp that is expressed as: 00031 # * stamp.secs: seconds (stamp_secs) since epoch 00032 # * stamp.nsecs: nanoseconds since stamp_secs 00033 # time-handling sugar is provided by the client library 00034 time stamp 00035 #Frame this data is associated with 00036 # 0: no frame 00037 # 1: global frame 00038 string frame_id 00039 00040 ================================================================================ 00041 MSG: actionlib_msgs/GoalID 00042 # The stamp should store the time at which this goal was requested. 00043 # It is used by an action server when it tries to preempt all 00044 # goals that were requested before a certain time 00045 time stamp 00046 00047 # The id provides a way to associate feedback and 00048 # result message with specific goal requests. The id 00049 # specified must be unique. 00050 string id 00051 00052 00053 ================================================================================ 00054 MSG: arm_navigation_msgs/MoveArmGoal 00055 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ====== 00056 # Service name to call for getting a motion plan 00057 # Move arm will call a service on this service name 00058 # using the MotionPlanRequest specified here 00059 string planner_service_name 00060 00061 # A planning scene diff 00062 PlanningScene planning_scene_diff 00063 00064 # A motion planning request 00065 MotionPlanRequest motion_plan_request 00066 00067 # OPTIONAL: Diff uses ordered collision operations in addition to allowed_collision_matrix 00068 arm_navigation_msgs/OrderedCollisionOperations operations 00069 00070 # OPTIONAL flag 00071 # Setting this flag to true will allow move_arm to accept plans that do not go all the way to the goal 00072 bool accept_partial_plans 00073 00074 # OPTIONAL flag 00075 # Setting this flag to true will allow move_arm to accept invalid goals 00076 # This is useful if you are using a planner like CHOMP along with a noisy rapidly changing collision map 00077 # and you would like to plan to a goal near an object. 00078 bool accept_invalid_goals 00079 00080 # OPTIONAL flag 00081 # Setting this flag to true will disable the call to IK for a pose goal 00082 bool disable_ik 00083 00084 # OPTIONAL flag 00085 # Setting this flag to true will disable collision monitoring during execution of a trajectory 00086 bool disable_collision_monitoring 00087 00088 ================================================================================ 00089 MSG: arm_navigation_msgs/PlanningScene 00090 #full robot state 00091 arm_navigation_msgs/RobotState robot_state 00092 00093 #additional frames for duplicating tf 00094 geometry_msgs/TransformStamped[] fixed_frame_transforms 00095 00096 #full allowed collision matrix 00097 AllowedCollisionMatrix allowed_collision_matrix 00098 00099 #allowed contacts 00100 arm_navigation_msgs/AllowedContactSpecification[] allowed_contacts 00101 00102 #all link paddings 00103 arm_navigation_msgs/LinkPadding[] link_padding 00104 00105 #collision objects 00106 arm_navigation_msgs/CollisionObject[] collision_objects 00107 arm_navigation_msgs/AttachedCollisionObject[] attached_collision_objects 00108 00109 #the collision map 00110 arm_navigation_msgs/CollisionMap collision_map 00111 00112 ================================================================================ 00113 MSG: arm_navigation_msgs/RobotState 00114 # This message contains information about the robot state, i.e. the positions of its joints and links 00115 sensor_msgs/JointState joint_state 00116 arm_navigation_msgs/MultiDOFJointState multi_dof_joint_state 00117 00118 ================================================================================ 00119 MSG: sensor_msgs/JointState 00120 # This is a message that holds data to describe the state of a set of torque controlled joints. 00121 # 00122 # The state of each joint (revolute or prismatic) is defined by: 00123 # * the position of the joint (rad or m), 00124 # * the velocity of the joint (rad/s or m/s) and 00125 # * the effort that is applied in the joint (Nm or N). 00126 # 00127 # Each joint is uniquely identified by its name 00128 # The header specifies the time at which the joint states were recorded. All the joint states 00129 # in one message have to be recorded at the same time. 00130 # 00131 # This message consists of a multiple arrays, one for each part of the joint state. 00132 # The goal is to make each of the fields optional. When e.g. your joints have no 00133 # effort associated with them, you can leave the effort array empty. 00134 # 00135 # All arrays in this message should have the same size, or be empty. 00136 # This is the only way to uniquely associate the joint name with the correct 00137 # states. 00138 00139 00140 Header header 00141 00142 string[] name 00143 float64[] position 00144 float64[] velocity 00145 float64[] effort 00146 00147 ================================================================================ 00148 MSG: arm_navigation_msgs/MultiDOFJointState 00149 #A representation of a multi-dof joint state 00150 time stamp 00151 string[] joint_names 00152 string[] frame_ids 00153 string[] child_frame_ids 00154 geometry_msgs/Pose[] poses 00155 00156 ================================================================================ 00157 MSG: geometry_msgs/Pose 00158 # A representation of pose in free space, composed of postion and orientation. 00159 Point position 00160 Quaternion orientation 00161 00162 ================================================================================ 00163 MSG: geometry_msgs/Point 00164 # This contains the position of a point in free space 00165 float64 x 00166 float64 y 00167 float64 z 00168 00169 ================================================================================ 00170 MSG: geometry_msgs/Quaternion 00171 # This represents an orientation in free space in quaternion form. 00172 00173 float64 x 00174 float64 y 00175 float64 z 00176 float64 w 00177 00178 ================================================================================ 00179 MSG: geometry_msgs/TransformStamped 00180 # This expresses a transform from coordinate frame header.frame_id 00181 # to the coordinate frame child_frame_id 00182 # 00183 # This message is mostly used by the 00184 # <a href="http://www.ros.org/wiki/tf">tf</a> package. 00185 # See it's documentation for more information. 00186 00187 Header header 00188 string child_frame_id # the frame id of the child frame 00189 Transform transform 00190 00191 ================================================================================ 00192 MSG: geometry_msgs/Transform 00193 # This represents the transform between two coordinate frames in free space. 00194 00195 Vector3 translation 00196 Quaternion rotation 00197 00198 ================================================================================ 00199 MSG: geometry_msgs/Vector3 00200 # This represents a vector in free space. 00201 00202 float64 x 00203 float64 y 00204 float64 z 00205 ================================================================================ 00206 MSG: arm_navigation_msgs/AllowedCollisionMatrix 00207 # the list of link names in the matrix 00208 string[] link_names 00209 00210 # the individual entries in the allowed collision matrix 00211 # symmetric, with same order as link_names 00212 AllowedCollisionEntry[] entries 00213 00214 ================================================================================ 00215 MSG: arm_navigation_msgs/AllowedCollisionEntry 00216 # whether or not collision checking is enabled 00217 bool[] enabled 00218 00219 ================================================================================ 00220 MSG: arm_navigation_msgs/AllowedContactSpecification 00221 # The names of the regions 00222 string name 00223 00224 # The shape of the region in the environment 00225 arm_navigation_msgs/Shape shape 00226 00227 # The pose of the space defining the region 00228 geometry_msgs/PoseStamped pose_stamped 00229 00230 # The set of links that will be allowed to have penetration contact within this region 00231 string[] link_names 00232 00233 # The maximum penetration depth allowed for every link 00234 float64 penetration_depth 00235 00236 ================================================================================ 00237 MSG: arm_navigation_msgs/Shape 00238 byte SPHERE=0 00239 byte BOX=1 00240 byte CYLINDER=2 00241 byte MESH=3 00242 00243 byte type 00244 00245 00246 #### define sphere, box, cylinder #### 00247 # the origin of each shape is considered at the shape's center 00248 00249 # for sphere 00250 # radius := dimensions[0] 00251 00252 # for cylinder 00253 # radius := dimensions[0] 00254 # length := dimensions[1] 00255 # the length is along the Z axis 00256 00257 # for box 00258 # size_x := dimensions[0] 00259 # size_y := dimensions[1] 00260 # size_z := dimensions[2] 00261 float64[] dimensions 00262 00263 00264 #### define mesh #### 00265 00266 # list of triangles; triangle k is defined by tre vertices located 00267 # at indices triangles[3k], triangles[3k+1], triangles[3k+2] 00268 int32[] triangles 00269 geometry_msgs/Point[] vertices 00270 00271 ================================================================================ 00272 MSG: geometry_msgs/PoseStamped 00273 # A Pose with reference coordinate frame and timestamp 00274 Header header 00275 Pose pose 00276 00277 ================================================================================ 00278 MSG: arm_navigation_msgs/LinkPadding 00279 #name for the link 00280 string link_name 00281 00282 # padding to apply to the link 00283 float64 padding 00284 00285 ================================================================================ 00286 MSG: arm_navigation_msgs/CollisionObject 00287 # a header, used for interpreting the poses 00288 Header header 00289 00290 # the id of the object 00291 string id 00292 00293 # The padding used for filtering points near the object. 00294 # This does not affect collision checking for the object. 00295 # Set to negative to get zero padding. 00296 float32 padding 00297 00298 #This contains what is to be done with the object 00299 CollisionObjectOperation operation 00300 00301 #the shapes associated with the object 00302 arm_navigation_msgs/Shape[] shapes 00303 00304 #the poses associated with the shapes - will be transformed using the header 00305 geometry_msgs/Pose[] poses 00306 00307 ================================================================================ 00308 MSG: arm_navigation_msgs/CollisionObjectOperation 00309 #Puts the object into the environment 00310 #or updates the object if already added 00311 byte ADD=0 00312 00313 #Removes the object from the environment entirely 00314 byte REMOVE=1 00315 00316 #Only valid within the context of a CollisionAttachedObject message 00317 #Will be ignored if sent with an CollisionObject message 00318 #Takes an attached object, detaches from the attached link 00319 #But adds back in as regular object 00320 byte DETACH_AND_ADD_AS_OBJECT=2 00321 00322 #Only valid within the context of a CollisionAttachedObject message 00323 #Will be ignored if sent with an CollisionObject message 00324 #Takes current object in the environment and removes it as 00325 #a regular object 00326 byte ATTACH_AND_REMOVE_AS_OBJECT=3 00327 00328 # Byte code for operation 00329 byte operation 00330 00331 ================================================================================ 00332 MSG: arm_navigation_msgs/AttachedCollisionObject 00333 # The CollisionObject will be attached with a fixed joint to this link 00334 # If link name is set to REMOVE_ALL_ATTACHED_OBJECTS and object.operation 00335 # is set to REMOVE will remove all attached bodies attached to any object 00336 string link_name 00337 00338 #Reserved for indicating that all attached objects should be removed 00339 string REMOVE_ALL_ATTACHED_OBJECTS = "all" 00340 00341 #This contains the actual shapes and poses for the CollisionObject 00342 #to be attached to the link 00343 #If action is remove and no object.id is set, all objects 00344 #attached to the link indicated by link_name will be removed 00345 CollisionObject object 00346 00347 # The set of links that the attached objects are allowed to touch 00348 # by default - the link_name is included by default 00349 string[] touch_links 00350 00351 ================================================================================ 00352 MSG: arm_navigation_msgs/CollisionMap 00353 #header for interpreting box positions 00354 Header header 00355 00356 #boxes for use in collision testing 00357 OrientedBoundingBox[] boxes 00358 00359 ================================================================================ 00360 MSG: arm_navigation_msgs/OrientedBoundingBox 00361 #the center of the box 00362 geometry_msgs/Point32 center 00363 00364 #the extents of the box, assuming the center is at the point 00365 geometry_msgs/Point32 extents 00366 00367 #the axis of the box 00368 geometry_msgs/Point32 axis 00369 00370 #the angle of rotation around the axis 00371 float32 angle 00372 00373 ================================================================================ 00374 MSG: geometry_msgs/Point32 00375 # This contains the position of a point in free space(with 32 bits of precision). 00376 # It is recommeded to use Point wherever possible instead of Point32. 00377 # 00378 # This recommendation is to promote interoperability. 00379 # 00380 # This message is designed to take up less space when sending 00381 # lots of points at once, as in the case of a PointCloud. 00382 00383 float32 x 00384 float32 y 00385 float32 z 00386 ================================================================================ 00387 MSG: arm_navigation_msgs/MotionPlanRequest 00388 # This service contains the definition for a request to the motion 00389 # planner and the output it provides 00390 00391 # Parameters for the workspace that the planner should work inside 00392 arm_navigation_msgs/WorkspaceParameters workspace_parameters 00393 00394 # Starting state updates. If certain joints should be considered 00395 # at positions other than the current ones, these positions should 00396 # be set here 00397 arm_navigation_msgs/RobotState start_state 00398 00399 # The goal state for the model to plan for. The goal is achieved 00400 # if all constraints are satisfied 00401 arm_navigation_msgs/Constraints goal_constraints 00402 00403 # No state at any point along the path in the produced motion plan will violate these constraints 00404 arm_navigation_msgs/Constraints path_constraints 00405 00406 # The name of the motion planner to use. If no name is specified, 00407 # a default motion planner will be used 00408 string planner_id 00409 00410 # The name of the group of joints on which this planner is operating 00411 string group_name 00412 00413 # The number of times this plan is to be computed. Shortest solution 00414 # will be reported. 00415 int32 num_planning_attempts 00416 00417 # The maximum amount of time the motion planner is allowed to plan for 00418 duration allowed_planning_time 00419 00420 # An expected path duration (in seconds) along with an expected discretization of the path allows the planner to determine the discretization of the trajectory that it returns 00421 duration expected_path_duration 00422 duration expected_path_dt 00423 00424 ================================================================================ 00425 MSG: arm_navigation_msgs/WorkspaceParameters 00426 # This message contains a set of parameters useful in 00427 # setting up the workspace for planning 00428 arm_navigation_msgs/Shape workspace_region_shape 00429 geometry_msgs/PoseStamped workspace_region_pose 00430 00431 00432 ================================================================================ 00433 MSG: arm_navigation_msgs/Constraints 00434 # This message contains a list of motion planning constraints. 00435 00436 arm_navigation_msgs/JointConstraint[] joint_constraints 00437 arm_navigation_msgs/PositionConstraint[] position_constraints 00438 arm_navigation_msgs/OrientationConstraint[] orientation_constraints 00439 arm_navigation_msgs/VisibilityConstraint[] visibility_constraints 00440 00441 ================================================================================ 00442 MSG: arm_navigation_msgs/JointConstraint 00443 # Constrain the position of a joint to be within a certain bound 00444 string joint_name 00445 00446 # the bound to be achieved is [position - tolerance_below, position + tolerance_above] 00447 float64 position 00448 float64 tolerance_above 00449 float64 tolerance_below 00450 00451 # A weighting factor for this constraint 00452 float64 weight 00453 ================================================================================ 00454 MSG: arm_navigation_msgs/PositionConstraint 00455 # This message contains the definition of a position constraint. 00456 Header header 00457 00458 # The robot link this constraint refers to 00459 string link_name 00460 00461 # The offset (in the link frame) for the target point on the link we are planning for 00462 geometry_msgs/Point target_point_offset 00463 00464 # The nominal/target position for the point we are planning for 00465 geometry_msgs/Point position 00466 00467 # The shape of the bounded region that constrains the position of the end-effector 00468 # This region is always centered at the position defined above 00469 arm_navigation_msgs/Shape constraint_region_shape 00470 00471 # The orientation of the bounded region that constrains the position of the end-effector. 00472 # This allows the specification of non-axis aligned constraints 00473 geometry_msgs/Quaternion constraint_region_orientation 00474 00475 # Constraint weighting factor - a weight for this constraint 00476 float64 weight 00477 00478 ================================================================================ 00479 MSG: arm_navigation_msgs/OrientationConstraint 00480 # This message contains the definition of an orientation constraint. 00481 Header header 00482 00483 # The robot link this constraint refers to 00484 string link_name 00485 00486 # The type of the constraint 00487 int32 type 00488 int32 LINK_FRAME=0 00489 int32 HEADER_FRAME=1 00490 00491 # The desired orientation of the robot link specified as a quaternion 00492 geometry_msgs/Quaternion orientation 00493 00494 # optional RPY error tolerances specified if 00495 float64 absolute_roll_tolerance 00496 float64 absolute_pitch_tolerance 00497 float64 absolute_yaw_tolerance 00498 00499 # Constraint weighting factor - a weight for this constraint 00500 float64 weight 00501 00502 ================================================================================ 00503 MSG: arm_navigation_msgs/VisibilityConstraint 00504 # This message contains the definition of a visibility constraint. 00505 Header header 00506 00507 # The point stamped target that needs to be kept within view of the sensor 00508 geometry_msgs/PointStamped target 00509 00510 # The local pose of the frame in which visibility is to be maintained 00511 # The frame id should represent the robot link to which the sensor is attached 00512 # The visual axis of the sensor is assumed to be along the X axis of this frame 00513 geometry_msgs/PoseStamped sensor_pose 00514 00515 # The deviation (in radians) that will be tolerated 00516 # Constraint error will be measured as the solid angle between the 00517 # X axis of the frame defined above and the vector between the origin 00518 # of the frame defined above and the target location 00519 float64 absolute_tolerance 00520 00521 00522 ================================================================================ 00523 MSG: geometry_msgs/PointStamped 00524 # This represents a Point with reference coordinate frame and timestamp 00525 Header header 00526 Point point 00527 00528 ================================================================================ 00529 MSG: arm_navigation_msgs/OrderedCollisionOperations 00530 # A set of collision operations that will be performed in the order they are specified 00531 CollisionOperation[] collision_operations 00532 ================================================================================ 00533 MSG: arm_navigation_msgs/CollisionOperation 00534 # A definition of a collision operation 00535 # E.g. ("gripper",COLLISION_SET_ALL,ENABLE) will enable collisions 00536 # between the gripper and all objects in the collision space 00537 00538 string object1 00539 string object2 00540 string COLLISION_SET_ALL="all" 00541 string COLLISION_SET_OBJECTS="objects" 00542 string COLLISION_SET_ATTACHED_OBJECTS="attached" 00543 00544 # The penetration distance to which collisions are allowed. This is 0.0 by default. 00545 float64 penetration_distance 00546 00547 # Flag that determines whether collisions will be enabled or disabled for the pair of objects specified above 00548 int32 operation 00549 int32 DISABLE=0 00550 int32 ENABLE=1 00551 00552 """ 00553 __slots__ = ['header','goal_id','goal'] 00554 _slot_types = ['Header','actionlib_msgs/GoalID','arm_navigation_msgs/MoveArmGoal'] 00555 00556 def __init__(self, *args, **kwds): 00557 """ 00558 Constructor. Any message fields that are implicitly/explicitly 00559 set to None will be assigned a default value. The recommend 00560 use is keyword arguments as this is more robust to future message 00561 changes. You cannot mix in-order arguments and keyword arguments. 00562 00563 The available fields are: 00564 header,goal_id,goal 00565 00566 @param args: complete set of field values, in .msg order 00567 @param kwds: use keyword arguments corresponding to message field names 00568 to set specific fields. 00569 """ 00570 if args or kwds: 00571 super(MoveArmActionGoal, self).__init__(*args, **kwds) 00572 #message fields cannot be None, assign default values for those that are 00573 if self.header is None: 00574 self.header = std_msgs.msg._Header.Header() 00575 if self.goal_id is None: 00576 self.goal_id = actionlib_msgs.msg.GoalID() 00577 if self.goal is None: 00578 self.goal = arm_navigation_msgs.msg.MoveArmGoal() 00579 else: 00580 self.header = std_msgs.msg._Header.Header() 00581 self.goal_id = actionlib_msgs.msg.GoalID() 00582 self.goal = arm_navigation_msgs.msg.MoveArmGoal() 00583 00584 def _get_types(self): 00585 """ 00586 internal API method 00587 """ 00588 return self._slot_types 00589 00590 def serialize(self, buff): 00591 """ 00592 serialize message into buffer 00593 @param buff: buffer 00594 @type buff: StringIO 00595 """ 00596 try: 00597 _x = self 00598 buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs)) 00599 _x = self.header.frame_id 00600 length = len(_x) 00601 buff.write(struct.pack('<I%ss'%length, length, _x)) 00602 _x = self 00603 buff.write(_struct_2I.pack(_x.goal_id.stamp.secs, _x.goal_id.stamp.nsecs)) 00604 _x = self.goal_id.id 00605 length = len(_x) 00606 buff.write(struct.pack('<I%ss'%length, length, _x)) 00607 _x = self.goal.planner_service_name 00608 length = len(_x) 00609 buff.write(struct.pack('<I%ss'%length, length, _x)) 00610 _x = self 00611 buff.write(_struct_3I.pack(_x.goal.planning_scene_diff.robot_state.joint_state.header.seq, _x.goal.planning_scene_diff.robot_state.joint_state.header.stamp.secs, _x.goal.planning_scene_diff.robot_state.joint_state.header.stamp.nsecs)) 00612 _x = self.goal.planning_scene_diff.robot_state.joint_state.header.frame_id 00613 length = len(_x) 00614 buff.write(struct.pack('<I%ss'%length, length, _x)) 00615 length = len(self.goal.planning_scene_diff.robot_state.joint_state.name) 00616 buff.write(_struct_I.pack(length)) 00617 for val1 in self.goal.planning_scene_diff.robot_state.joint_state.name: 00618 length = len(val1) 00619 buff.write(struct.pack('<I%ss'%length, length, val1)) 00620 length = len(self.goal.planning_scene_diff.robot_state.joint_state.position) 00621 buff.write(_struct_I.pack(length)) 00622 pattern = '<%sd'%length 00623 buff.write(struct.pack(pattern, *self.goal.planning_scene_diff.robot_state.joint_state.position)) 00624 length = len(self.goal.planning_scene_diff.robot_state.joint_state.velocity) 00625 buff.write(_struct_I.pack(length)) 00626 pattern = '<%sd'%length 00627 buff.write(struct.pack(pattern, *self.goal.planning_scene_diff.robot_state.joint_state.velocity)) 00628 length = len(self.goal.planning_scene_diff.robot_state.joint_state.effort) 00629 buff.write(_struct_I.pack(length)) 00630 pattern = '<%sd'%length 00631 buff.write(struct.pack(pattern, *self.goal.planning_scene_diff.robot_state.joint_state.effort)) 00632 _x = self 00633 buff.write(_struct_2I.pack(_x.goal.planning_scene_diff.robot_state.multi_dof_joint_state.stamp.secs, _x.goal.planning_scene_diff.robot_state.multi_dof_joint_state.stamp.nsecs)) 00634 length = len(self.goal.planning_scene_diff.robot_state.multi_dof_joint_state.joint_names) 00635 buff.write(_struct_I.pack(length)) 00636 for val1 in self.goal.planning_scene_diff.robot_state.multi_dof_joint_state.joint_names: 00637 length = len(val1) 00638 buff.write(struct.pack('<I%ss'%length, length, val1)) 00639 length = len(self.goal.planning_scene_diff.robot_state.multi_dof_joint_state.frame_ids) 00640 buff.write(_struct_I.pack(length)) 00641 for val1 in self.goal.planning_scene_diff.robot_state.multi_dof_joint_state.frame_ids: 00642 length = len(val1) 00643 buff.write(struct.pack('<I%ss'%length, length, val1)) 00644 length = len(self.goal.planning_scene_diff.robot_state.multi_dof_joint_state.child_frame_ids) 00645 buff.write(_struct_I.pack(length)) 00646 for val1 in self.goal.planning_scene_diff.robot_state.multi_dof_joint_state.child_frame_ids: 00647 length = len(val1) 00648 buff.write(struct.pack('<I%ss'%length, length, val1)) 00649 length = len(self.goal.planning_scene_diff.robot_state.multi_dof_joint_state.poses) 00650 buff.write(_struct_I.pack(length)) 00651 for val1 in self.goal.planning_scene_diff.robot_state.multi_dof_joint_state.poses: 00652 _v1 = val1.position 00653 _x = _v1 00654 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00655 _v2 = val1.orientation 00656 _x = _v2 00657 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 00658 length = len(self.goal.planning_scene_diff.fixed_frame_transforms) 00659 buff.write(_struct_I.pack(length)) 00660 for val1 in self.goal.planning_scene_diff.fixed_frame_transforms: 00661 _v3 = val1.header 00662 buff.write(_struct_I.pack(_v3.seq)) 00663 _v4 = _v3.stamp 00664 _x = _v4 00665 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00666 _x = _v3.frame_id 00667 length = len(_x) 00668 buff.write(struct.pack('<I%ss'%length, length, _x)) 00669 _x = val1.child_frame_id 00670 length = len(_x) 00671 buff.write(struct.pack('<I%ss'%length, length, _x)) 00672 _v5 = val1.transform 00673 _v6 = _v5.translation 00674 _x = _v6 00675 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00676 _v7 = _v5.rotation 00677 _x = _v7 00678 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 00679 length = len(self.goal.planning_scene_diff.allowed_collision_matrix.link_names) 00680 buff.write(_struct_I.pack(length)) 00681 for val1 in self.goal.planning_scene_diff.allowed_collision_matrix.link_names: 00682 length = len(val1) 00683 buff.write(struct.pack('<I%ss'%length, length, val1)) 00684 length = len(self.goal.planning_scene_diff.allowed_collision_matrix.entries) 00685 buff.write(_struct_I.pack(length)) 00686 for val1 in self.goal.planning_scene_diff.allowed_collision_matrix.entries: 00687 length = len(val1.enabled) 00688 buff.write(_struct_I.pack(length)) 00689 pattern = '<%sB'%length 00690 buff.write(struct.pack(pattern, *val1.enabled)) 00691 length = len(self.goal.planning_scene_diff.allowed_contacts) 00692 buff.write(_struct_I.pack(length)) 00693 for val1 in self.goal.planning_scene_diff.allowed_contacts: 00694 _x = val1.name 00695 length = len(_x) 00696 buff.write(struct.pack('<I%ss'%length, length, _x)) 00697 _v8 = val1.shape 00698 buff.write(_struct_b.pack(_v8.type)) 00699 length = len(_v8.dimensions) 00700 buff.write(_struct_I.pack(length)) 00701 pattern = '<%sd'%length 00702 buff.write(struct.pack(pattern, *_v8.dimensions)) 00703 length = len(_v8.triangles) 00704 buff.write(_struct_I.pack(length)) 00705 pattern = '<%si'%length 00706 buff.write(struct.pack(pattern, *_v8.triangles)) 00707 length = len(_v8.vertices) 00708 buff.write(_struct_I.pack(length)) 00709 for val3 in _v8.vertices: 00710 _x = val3 00711 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00712 _v9 = val1.pose_stamped 00713 _v10 = _v9.header 00714 buff.write(_struct_I.pack(_v10.seq)) 00715 _v11 = _v10.stamp 00716 _x = _v11 00717 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00718 _x = _v10.frame_id 00719 length = len(_x) 00720 buff.write(struct.pack('<I%ss'%length, length, _x)) 00721 _v12 = _v9.pose 00722 _v13 = _v12.position 00723 _x = _v13 00724 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00725 _v14 = _v12.orientation 00726 _x = _v14 00727 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 00728 length = len(val1.link_names) 00729 buff.write(_struct_I.pack(length)) 00730 for val2 in val1.link_names: 00731 length = len(val2) 00732 buff.write(struct.pack('<I%ss'%length, length, val2)) 00733 buff.write(_struct_d.pack(val1.penetration_depth)) 00734 length = len(self.goal.planning_scene_diff.link_padding) 00735 buff.write(_struct_I.pack(length)) 00736 for val1 in self.goal.planning_scene_diff.link_padding: 00737 _x = val1.link_name 00738 length = len(_x) 00739 buff.write(struct.pack('<I%ss'%length, length, _x)) 00740 buff.write(_struct_d.pack(val1.padding)) 00741 length = len(self.goal.planning_scene_diff.collision_objects) 00742 buff.write(_struct_I.pack(length)) 00743 for val1 in self.goal.planning_scene_diff.collision_objects: 00744 _v15 = val1.header 00745 buff.write(_struct_I.pack(_v15.seq)) 00746 _v16 = _v15.stamp 00747 _x = _v16 00748 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00749 _x = _v15.frame_id 00750 length = len(_x) 00751 buff.write(struct.pack('<I%ss'%length, length, _x)) 00752 _x = val1.id 00753 length = len(_x) 00754 buff.write(struct.pack('<I%ss'%length, length, _x)) 00755 buff.write(_struct_f.pack(val1.padding)) 00756 _v17 = val1.operation 00757 buff.write(_struct_b.pack(_v17.operation)) 00758 length = len(val1.shapes) 00759 buff.write(_struct_I.pack(length)) 00760 for val2 in val1.shapes: 00761 buff.write(_struct_b.pack(val2.type)) 00762 length = len(val2.dimensions) 00763 buff.write(_struct_I.pack(length)) 00764 pattern = '<%sd'%length 00765 buff.write(struct.pack(pattern, *val2.dimensions)) 00766 length = len(val2.triangles) 00767 buff.write(_struct_I.pack(length)) 00768 pattern = '<%si'%length 00769 buff.write(struct.pack(pattern, *val2.triangles)) 00770 length = len(val2.vertices) 00771 buff.write(_struct_I.pack(length)) 00772 for val3 in val2.vertices: 00773 _x = val3 00774 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00775 length = len(val1.poses) 00776 buff.write(_struct_I.pack(length)) 00777 for val2 in val1.poses: 00778 _v18 = val2.position 00779 _x = _v18 00780 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00781 _v19 = val2.orientation 00782 _x = _v19 00783 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 00784 length = len(self.goal.planning_scene_diff.attached_collision_objects) 00785 buff.write(_struct_I.pack(length)) 00786 for val1 in self.goal.planning_scene_diff.attached_collision_objects: 00787 _x = val1.link_name 00788 length = len(_x) 00789 buff.write(struct.pack('<I%ss'%length, length, _x)) 00790 _v20 = val1.object 00791 _v21 = _v20.header 00792 buff.write(_struct_I.pack(_v21.seq)) 00793 _v22 = _v21.stamp 00794 _x = _v22 00795 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00796 _x = _v21.frame_id 00797 length = len(_x) 00798 buff.write(struct.pack('<I%ss'%length, length, _x)) 00799 _x = _v20.id 00800 length = len(_x) 00801 buff.write(struct.pack('<I%ss'%length, length, _x)) 00802 buff.write(_struct_f.pack(_v20.padding)) 00803 _v23 = _v20.operation 00804 buff.write(_struct_b.pack(_v23.operation)) 00805 length = len(_v20.shapes) 00806 buff.write(_struct_I.pack(length)) 00807 for val3 in _v20.shapes: 00808 buff.write(_struct_b.pack(val3.type)) 00809 length = len(val3.dimensions) 00810 buff.write(_struct_I.pack(length)) 00811 pattern = '<%sd'%length 00812 buff.write(struct.pack(pattern, *val3.dimensions)) 00813 length = len(val3.triangles) 00814 buff.write(_struct_I.pack(length)) 00815 pattern = '<%si'%length 00816 buff.write(struct.pack(pattern, *val3.triangles)) 00817 length = len(val3.vertices) 00818 buff.write(_struct_I.pack(length)) 00819 for val4 in val3.vertices: 00820 _x = val4 00821 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00822 length = len(_v20.poses) 00823 buff.write(_struct_I.pack(length)) 00824 for val3 in _v20.poses: 00825 _v24 = val3.position 00826 _x = _v24 00827 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00828 _v25 = val3.orientation 00829 _x = _v25 00830 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 00831 length = len(val1.touch_links) 00832 buff.write(_struct_I.pack(length)) 00833 for val2 in val1.touch_links: 00834 length = len(val2) 00835 buff.write(struct.pack('<I%ss'%length, length, val2)) 00836 _x = self 00837 buff.write(_struct_3I.pack(_x.goal.planning_scene_diff.collision_map.header.seq, _x.goal.planning_scene_diff.collision_map.header.stamp.secs, _x.goal.planning_scene_diff.collision_map.header.stamp.nsecs)) 00838 _x = self.goal.planning_scene_diff.collision_map.header.frame_id 00839 length = len(_x) 00840 buff.write(struct.pack('<I%ss'%length, length, _x)) 00841 length = len(self.goal.planning_scene_diff.collision_map.boxes) 00842 buff.write(_struct_I.pack(length)) 00843 for val1 in self.goal.planning_scene_diff.collision_map.boxes: 00844 _v26 = val1.center 00845 _x = _v26 00846 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z)) 00847 _v27 = val1.extents 00848 _x = _v27 00849 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z)) 00850 _v28 = val1.axis 00851 _x = _v28 00852 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z)) 00853 buff.write(_struct_f.pack(val1.angle)) 00854 buff.write(_struct_b.pack(self.goal.motion_plan_request.workspace_parameters.workspace_region_shape.type)) 00855 length = len(self.goal.motion_plan_request.workspace_parameters.workspace_region_shape.dimensions) 00856 buff.write(_struct_I.pack(length)) 00857 pattern = '<%sd'%length 00858 buff.write(struct.pack(pattern, *self.goal.motion_plan_request.workspace_parameters.workspace_region_shape.dimensions)) 00859 length = len(self.goal.motion_plan_request.workspace_parameters.workspace_region_shape.triangles) 00860 buff.write(_struct_I.pack(length)) 00861 pattern = '<%si'%length 00862 buff.write(struct.pack(pattern, *self.goal.motion_plan_request.workspace_parameters.workspace_region_shape.triangles)) 00863 length = len(self.goal.motion_plan_request.workspace_parameters.workspace_region_shape.vertices) 00864 buff.write(_struct_I.pack(length)) 00865 for val1 in self.goal.motion_plan_request.workspace_parameters.workspace_region_shape.vertices: 00866 _x = val1 00867 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00868 _x = self 00869 buff.write(_struct_3I.pack(_x.goal.motion_plan_request.workspace_parameters.workspace_region_pose.header.seq, _x.goal.motion_plan_request.workspace_parameters.workspace_region_pose.header.stamp.secs, _x.goal.motion_plan_request.workspace_parameters.workspace_region_pose.header.stamp.nsecs)) 00870 _x = self.goal.motion_plan_request.workspace_parameters.workspace_region_pose.header.frame_id 00871 length = len(_x) 00872 buff.write(struct.pack('<I%ss'%length, length, _x)) 00873 _x = self 00874 buff.write(_struct_7d3I.pack(_x.goal.motion_plan_request.workspace_parameters.workspace_region_pose.pose.position.x, _x.goal.motion_plan_request.workspace_parameters.workspace_region_pose.pose.position.y, _x.goal.motion_plan_request.workspace_parameters.workspace_region_pose.pose.position.z, _x.goal.motion_plan_request.workspace_parameters.workspace_region_pose.pose.orientation.x, _x.goal.motion_plan_request.workspace_parameters.workspace_region_pose.pose.orientation.y, _x.goal.motion_plan_request.workspace_parameters.workspace_region_pose.pose.orientation.z, _x.goal.motion_plan_request.workspace_parameters.workspace_region_pose.pose.orientation.w, _x.goal.motion_plan_request.start_state.joint_state.header.seq, _x.goal.motion_plan_request.start_state.joint_state.header.stamp.secs, _x.goal.motion_plan_request.start_state.joint_state.header.stamp.nsecs)) 00875 _x = self.goal.motion_plan_request.start_state.joint_state.header.frame_id 00876 length = len(_x) 00877 buff.write(struct.pack('<I%ss'%length, length, _x)) 00878 length = len(self.goal.motion_plan_request.start_state.joint_state.name) 00879 buff.write(_struct_I.pack(length)) 00880 for val1 in self.goal.motion_plan_request.start_state.joint_state.name: 00881 length = len(val1) 00882 buff.write(struct.pack('<I%ss'%length, length, val1)) 00883 length = len(self.goal.motion_plan_request.start_state.joint_state.position) 00884 buff.write(_struct_I.pack(length)) 00885 pattern = '<%sd'%length 00886 buff.write(struct.pack(pattern, *self.goal.motion_plan_request.start_state.joint_state.position)) 00887 length = len(self.goal.motion_plan_request.start_state.joint_state.velocity) 00888 buff.write(_struct_I.pack(length)) 00889 pattern = '<%sd'%length 00890 buff.write(struct.pack(pattern, *self.goal.motion_plan_request.start_state.joint_state.velocity)) 00891 length = len(self.goal.motion_plan_request.start_state.joint_state.effort) 00892 buff.write(_struct_I.pack(length)) 00893 pattern = '<%sd'%length 00894 buff.write(struct.pack(pattern, *self.goal.motion_plan_request.start_state.joint_state.effort)) 00895 _x = self 00896 buff.write(_struct_2I.pack(_x.goal.motion_plan_request.start_state.multi_dof_joint_state.stamp.secs, _x.goal.motion_plan_request.start_state.multi_dof_joint_state.stamp.nsecs)) 00897 length = len(self.goal.motion_plan_request.start_state.multi_dof_joint_state.joint_names) 00898 buff.write(_struct_I.pack(length)) 00899 for val1 in self.goal.motion_plan_request.start_state.multi_dof_joint_state.joint_names: 00900 length = len(val1) 00901 buff.write(struct.pack('<I%ss'%length, length, val1)) 00902 length = len(self.goal.motion_plan_request.start_state.multi_dof_joint_state.frame_ids) 00903 buff.write(_struct_I.pack(length)) 00904 for val1 in self.goal.motion_plan_request.start_state.multi_dof_joint_state.frame_ids: 00905 length = len(val1) 00906 buff.write(struct.pack('<I%ss'%length, length, val1)) 00907 length = len(self.goal.motion_plan_request.start_state.multi_dof_joint_state.child_frame_ids) 00908 buff.write(_struct_I.pack(length)) 00909 for val1 in self.goal.motion_plan_request.start_state.multi_dof_joint_state.child_frame_ids: 00910 length = len(val1) 00911 buff.write(struct.pack('<I%ss'%length, length, val1)) 00912 length = len(self.goal.motion_plan_request.start_state.multi_dof_joint_state.poses) 00913 buff.write(_struct_I.pack(length)) 00914 for val1 in self.goal.motion_plan_request.start_state.multi_dof_joint_state.poses: 00915 _v29 = val1.position 00916 _x = _v29 00917 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00918 _v30 = val1.orientation 00919 _x = _v30 00920 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 00921 length = len(self.goal.motion_plan_request.goal_constraints.joint_constraints) 00922 buff.write(_struct_I.pack(length)) 00923 for val1 in self.goal.motion_plan_request.goal_constraints.joint_constraints: 00924 _x = val1.joint_name 00925 length = len(_x) 00926 buff.write(struct.pack('<I%ss'%length, length, _x)) 00927 _x = val1 00928 buff.write(_struct_4d.pack(_x.position, _x.tolerance_above, _x.tolerance_below, _x.weight)) 00929 length = len(self.goal.motion_plan_request.goal_constraints.position_constraints) 00930 buff.write(_struct_I.pack(length)) 00931 for val1 in self.goal.motion_plan_request.goal_constraints.position_constraints: 00932 _v31 = val1.header 00933 buff.write(_struct_I.pack(_v31.seq)) 00934 _v32 = _v31.stamp 00935 _x = _v32 00936 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00937 _x = _v31.frame_id 00938 length = len(_x) 00939 buff.write(struct.pack('<I%ss'%length, length, _x)) 00940 _x = val1.link_name 00941 length = len(_x) 00942 buff.write(struct.pack('<I%ss'%length, length, _x)) 00943 _v33 = val1.target_point_offset 00944 _x = _v33 00945 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00946 _v34 = val1.position 00947 _x = _v34 00948 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00949 _v35 = val1.constraint_region_shape 00950 buff.write(_struct_b.pack(_v35.type)) 00951 length = len(_v35.dimensions) 00952 buff.write(_struct_I.pack(length)) 00953 pattern = '<%sd'%length 00954 buff.write(struct.pack(pattern, *_v35.dimensions)) 00955 length = len(_v35.triangles) 00956 buff.write(_struct_I.pack(length)) 00957 pattern = '<%si'%length 00958 buff.write(struct.pack(pattern, *_v35.triangles)) 00959 length = len(_v35.vertices) 00960 buff.write(_struct_I.pack(length)) 00961 for val3 in _v35.vertices: 00962 _x = val3 00963 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00964 _v36 = val1.constraint_region_orientation 00965 _x = _v36 00966 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 00967 buff.write(_struct_d.pack(val1.weight)) 00968 length = len(self.goal.motion_plan_request.goal_constraints.orientation_constraints) 00969 buff.write(_struct_I.pack(length)) 00970 for val1 in self.goal.motion_plan_request.goal_constraints.orientation_constraints: 00971 _v37 = val1.header 00972 buff.write(_struct_I.pack(_v37.seq)) 00973 _v38 = _v37.stamp 00974 _x = _v38 00975 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00976 _x = _v37.frame_id 00977 length = len(_x) 00978 buff.write(struct.pack('<I%ss'%length, length, _x)) 00979 _x = val1.link_name 00980 length = len(_x) 00981 buff.write(struct.pack('<I%ss'%length, length, _x)) 00982 buff.write(_struct_i.pack(val1.type)) 00983 _v39 = val1.orientation 00984 _x = _v39 00985 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 00986 _x = val1 00987 buff.write(_struct_4d.pack(_x.absolute_roll_tolerance, _x.absolute_pitch_tolerance, _x.absolute_yaw_tolerance, _x.weight)) 00988 length = len(self.goal.motion_plan_request.goal_constraints.visibility_constraints) 00989 buff.write(_struct_I.pack(length)) 00990 for val1 in self.goal.motion_plan_request.goal_constraints.visibility_constraints: 00991 _v40 = val1.header 00992 buff.write(_struct_I.pack(_v40.seq)) 00993 _v41 = _v40.stamp 00994 _x = _v41 00995 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00996 _x = _v40.frame_id 00997 length = len(_x) 00998 buff.write(struct.pack('<I%ss'%length, length, _x)) 00999 _v42 = val1.target 01000 _v43 = _v42.header 01001 buff.write(_struct_I.pack(_v43.seq)) 01002 _v44 = _v43.stamp 01003 _x = _v44 01004 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01005 _x = _v43.frame_id 01006 length = len(_x) 01007 buff.write(struct.pack('<I%ss'%length, length, _x)) 01008 _v45 = _v42.point 01009 _x = _v45 01010 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 01011 _v46 = val1.sensor_pose 01012 _v47 = _v46.header 01013 buff.write(_struct_I.pack(_v47.seq)) 01014 _v48 = _v47.stamp 01015 _x = _v48 01016 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01017 _x = _v47.frame_id 01018 length = len(_x) 01019 buff.write(struct.pack('<I%ss'%length, length, _x)) 01020 _v49 = _v46.pose 01021 _v50 = _v49.position 01022 _x = _v50 01023 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 01024 _v51 = _v49.orientation 01025 _x = _v51 01026 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 01027 buff.write(_struct_d.pack(val1.absolute_tolerance)) 01028 length = len(self.goal.motion_plan_request.path_constraints.joint_constraints) 01029 buff.write(_struct_I.pack(length)) 01030 for val1 in self.goal.motion_plan_request.path_constraints.joint_constraints: 01031 _x = val1.joint_name 01032 length = len(_x) 01033 buff.write(struct.pack('<I%ss'%length, length, _x)) 01034 _x = val1 01035 buff.write(_struct_4d.pack(_x.position, _x.tolerance_above, _x.tolerance_below, _x.weight)) 01036 length = len(self.goal.motion_plan_request.path_constraints.position_constraints) 01037 buff.write(_struct_I.pack(length)) 01038 for val1 in self.goal.motion_plan_request.path_constraints.position_constraints: 01039 _v52 = val1.header 01040 buff.write(_struct_I.pack(_v52.seq)) 01041 _v53 = _v52.stamp 01042 _x = _v53 01043 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01044 _x = _v52.frame_id 01045 length = len(_x) 01046 buff.write(struct.pack('<I%ss'%length, length, _x)) 01047 _x = val1.link_name 01048 length = len(_x) 01049 buff.write(struct.pack('<I%ss'%length, length, _x)) 01050 _v54 = val1.target_point_offset 01051 _x = _v54 01052 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 01053 _v55 = val1.position 01054 _x = _v55 01055 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 01056 _v56 = val1.constraint_region_shape 01057 buff.write(_struct_b.pack(_v56.type)) 01058 length = len(_v56.dimensions) 01059 buff.write(_struct_I.pack(length)) 01060 pattern = '<%sd'%length 01061 buff.write(struct.pack(pattern, *_v56.dimensions)) 01062 length = len(_v56.triangles) 01063 buff.write(_struct_I.pack(length)) 01064 pattern = '<%si'%length 01065 buff.write(struct.pack(pattern, *_v56.triangles)) 01066 length = len(_v56.vertices) 01067 buff.write(_struct_I.pack(length)) 01068 for val3 in _v56.vertices: 01069 _x = val3 01070 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 01071 _v57 = val1.constraint_region_orientation 01072 _x = _v57 01073 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 01074 buff.write(_struct_d.pack(val1.weight)) 01075 length = len(self.goal.motion_plan_request.path_constraints.orientation_constraints) 01076 buff.write(_struct_I.pack(length)) 01077 for val1 in self.goal.motion_plan_request.path_constraints.orientation_constraints: 01078 _v58 = val1.header 01079 buff.write(_struct_I.pack(_v58.seq)) 01080 _v59 = _v58.stamp 01081 _x = _v59 01082 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01083 _x = _v58.frame_id 01084 length = len(_x) 01085 buff.write(struct.pack('<I%ss'%length, length, _x)) 01086 _x = val1.link_name 01087 length = len(_x) 01088 buff.write(struct.pack('<I%ss'%length, length, _x)) 01089 buff.write(_struct_i.pack(val1.type)) 01090 _v60 = val1.orientation 01091 _x = _v60 01092 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 01093 _x = val1 01094 buff.write(_struct_4d.pack(_x.absolute_roll_tolerance, _x.absolute_pitch_tolerance, _x.absolute_yaw_tolerance, _x.weight)) 01095 length = len(self.goal.motion_plan_request.path_constraints.visibility_constraints) 01096 buff.write(_struct_I.pack(length)) 01097 for val1 in self.goal.motion_plan_request.path_constraints.visibility_constraints: 01098 _v61 = val1.header 01099 buff.write(_struct_I.pack(_v61.seq)) 01100 _v62 = _v61.stamp 01101 _x = _v62 01102 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01103 _x = _v61.frame_id 01104 length = len(_x) 01105 buff.write(struct.pack('<I%ss'%length, length, _x)) 01106 _v63 = val1.target 01107 _v64 = _v63.header 01108 buff.write(_struct_I.pack(_v64.seq)) 01109 _v65 = _v64.stamp 01110 _x = _v65 01111 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01112 _x = _v64.frame_id 01113 length = len(_x) 01114 buff.write(struct.pack('<I%ss'%length, length, _x)) 01115 _v66 = _v63.point 01116 _x = _v66 01117 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 01118 _v67 = val1.sensor_pose 01119 _v68 = _v67.header 01120 buff.write(_struct_I.pack(_v68.seq)) 01121 _v69 = _v68.stamp 01122 _x = _v69 01123 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01124 _x = _v68.frame_id 01125 length = len(_x) 01126 buff.write(struct.pack('<I%ss'%length, length, _x)) 01127 _v70 = _v67.pose 01128 _v71 = _v70.position 01129 _x = _v71 01130 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 01131 _v72 = _v70.orientation 01132 _x = _v72 01133 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 01134 buff.write(_struct_d.pack(val1.absolute_tolerance)) 01135 _x = self.goal.motion_plan_request.planner_id 01136 length = len(_x) 01137 buff.write(struct.pack('<I%ss'%length, length, _x)) 01138 _x = self.goal.motion_plan_request.group_name 01139 length = len(_x) 01140 buff.write(struct.pack('<I%ss'%length, length, _x)) 01141 _x = self 01142 buff.write(_struct_7i.pack(_x.goal.motion_plan_request.num_planning_attempts, _x.goal.motion_plan_request.allowed_planning_time.secs, _x.goal.motion_plan_request.allowed_planning_time.nsecs, _x.goal.motion_plan_request.expected_path_duration.secs, _x.goal.motion_plan_request.expected_path_duration.nsecs, _x.goal.motion_plan_request.expected_path_dt.secs, _x.goal.motion_plan_request.expected_path_dt.nsecs)) 01143 length = len(self.goal.operations.collision_operations) 01144 buff.write(_struct_I.pack(length)) 01145 for val1 in self.goal.operations.collision_operations: 01146 _x = val1.object1 01147 length = len(_x) 01148 buff.write(struct.pack('<I%ss'%length, length, _x)) 01149 _x = val1.object2 01150 length = len(_x) 01151 buff.write(struct.pack('<I%ss'%length, length, _x)) 01152 _x = val1 01153 buff.write(_struct_di.pack(_x.penetration_distance, _x.operation)) 01154 _x = self 01155 buff.write(_struct_4B.pack(_x.goal.accept_partial_plans, _x.goal.accept_invalid_goals, _x.goal.disable_ik, _x.goal.disable_collision_monitoring)) 01156 except struct.error as se: self._check_types(se) 01157 except TypeError as te: self._check_types(te) 01158 01159 def deserialize(self, str): 01160 """ 01161 unpack serialized message in str into this message instance 01162 @param str: byte array of serialized message 01163 @type str: str 01164 """ 01165 try: 01166 if self.header is None: 01167 self.header = std_msgs.msg._Header.Header() 01168 if self.goal_id is None: 01169 self.goal_id = actionlib_msgs.msg.GoalID() 01170 if self.goal is None: 01171 self.goal = arm_navigation_msgs.msg.MoveArmGoal() 01172 end = 0 01173 _x = self 01174 start = end 01175 end += 12 01176 (_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 01177 start = end 01178 end += 4 01179 (length,) = _struct_I.unpack(str[start:end]) 01180 start = end 01181 end += length 01182 self.header.frame_id = str[start:end] 01183 _x = self 01184 start = end 01185 end += 8 01186 (_x.goal_id.stamp.secs, _x.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end]) 01187 start = end 01188 end += 4 01189 (length,) = _struct_I.unpack(str[start:end]) 01190 start = end 01191 end += length 01192 self.goal_id.id = str[start:end] 01193 start = end 01194 end += 4 01195 (length,) = _struct_I.unpack(str[start:end]) 01196 start = end 01197 end += length 01198 self.goal.planner_service_name = str[start:end] 01199 _x = self 01200 start = end 01201 end += 12 01202 (_x.goal.planning_scene_diff.robot_state.joint_state.header.seq, _x.goal.planning_scene_diff.robot_state.joint_state.header.stamp.secs, _x.goal.planning_scene_diff.robot_state.joint_state.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 01203 start = end 01204 end += 4 01205 (length,) = _struct_I.unpack(str[start:end]) 01206 start = end 01207 end += length 01208 self.goal.planning_scene_diff.robot_state.joint_state.header.frame_id = str[start:end] 01209 start = end 01210 end += 4 01211 (length,) = _struct_I.unpack(str[start:end]) 01212 self.goal.planning_scene_diff.robot_state.joint_state.name = [] 01213 for i in range(0, length): 01214 start = end 01215 end += 4 01216 (length,) = _struct_I.unpack(str[start:end]) 01217 start = end 01218 end += length 01219 val1 = str[start:end] 01220 self.goal.planning_scene_diff.robot_state.joint_state.name.append(val1) 01221 start = end 01222 end += 4 01223 (length,) = _struct_I.unpack(str[start:end]) 01224 pattern = '<%sd'%length 01225 start = end 01226 end += struct.calcsize(pattern) 01227 self.goal.planning_scene_diff.robot_state.joint_state.position = struct.unpack(pattern, str[start:end]) 01228 start = end 01229 end += 4 01230 (length,) = _struct_I.unpack(str[start:end]) 01231 pattern = '<%sd'%length 01232 start = end 01233 end += struct.calcsize(pattern) 01234 self.goal.planning_scene_diff.robot_state.joint_state.velocity = struct.unpack(pattern, str[start:end]) 01235 start = end 01236 end += 4 01237 (length,) = _struct_I.unpack(str[start:end]) 01238 pattern = '<%sd'%length 01239 start = end 01240 end += struct.calcsize(pattern) 01241 self.goal.planning_scene_diff.robot_state.joint_state.effort = struct.unpack(pattern, str[start:end]) 01242 _x = self 01243 start = end 01244 end += 8 01245 (_x.goal.planning_scene_diff.robot_state.multi_dof_joint_state.stamp.secs, _x.goal.planning_scene_diff.robot_state.multi_dof_joint_state.stamp.nsecs,) = _struct_2I.unpack(str[start:end]) 01246 start = end 01247 end += 4 01248 (length,) = _struct_I.unpack(str[start:end]) 01249 self.goal.planning_scene_diff.robot_state.multi_dof_joint_state.joint_names = [] 01250 for i in range(0, length): 01251 start = end 01252 end += 4 01253 (length,) = _struct_I.unpack(str[start:end]) 01254 start = end 01255 end += length 01256 val1 = str[start:end] 01257 self.goal.planning_scene_diff.robot_state.multi_dof_joint_state.joint_names.append(val1) 01258 start = end 01259 end += 4 01260 (length,) = _struct_I.unpack(str[start:end]) 01261 self.goal.planning_scene_diff.robot_state.multi_dof_joint_state.frame_ids = [] 01262 for i in range(0, length): 01263 start = end 01264 end += 4 01265 (length,) = _struct_I.unpack(str[start:end]) 01266 start = end 01267 end += length 01268 val1 = str[start:end] 01269 self.goal.planning_scene_diff.robot_state.multi_dof_joint_state.frame_ids.append(val1) 01270 start = end 01271 end += 4 01272 (length,) = _struct_I.unpack(str[start:end]) 01273 self.goal.planning_scene_diff.robot_state.multi_dof_joint_state.child_frame_ids = [] 01274 for i in range(0, length): 01275 start = end 01276 end += 4 01277 (length,) = _struct_I.unpack(str[start:end]) 01278 start = end 01279 end += length 01280 val1 = str[start:end] 01281 self.goal.planning_scene_diff.robot_state.multi_dof_joint_state.child_frame_ids.append(val1) 01282 start = end 01283 end += 4 01284 (length,) = _struct_I.unpack(str[start:end]) 01285 self.goal.planning_scene_diff.robot_state.multi_dof_joint_state.poses = [] 01286 for i in range(0, length): 01287 val1 = geometry_msgs.msg.Pose() 01288 _v73 = val1.position 01289 _x = _v73 01290 start = end 01291 end += 24 01292 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 01293 _v74 = val1.orientation 01294 _x = _v74 01295 start = end 01296 end += 32 01297 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 01298 self.goal.planning_scene_diff.robot_state.multi_dof_joint_state.poses.append(val1) 01299 start = end 01300 end += 4 01301 (length,) = _struct_I.unpack(str[start:end]) 01302 self.goal.planning_scene_diff.fixed_frame_transforms = [] 01303 for i in range(0, length): 01304 val1 = geometry_msgs.msg.TransformStamped() 01305 _v75 = val1.header 01306 start = end 01307 end += 4 01308 (_v75.seq,) = _struct_I.unpack(str[start:end]) 01309 _v76 = _v75.stamp 01310 _x = _v76 01311 start = end 01312 end += 8 01313 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01314 start = end 01315 end += 4 01316 (length,) = _struct_I.unpack(str[start:end]) 01317 start = end 01318 end += length 01319 _v75.frame_id = str[start:end] 01320 start = end 01321 end += 4 01322 (length,) = _struct_I.unpack(str[start:end]) 01323 start = end 01324 end += length 01325 val1.child_frame_id = str[start:end] 01326 _v77 = val1.transform 01327 _v78 = _v77.translation 01328 _x = _v78 01329 start = end 01330 end += 24 01331 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 01332 _v79 = _v77.rotation 01333 _x = _v79 01334 start = end 01335 end += 32 01336 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 01337 self.goal.planning_scene_diff.fixed_frame_transforms.append(val1) 01338 start = end 01339 end += 4 01340 (length,) = _struct_I.unpack(str[start:end]) 01341 self.goal.planning_scene_diff.allowed_collision_matrix.link_names = [] 01342 for i in range(0, length): 01343 start = end 01344 end += 4 01345 (length,) = _struct_I.unpack(str[start:end]) 01346 start = end 01347 end += length 01348 val1 = str[start:end] 01349 self.goal.planning_scene_diff.allowed_collision_matrix.link_names.append(val1) 01350 start = end 01351 end += 4 01352 (length,) = _struct_I.unpack(str[start:end]) 01353 self.goal.planning_scene_diff.allowed_collision_matrix.entries = [] 01354 for i in range(0, length): 01355 val1 = arm_navigation_msgs.msg.AllowedCollisionEntry() 01356 start = end 01357 end += 4 01358 (length,) = _struct_I.unpack(str[start:end]) 01359 pattern = '<%sB'%length 01360 start = end 01361 end += struct.calcsize(pattern) 01362 val1.enabled = struct.unpack(pattern, str[start:end]) 01363 val1.enabled = map(bool, val1.enabled) 01364 self.goal.planning_scene_diff.allowed_collision_matrix.entries.append(val1) 01365 start = end 01366 end += 4 01367 (length,) = _struct_I.unpack(str[start:end]) 01368 self.goal.planning_scene_diff.allowed_contacts = [] 01369 for i in range(0, length): 01370 val1 = arm_navigation_msgs.msg.AllowedContactSpecification() 01371 start = end 01372 end += 4 01373 (length,) = _struct_I.unpack(str[start:end]) 01374 start = end 01375 end += length 01376 val1.name = str[start:end] 01377 _v80 = val1.shape 01378 start = end 01379 end += 1 01380 (_v80.type,) = _struct_b.unpack(str[start:end]) 01381 start = end 01382 end += 4 01383 (length,) = _struct_I.unpack(str[start:end]) 01384 pattern = '<%sd'%length 01385 start = end 01386 end += struct.calcsize(pattern) 01387 _v80.dimensions = struct.unpack(pattern, str[start:end]) 01388 start = end 01389 end += 4 01390 (length,) = _struct_I.unpack(str[start:end]) 01391 pattern = '<%si'%length 01392 start = end 01393 end += struct.calcsize(pattern) 01394 _v80.triangles = struct.unpack(pattern, str[start:end]) 01395 start = end 01396 end += 4 01397 (length,) = _struct_I.unpack(str[start:end]) 01398 _v80.vertices = [] 01399 for i in range(0, length): 01400 val3 = geometry_msgs.msg.Point() 01401 _x = val3 01402 start = end 01403 end += 24 01404 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 01405 _v80.vertices.append(val3) 01406 _v81 = val1.pose_stamped 01407 _v82 = _v81.header 01408 start = end 01409 end += 4 01410 (_v82.seq,) = _struct_I.unpack(str[start:end]) 01411 _v83 = _v82.stamp 01412 _x = _v83 01413 start = end 01414 end += 8 01415 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01416 start = end 01417 end += 4 01418 (length,) = _struct_I.unpack(str[start:end]) 01419 start = end 01420 end += length 01421 _v82.frame_id = str[start:end] 01422 _v84 = _v81.pose 01423 _v85 = _v84.position 01424 _x = _v85 01425 start = end 01426 end += 24 01427 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 01428 _v86 = _v84.orientation 01429 _x = _v86 01430 start = end 01431 end += 32 01432 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 01433 start = end 01434 end += 4 01435 (length,) = _struct_I.unpack(str[start:end]) 01436 val1.link_names = [] 01437 for i in range(0, length): 01438 start = end 01439 end += 4 01440 (length,) = _struct_I.unpack(str[start:end]) 01441 start = end 01442 end += length 01443 val2 = str[start:end] 01444 val1.link_names.append(val2) 01445 start = end 01446 end += 8 01447 (val1.penetration_depth,) = _struct_d.unpack(str[start:end]) 01448 self.goal.planning_scene_diff.allowed_contacts.append(val1) 01449 start = end 01450 end += 4 01451 (length,) = _struct_I.unpack(str[start:end]) 01452 self.goal.planning_scene_diff.link_padding = [] 01453 for i in range(0, length): 01454 val1 = arm_navigation_msgs.msg.LinkPadding() 01455 start = end 01456 end += 4 01457 (length,) = _struct_I.unpack(str[start:end]) 01458 start = end 01459 end += length 01460 val1.link_name = str[start:end] 01461 start = end 01462 end += 8 01463 (val1.padding,) = _struct_d.unpack(str[start:end]) 01464 self.goal.planning_scene_diff.link_padding.append(val1) 01465 start = end 01466 end += 4 01467 (length,) = _struct_I.unpack(str[start:end]) 01468 self.goal.planning_scene_diff.collision_objects = [] 01469 for i in range(0, length): 01470 val1 = arm_navigation_msgs.msg.CollisionObject() 01471 _v87 = val1.header 01472 start = end 01473 end += 4 01474 (_v87.seq,) = _struct_I.unpack(str[start:end]) 01475 _v88 = _v87.stamp 01476 _x = _v88 01477 start = end 01478 end += 8 01479 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01480 start = end 01481 end += 4 01482 (length,) = _struct_I.unpack(str[start:end]) 01483 start = end 01484 end += length 01485 _v87.frame_id = str[start:end] 01486 start = end 01487 end += 4 01488 (length,) = _struct_I.unpack(str[start:end]) 01489 start = end 01490 end += length 01491 val1.id = str[start:end] 01492 start = end 01493 end += 4 01494 (val1.padding,) = _struct_f.unpack(str[start:end]) 01495 _v89 = val1.operation 01496 start = end 01497 end += 1 01498 (_v89.operation,) = _struct_b.unpack(str[start:end]) 01499 start = end 01500 end += 4 01501 (length,) = _struct_I.unpack(str[start:end]) 01502 val1.shapes = [] 01503 for i in range(0, length): 01504 val2 = arm_navigation_msgs.msg.Shape() 01505 start = end 01506 end += 1 01507 (val2.type,) = _struct_b.unpack(str[start:end]) 01508 start = end 01509 end += 4 01510 (length,) = _struct_I.unpack(str[start:end]) 01511 pattern = '<%sd'%length 01512 start = end 01513 end += struct.calcsize(pattern) 01514 val2.dimensions = struct.unpack(pattern, str[start:end]) 01515 start = end 01516 end += 4 01517 (length,) = _struct_I.unpack(str[start:end]) 01518 pattern = '<%si'%length 01519 start = end 01520 end += struct.calcsize(pattern) 01521 val2.triangles = struct.unpack(pattern, str[start:end]) 01522 start = end 01523 end += 4 01524 (length,) = _struct_I.unpack(str[start:end]) 01525 val2.vertices = [] 01526 for i in range(0, length): 01527 val3 = geometry_msgs.msg.Point() 01528 _x = val3 01529 start = end 01530 end += 24 01531 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 01532 val2.vertices.append(val3) 01533 val1.shapes.append(val2) 01534 start = end 01535 end += 4 01536 (length,) = _struct_I.unpack(str[start:end]) 01537 val1.poses = [] 01538 for i in range(0, length): 01539 val2 = geometry_msgs.msg.Pose() 01540 _v90 = val2.position 01541 _x = _v90 01542 start = end 01543 end += 24 01544 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 01545 _v91 = val2.orientation 01546 _x = _v91 01547 start = end 01548 end += 32 01549 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 01550 val1.poses.append(val2) 01551 self.goal.planning_scene_diff.collision_objects.append(val1) 01552 start = end 01553 end += 4 01554 (length,) = _struct_I.unpack(str[start:end]) 01555 self.goal.planning_scene_diff.attached_collision_objects = [] 01556 for i in range(0, length): 01557 val1 = arm_navigation_msgs.msg.AttachedCollisionObject() 01558 start = end 01559 end += 4 01560 (length,) = _struct_I.unpack(str[start:end]) 01561 start = end 01562 end += length 01563 val1.link_name = str[start:end] 01564 _v92 = val1.object 01565 _v93 = _v92.header 01566 start = end 01567 end += 4 01568 (_v93.seq,) = _struct_I.unpack(str[start:end]) 01569 _v94 = _v93.stamp 01570 _x = _v94 01571 start = end 01572 end += 8 01573 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01574 start = end 01575 end += 4 01576 (length,) = _struct_I.unpack(str[start:end]) 01577 start = end 01578 end += length 01579 _v93.frame_id = str[start:end] 01580 start = end 01581 end += 4 01582 (length,) = _struct_I.unpack(str[start:end]) 01583 start = end 01584 end += length 01585 _v92.id = str[start:end] 01586 start = end 01587 end += 4 01588 (_v92.padding,) = _struct_f.unpack(str[start:end]) 01589 _v95 = _v92.operation 01590 start = end 01591 end += 1 01592 (_v95.operation,) = _struct_b.unpack(str[start:end]) 01593 start = end 01594 end += 4 01595 (length,) = _struct_I.unpack(str[start:end]) 01596 _v92.shapes = [] 01597 for i in range(0, length): 01598 val3 = arm_navigation_msgs.msg.Shape() 01599 start = end 01600 end += 1 01601 (val3.type,) = _struct_b.unpack(str[start:end]) 01602 start = end 01603 end += 4 01604 (length,) = _struct_I.unpack(str[start:end]) 01605 pattern = '<%sd'%length 01606 start = end 01607 end += struct.calcsize(pattern) 01608 val3.dimensions = struct.unpack(pattern, str[start:end]) 01609 start = end 01610 end += 4 01611 (length,) = _struct_I.unpack(str[start:end]) 01612 pattern = '<%si'%length 01613 start = end 01614 end += struct.calcsize(pattern) 01615 val3.triangles = struct.unpack(pattern, str[start:end]) 01616 start = end 01617 end += 4 01618 (length,) = _struct_I.unpack(str[start:end]) 01619 val3.vertices = [] 01620 for i in range(0, length): 01621 val4 = geometry_msgs.msg.Point() 01622 _x = val4 01623 start = end 01624 end += 24 01625 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 01626 val3.vertices.append(val4) 01627 _v92.shapes.append(val3) 01628 start = end 01629 end += 4 01630 (length,) = _struct_I.unpack(str[start:end]) 01631 _v92.poses = [] 01632 for i in range(0, length): 01633 val3 = geometry_msgs.msg.Pose() 01634 _v96 = val3.position 01635 _x = _v96 01636 start = end 01637 end += 24 01638 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 01639 _v97 = val3.orientation 01640 _x = _v97 01641 start = end 01642 end += 32 01643 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 01644 _v92.poses.append(val3) 01645 start = end 01646 end += 4 01647 (length,) = _struct_I.unpack(str[start:end]) 01648 val1.touch_links = [] 01649 for i in range(0, length): 01650 start = end 01651 end += 4 01652 (length,) = _struct_I.unpack(str[start:end]) 01653 start = end 01654 end += length 01655 val2 = str[start:end] 01656 val1.touch_links.append(val2) 01657 self.goal.planning_scene_diff.attached_collision_objects.append(val1) 01658 _x = self 01659 start = end 01660 end += 12 01661 (_x.goal.planning_scene_diff.collision_map.header.seq, _x.goal.planning_scene_diff.collision_map.header.stamp.secs, _x.goal.planning_scene_diff.collision_map.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 01662 start = end 01663 end += 4 01664 (length,) = _struct_I.unpack(str[start:end]) 01665 start = end 01666 end += length 01667 self.goal.planning_scene_diff.collision_map.header.frame_id = str[start:end] 01668 start = end 01669 end += 4 01670 (length,) = _struct_I.unpack(str[start:end]) 01671 self.goal.planning_scene_diff.collision_map.boxes = [] 01672 for i in range(0, length): 01673 val1 = arm_navigation_msgs.msg.OrientedBoundingBox() 01674 _v98 = val1.center 01675 _x = _v98 01676 start = end 01677 end += 12 01678 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end]) 01679 _v99 = val1.extents 01680 _x = _v99 01681 start = end 01682 end += 12 01683 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end]) 01684 _v100 = val1.axis 01685 _x = _v100 01686 start = end 01687 end += 12 01688 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end]) 01689 start = end 01690 end += 4 01691 (val1.angle,) = _struct_f.unpack(str[start:end]) 01692 self.goal.planning_scene_diff.collision_map.boxes.append(val1) 01693 start = end 01694 end += 1 01695 (self.goal.motion_plan_request.workspace_parameters.workspace_region_shape.type,) = _struct_b.unpack(str[start:end]) 01696 start = end 01697 end += 4 01698 (length,) = _struct_I.unpack(str[start:end]) 01699 pattern = '<%sd'%length 01700 start = end 01701 end += struct.calcsize(pattern) 01702 self.goal.motion_plan_request.workspace_parameters.workspace_region_shape.dimensions = struct.unpack(pattern, str[start:end]) 01703 start = end 01704 end += 4 01705 (length,) = _struct_I.unpack(str[start:end]) 01706 pattern = '<%si'%length 01707 start = end 01708 end += struct.calcsize(pattern) 01709 self.goal.motion_plan_request.workspace_parameters.workspace_region_shape.triangles = struct.unpack(pattern, str[start:end]) 01710 start = end 01711 end += 4 01712 (length,) = _struct_I.unpack(str[start:end]) 01713 self.goal.motion_plan_request.workspace_parameters.workspace_region_shape.vertices = [] 01714 for i in range(0, length): 01715 val1 = geometry_msgs.msg.Point() 01716 _x = val1 01717 start = end 01718 end += 24 01719 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 01720 self.goal.motion_plan_request.workspace_parameters.workspace_region_shape.vertices.append(val1) 01721 _x = self 01722 start = end 01723 end += 12 01724 (_x.goal.motion_plan_request.workspace_parameters.workspace_region_pose.header.seq, _x.goal.motion_plan_request.workspace_parameters.workspace_region_pose.header.stamp.secs, _x.goal.motion_plan_request.workspace_parameters.workspace_region_pose.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 01725 start = end 01726 end += 4 01727 (length,) = _struct_I.unpack(str[start:end]) 01728 start = end 01729 end += length 01730 self.goal.motion_plan_request.workspace_parameters.workspace_region_pose.header.frame_id = str[start:end] 01731 _x = self 01732 start = end 01733 end += 68 01734 (_x.goal.motion_plan_request.workspace_parameters.workspace_region_pose.pose.position.x, _x.goal.motion_plan_request.workspace_parameters.workspace_region_pose.pose.position.y, _x.goal.motion_plan_request.workspace_parameters.workspace_region_pose.pose.position.z, _x.goal.motion_plan_request.workspace_parameters.workspace_region_pose.pose.orientation.x, _x.goal.motion_plan_request.workspace_parameters.workspace_region_pose.pose.orientation.y, _x.goal.motion_plan_request.workspace_parameters.workspace_region_pose.pose.orientation.z, _x.goal.motion_plan_request.workspace_parameters.workspace_region_pose.pose.orientation.w, _x.goal.motion_plan_request.start_state.joint_state.header.seq, _x.goal.motion_plan_request.start_state.joint_state.header.stamp.secs, _x.goal.motion_plan_request.start_state.joint_state.header.stamp.nsecs,) = _struct_7d3I.unpack(str[start:end]) 01735 start = end 01736 end += 4 01737 (length,) = _struct_I.unpack(str[start:end]) 01738 start = end 01739 end += length 01740 self.goal.motion_plan_request.start_state.joint_state.header.frame_id = str[start:end] 01741 start = end 01742 end += 4 01743 (length,) = _struct_I.unpack(str[start:end]) 01744 self.goal.motion_plan_request.start_state.joint_state.name = [] 01745 for i in range(0, length): 01746 start = end 01747 end += 4 01748 (length,) = _struct_I.unpack(str[start:end]) 01749 start = end 01750 end += length 01751 val1 = str[start:end] 01752 self.goal.motion_plan_request.start_state.joint_state.name.append(val1) 01753 start = end 01754 end += 4 01755 (length,) = _struct_I.unpack(str[start:end]) 01756 pattern = '<%sd'%length 01757 start = end 01758 end += struct.calcsize(pattern) 01759 self.goal.motion_plan_request.start_state.joint_state.position = struct.unpack(pattern, str[start:end]) 01760 start = end 01761 end += 4 01762 (length,) = _struct_I.unpack(str[start:end]) 01763 pattern = '<%sd'%length 01764 start = end 01765 end += struct.calcsize(pattern) 01766 self.goal.motion_plan_request.start_state.joint_state.velocity = struct.unpack(pattern, str[start:end]) 01767 start = end 01768 end += 4 01769 (length,) = _struct_I.unpack(str[start:end]) 01770 pattern = '<%sd'%length 01771 start = end 01772 end += struct.calcsize(pattern) 01773 self.goal.motion_plan_request.start_state.joint_state.effort = struct.unpack(pattern, str[start:end]) 01774 _x = self 01775 start = end 01776 end += 8 01777 (_x.goal.motion_plan_request.start_state.multi_dof_joint_state.stamp.secs, _x.goal.motion_plan_request.start_state.multi_dof_joint_state.stamp.nsecs,) = _struct_2I.unpack(str[start:end]) 01778 start = end 01779 end += 4 01780 (length,) = _struct_I.unpack(str[start:end]) 01781 self.goal.motion_plan_request.start_state.multi_dof_joint_state.joint_names = [] 01782 for i in range(0, length): 01783 start = end 01784 end += 4 01785 (length,) = _struct_I.unpack(str[start:end]) 01786 start = end 01787 end += length 01788 val1 = str[start:end] 01789 self.goal.motion_plan_request.start_state.multi_dof_joint_state.joint_names.append(val1) 01790 start = end 01791 end += 4 01792 (length,) = _struct_I.unpack(str[start:end]) 01793 self.goal.motion_plan_request.start_state.multi_dof_joint_state.frame_ids = [] 01794 for i in range(0, length): 01795 start = end 01796 end += 4 01797 (length,) = _struct_I.unpack(str[start:end]) 01798 start = end 01799 end += length 01800 val1 = str[start:end] 01801 self.goal.motion_plan_request.start_state.multi_dof_joint_state.frame_ids.append(val1) 01802 start = end 01803 end += 4 01804 (length,) = _struct_I.unpack(str[start:end]) 01805 self.goal.motion_plan_request.start_state.multi_dof_joint_state.child_frame_ids = [] 01806 for i in range(0, length): 01807 start = end 01808 end += 4 01809 (length,) = _struct_I.unpack(str[start:end]) 01810 start = end 01811 end += length 01812 val1 = str[start:end] 01813 self.goal.motion_plan_request.start_state.multi_dof_joint_state.child_frame_ids.append(val1) 01814 start = end 01815 end += 4 01816 (length,) = _struct_I.unpack(str[start:end]) 01817 self.goal.motion_plan_request.start_state.multi_dof_joint_state.poses = [] 01818 for i in range(0, length): 01819 val1 = geometry_msgs.msg.Pose() 01820 _v101 = val1.position 01821 _x = _v101 01822 start = end 01823 end += 24 01824 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 01825 _v102 = val1.orientation 01826 _x = _v102 01827 start = end 01828 end += 32 01829 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 01830 self.goal.motion_plan_request.start_state.multi_dof_joint_state.poses.append(val1) 01831 start = end 01832 end += 4 01833 (length,) = _struct_I.unpack(str[start:end]) 01834 self.goal.motion_plan_request.goal_constraints.joint_constraints = [] 01835 for i in range(0, length): 01836 val1 = arm_navigation_msgs.msg.JointConstraint() 01837 start = end 01838 end += 4 01839 (length,) = _struct_I.unpack(str[start:end]) 01840 start = end 01841 end += length 01842 val1.joint_name = str[start:end] 01843 _x = val1 01844 start = end 01845 end += 32 01846 (_x.position, _x.tolerance_above, _x.tolerance_below, _x.weight,) = _struct_4d.unpack(str[start:end]) 01847 self.goal.motion_plan_request.goal_constraints.joint_constraints.append(val1) 01848 start = end 01849 end += 4 01850 (length,) = _struct_I.unpack(str[start:end]) 01851 self.goal.motion_plan_request.goal_constraints.position_constraints = [] 01852 for i in range(0, length): 01853 val1 = arm_navigation_msgs.msg.PositionConstraint() 01854 _v103 = val1.header 01855 start = end 01856 end += 4 01857 (_v103.seq,) = _struct_I.unpack(str[start:end]) 01858 _v104 = _v103.stamp 01859 _x = _v104 01860 start = end 01861 end += 8 01862 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01863 start = end 01864 end += 4 01865 (length,) = _struct_I.unpack(str[start:end]) 01866 start = end 01867 end += length 01868 _v103.frame_id = str[start:end] 01869 start = end 01870 end += 4 01871 (length,) = _struct_I.unpack(str[start:end]) 01872 start = end 01873 end += length 01874 val1.link_name = str[start:end] 01875 _v105 = val1.target_point_offset 01876 _x = _v105 01877 start = end 01878 end += 24 01879 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 01880 _v106 = val1.position 01881 _x = _v106 01882 start = end 01883 end += 24 01884 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 01885 _v107 = val1.constraint_region_shape 01886 start = end 01887 end += 1 01888 (_v107.type,) = _struct_b.unpack(str[start:end]) 01889 start = end 01890 end += 4 01891 (length,) = _struct_I.unpack(str[start:end]) 01892 pattern = '<%sd'%length 01893 start = end 01894 end += struct.calcsize(pattern) 01895 _v107.dimensions = struct.unpack(pattern, str[start:end]) 01896 start = end 01897 end += 4 01898 (length,) = _struct_I.unpack(str[start:end]) 01899 pattern = '<%si'%length 01900 start = end 01901 end += struct.calcsize(pattern) 01902 _v107.triangles = struct.unpack(pattern, str[start:end]) 01903 start = end 01904 end += 4 01905 (length,) = _struct_I.unpack(str[start:end]) 01906 _v107.vertices = [] 01907 for i in range(0, length): 01908 val3 = geometry_msgs.msg.Point() 01909 _x = val3 01910 start = end 01911 end += 24 01912 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 01913 _v107.vertices.append(val3) 01914 _v108 = val1.constraint_region_orientation 01915 _x = _v108 01916 start = end 01917 end += 32 01918 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 01919 start = end 01920 end += 8 01921 (val1.weight,) = _struct_d.unpack(str[start:end]) 01922 self.goal.motion_plan_request.goal_constraints.position_constraints.append(val1) 01923 start = end 01924 end += 4 01925 (length,) = _struct_I.unpack(str[start:end]) 01926 self.goal.motion_plan_request.goal_constraints.orientation_constraints = [] 01927 for i in range(0, length): 01928 val1 = arm_navigation_msgs.msg.OrientationConstraint() 01929 _v109 = val1.header 01930 start = end 01931 end += 4 01932 (_v109.seq,) = _struct_I.unpack(str[start:end]) 01933 _v110 = _v109.stamp 01934 _x = _v110 01935 start = end 01936 end += 8 01937 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01938 start = end 01939 end += 4 01940 (length,) = _struct_I.unpack(str[start:end]) 01941 start = end 01942 end += length 01943 _v109.frame_id = str[start:end] 01944 start = end 01945 end += 4 01946 (length,) = _struct_I.unpack(str[start:end]) 01947 start = end 01948 end += length 01949 val1.link_name = str[start:end] 01950 start = end 01951 end += 4 01952 (val1.type,) = _struct_i.unpack(str[start:end]) 01953 _v111 = val1.orientation 01954 _x = _v111 01955 start = end 01956 end += 32 01957 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 01958 _x = val1 01959 start = end 01960 end += 32 01961 (_x.absolute_roll_tolerance, _x.absolute_pitch_tolerance, _x.absolute_yaw_tolerance, _x.weight,) = _struct_4d.unpack(str[start:end]) 01962 self.goal.motion_plan_request.goal_constraints.orientation_constraints.append(val1) 01963 start = end 01964 end += 4 01965 (length,) = _struct_I.unpack(str[start:end]) 01966 self.goal.motion_plan_request.goal_constraints.visibility_constraints = [] 01967 for i in range(0, length): 01968 val1 = arm_navigation_msgs.msg.VisibilityConstraint() 01969 _v112 = val1.header 01970 start = end 01971 end += 4 01972 (_v112.seq,) = _struct_I.unpack(str[start:end]) 01973 _v113 = _v112.stamp 01974 _x = _v113 01975 start = end 01976 end += 8 01977 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01978 start = end 01979 end += 4 01980 (length,) = _struct_I.unpack(str[start:end]) 01981 start = end 01982 end += length 01983 _v112.frame_id = str[start:end] 01984 _v114 = val1.target 01985 _v115 = _v114.header 01986 start = end 01987 end += 4 01988 (_v115.seq,) = _struct_I.unpack(str[start:end]) 01989 _v116 = _v115.stamp 01990 _x = _v116 01991 start = end 01992 end += 8 01993 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01994 start = end 01995 end += 4 01996 (length,) = _struct_I.unpack(str[start:end]) 01997 start = end 01998 end += length 01999 _v115.frame_id = str[start:end] 02000 _v117 = _v114.point 02001 _x = _v117 02002 start = end 02003 end += 24 02004 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 02005 _v118 = val1.sensor_pose 02006 _v119 = _v118.header 02007 start = end 02008 end += 4 02009 (_v119.seq,) = _struct_I.unpack(str[start:end]) 02010 _v120 = _v119.stamp 02011 _x = _v120 02012 start = end 02013 end += 8 02014 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 02015 start = end 02016 end += 4 02017 (length,) = _struct_I.unpack(str[start:end]) 02018 start = end 02019 end += length 02020 _v119.frame_id = str[start:end] 02021 _v121 = _v118.pose 02022 _v122 = _v121.position 02023 _x = _v122 02024 start = end 02025 end += 24 02026 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 02027 _v123 = _v121.orientation 02028 _x = _v123 02029 start = end 02030 end += 32 02031 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 02032 start = end 02033 end += 8 02034 (val1.absolute_tolerance,) = _struct_d.unpack(str[start:end]) 02035 self.goal.motion_plan_request.goal_constraints.visibility_constraints.append(val1) 02036 start = end 02037 end += 4 02038 (length,) = _struct_I.unpack(str[start:end]) 02039 self.goal.motion_plan_request.path_constraints.joint_constraints = [] 02040 for i in range(0, length): 02041 val1 = arm_navigation_msgs.msg.JointConstraint() 02042 start = end 02043 end += 4 02044 (length,) = _struct_I.unpack(str[start:end]) 02045 start = end 02046 end += length 02047 val1.joint_name = str[start:end] 02048 _x = val1 02049 start = end 02050 end += 32 02051 (_x.position, _x.tolerance_above, _x.tolerance_below, _x.weight,) = _struct_4d.unpack(str[start:end]) 02052 self.goal.motion_plan_request.path_constraints.joint_constraints.append(val1) 02053 start = end 02054 end += 4 02055 (length,) = _struct_I.unpack(str[start:end]) 02056 self.goal.motion_plan_request.path_constraints.position_constraints = [] 02057 for i in range(0, length): 02058 val1 = arm_navigation_msgs.msg.PositionConstraint() 02059 _v124 = val1.header 02060 start = end 02061 end += 4 02062 (_v124.seq,) = _struct_I.unpack(str[start:end]) 02063 _v125 = _v124.stamp 02064 _x = _v125 02065 start = end 02066 end += 8 02067 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 02068 start = end 02069 end += 4 02070 (length,) = _struct_I.unpack(str[start:end]) 02071 start = end 02072 end += length 02073 _v124.frame_id = str[start:end] 02074 start = end 02075 end += 4 02076 (length,) = _struct_I.unpack(str[start:end]) 02077 start = end 02078 end += length 02079 val1.link_name = str[start:end] 02080 _v126 = val1.target_point_offset 02081 _x = _v126 02082 start = end 02083 end += 24 02084 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 02085 _v127 = val1.position 02086 _x = _v127 02087 start = end 02088 end += 24 02089 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 02090 _v128 = val1.constraint_region_shape 02091 start = end 02092 end += 1 02093 (_v128.type,) = _struct_b.unpack(str[start:end]) 02094 start = end 02095 end += 4 02096 (length,) = _struct_I.unpack(str[start:end]) 02097 pattern = '<%sd'%length 02098 start = end 02099 end += struct.calcsize(pattern) 02100 _v128.dimensions = struct.unpack(pattern, str[start:end]) 02101 start = end 02102 end += 4 02103 (length,) = _struct_I.unpack(str[start:end]) 02104 pattern = '<%si'%length 02105 start = end 02106 end += struct.calcsize(pattern) 02107 _v128.triangles = struct.unpack(pattern, str[start:end]) 02108 start = end 02109 end += 4 02110 (length,) = _struct_I.unpack(str[start:end]) 02111 _v128.vertices = [] 02112 for i in range(0, length): 02113 val3 = geometry_msgs.msg.Point() 02114 _x = val3 02115 start = end 02116 end += 24 02117 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 02118 _v128.vertices.append(val3) 02119 _v129 = val1.constraint_region_orientation 02120 _x = _v129 02121 start = end 02122 end += 32 02123 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 02124 start = end 02125 end += 8 02126 (val1.weight,) = _struct_d.unpack(str[start:end]) 02127 self.goal.motion_plan_request.path_constraints.position_constraints.append(val1) 02128 start = end 02129 end += 4 02130 (length,) = _struct_I.unpack(str[start:end]) 02131 self.goal.motion_plan_request.path_constraints.orientation_constraints = [] 02132 for i in range(0, length): 02133 val1 = arm_navigation_msgs.msg.OrientationConstraint() 02134 _v130 = val1.header 02135 start = end 02136 end += 4 02137 (_v130.seq,) = _struct_I.unpack(str[start:end]) 02138 _v131 = _v130.stamp 02139 _x = _v131 02140 start = end 02141 end += 8 02142 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 02143 start = end 02144 end += 4 02145 (length,) = _struct_I.unpack(str[start:end]) 02146 start = end 02147 end += length 02148 _v130.frame_id = str[start:end] 02149 start = end 02150 end += 4 02151 (length,) = _struct_I.unpack(str[start:end]) 02152 start = end 02153 end += length 02154 val1.link_name = str[start:end] 02155 start = end 02156 end += 4 02157 (val1.type,) = _struct_i.unpack(str[start:end]) 02158 _v132 = val1.orientation 02159 _x = _v132 02160 start = end 02161 end += 32 02162 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 02163 _x = val1 02164 start = end 02165 end += 32 02166 (_x.absolute_roll_tolerance, _x.absolute_pitch_tolerance, _x.absolute_yaw_tolerance, _x.weight,) = _struct_4d.unpack(str[start:end]) 02167 self.goal.motion_plan_request.path_constraints.orientation_constraints.append(val1) 02168 start = end 02169 end += 4 02170 (length,) = _struct_I.unpack(str[start:end]) 02171 self.goal.motion_plan_request.path_constraints.visibility_constraints = [] 02172 for i in range(0, length): 02173 val1 = arm_navigation_msgs.msg.VisibilityConstraint() 02174 _v133 = val1.header 02175 start = end 02176 end += 4 02177 (_v133.seq,) = _struct_I.unpack(str[start:end]) 02178 _v134 = _v133.stamp 02179 _x = _v134 02180 start = end 02181 end += 8 02182 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 02183 start = end 02184 end += 4 02185 (length,) = _struct_I.unpack(str[start:end]) 02186 start = end 02187 end += length 02188 _v133.frame_id = str[start:end] 02189 _v135 = val1.target 02190 _v136 = _v135.header 02191 start = end 02192 end += 4 02193 (_v136.seq,) = _struct_I.unpack(str[start:end]) 02194 _v137 = _v136.stamp 02195 _x = _v137 02196 start = end 02197 end += 8 02198 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 02199 start = end 02200 end += 4 02201 (length,) = _struct_I.unpack(str[start:end]) 02202 start = end 02203 end += length 02204 _v136.frame_id = str[start:end] 02205 _v138 = _v135.point 02206 _x = _v138 02207 start = end 02208 end += 24 02209 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 02210 _v139 = val1.sensor_pose 02211 _v140 = _v139.header 02212 start = end 02213 end += 4 02214 (_v140.seq,) = _struct_I.unpack(str[start:end]) 02215 _v141 = _v140.stamp 02216 _x = _v141 02217 start = end 02218 end += 8 02219 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 02220 start = end 02221 end += 4 02222 (length,) = _struct_I.unpack(str[start:end]) 02223 start = end 02224 end += length 02225 _v140.frame_id = str[start:end] 02226 _v142 = _v139.pose 02227 _v143 = _v142.position 02228 _x = _v143 02229 start = end 02230 end += 24 02231 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 02232 _v144 = _v142.orientation 02233 _x = _v144 02234 start = end 02235 end += 32 02236 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 02237 start = end 02238 end += 8 02239 (val1.absolute_tolerance,) = _struct_d.unpack(str[start:end]) 02240 self.goal.motion_plan_request.path_constraints.visibility_constraints.append(val1) 02241 start = end 02242 end += 4 02243 (length,) = _struct_I.unpack(str[start:end]) 02244 start = end 02245 end += length 02246 self.goal.motion_plan_request.planner_id = str[start:end] 02247 start = end 02248 end += 4 02249 (length,) = _struct_I.unpack(str[start:end]) 02250 start = end 02251 end += length 02252 self.goal.motion_plan_request.group_name = str[start:end] 02253 _x = self 02254 start = end 02255 end += 28 02256 (_x.goal.motion_plan_request.num_planning_attempts, _x.goal.motion_plan_request.allowed_planning_time.secs, _x.goal.motion_plan_request.allowed_planning_time.nsecs, _x.goal.motion_plan_request.expected_path_duration.secs, _x.goal.motion_plan_request.expected_path_duration.nsecs, _x.goal.motion_plan_request.expected_path_dt.secs, _x.goal.motion_plan_request.expected_path_dt.nsecs,) = _struct_7i.unpack(str[start:end]) 02257 start = end 02258 end += 4 02259 (length,) = _struct_I.unpack(str[start:end]) 02260 self.goal.operations.collision_operations = [] 02261 for i in range(0, length): 02262 val1 = arm_navigation_msgs.msg.CollisionOperation() 02263 start = end 02264 end += 4 02265 (length,) = _struct_I.unpack(str[start:end]) 02266 start = end 02267 end += length 02268 val1.object1 = str[start:end] 02269 start = end 02270 end += 4 02271 (length,) = _struct_I.unpack(str[start:end]) 02272 start = end 02273 end += length 02274 val1.object2 = str[start:end] 02275 _x = val1 02276 start = end 02277 end += 12 02278 (_x.penetration_distance, _x.operation,) = _struct_di.unpack(str[start:end]) 02279 self.goal.operations.collision_operations.append(val1) 02280 _x = self 02281 start = end 02282 end += 4 02283 (_x.goal.accept_partial_plans, _x.goal.accept_invalid_goals, _x.goal.disable_ik, _x.goal.disable_collision_monitoring,) = _struct_4B.unpack(str[start:end]) 02284 self.goal.accept_partial_plans = bool(self.goal.accept_partial_plans) 02285 self.goal.accept_invalid_goals = bool(self.goal.accept_invalid_goals) 02286 self.goal.disable_ik = bool(self.goal.disable_ik) 02287 self.goal.disable_collision_monitoring = bool(self.goal.disable_collision_monitoring) 02288 return self 02289 except struct.error as e: 02290 raise roslib.message.DeserializationError(e) #most likely buffer underfill 02291 02292 02293 def serialize_numpy(self, buff, numpy): 02294 """ 02295 serialize message with numpy array types into buffer 02296 @param buff: buffer 02297 @type buff: StringIO 02298 @param numpy: numpy python module 02299 @type numpy module 02300 """ 02301 try: 02302 _x = self 02303 buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs)) 02304 _x = self.header.frame_id 02305 length = len(_x) 02306 buff.write(struct.pack('<I%ss'%length, length, _x)) 02307 _x = self 02308 buff.write(_struct_2I.pack(_x.goal_id.stamp.secs, _x.goal_id.stamp.nsecs)) 02309 _x = self.goal_id.id 02310 length = len(_x) 02311 buff.write(struct.pack('<I%ss'%length, length, _x)) 02312 _x = self.goal.planner_service_name 02313 length = len(_x) 02314 buff.write(struct.pack('<I%ss'%length, length, _x)) 02315 _x = self 02316 buff.write(_struct_3I.pack(_x.goal.planning_scene_diff.robot_state.joint_state.header.seq, _x.goal.planning_scene_diff.robot_state.joint_state.header.stamp.secs, _x.goal.planning_scene_diff.robot_state.joint_state.header.stamp.nsecs)) 02317 _x = self.goal.planning_scene_diff.robot_state.joint_state.header.frame_id 02318 length = len(_x) 02319 buff.write(struct.pack('<I%ss'%length, length, _x)) 02320 length = len(self.goal.planning_scene_diff.robot_state.joint_state.name) 02321 buff.write(_struct_I.pack(length)) 02322 for val1 in self.goal.planning_scene_diff.robot_state.joint_state.name: 02323 length = len(val1) 02324 buff.write(struct.pack('<I%ss'%length, length, val1)) 02325 length = len(self.goal.planning_scene_diff.robot_state.joint_state.position) 02326 buff.write(_struct_I.pack(length)) 02327 pattern = '<%sd'%length 02328 buff.write(self.goal.planning_scene_diff.robot_state.joint_state.position.tostring()) 02329 length = len(self.goal.planning_scene_diff.robot_state.joint_state.velocity) 02330 buff.write(_struct_I.pack(length)) 02331 pattern = '<%sd'%length 02332 buff.write(self.goal.planning_scene_diff.robot_state.joint_state.velocity.tostring()) 02333 length = len(self.goal.planning_scene_diff.robot_state.joint_state.effort) 02334 buff.write(_struct_I.pack(length)) 02335 pattern = '<%sd'%length 02336 buff.write(self.goal.planning_scene_diff.robot_state.joint_state.effort.tostring()) 02337 _x = self 02338 buff.write(_struct_2I.pack(_x.goal.planning_scene_diff.robot_state.multi_dof_joint_state.stamp.secs, _x.goal.planning_scene_diff.robot_state.multi_dof_joint_state.stamp.nsecs)) 02339 length = len(self.goal.planning_scene_diff.robot_state.multi_dof_joint_state.joint_names) 02340 buff.write(_struct_I.pack(length)) 02341 for val1 in self.goal.planning_scene_diff.robot_state.multi_dof_joint_state.joint_names: 02342 length = len(val1) 02343 buff.write(struct.pack('<I%ss'%length, length, val1)) 02344 length = len(self.goal.planning_scene_diff.robot_state.multi_dof_joint_state.frame_ids) 02345 buff.write(_struct_I.pack(length)) 02346 for val1 in self.goal.planning_scene_diff.robot_state.multi_dof_joint_state.frame_ids: 02347 length = len(val1) 02348 buff.write(struct.pack('<I%ss'%length, length, val1)) 02349 length = len(self.goal.planning_scene_diff.robot_state.multi_dof_joint_state.child_frame_ids) 02350 buff.write(_struct_I.pack(length)) 02351 for val1 in self.goal.planning_scene_diff.robot_state.multi_dof_joint_state.child_frame_ids: 02352 length = len(val1) 02353 buff.write(struct.pack('<I%ss'%length, length, val1)) 02354 length = len(self.goal.planning_scene_diff.robot_state.multi_dof_joint_state.poses) 02355 buff.write(_struct_I.pack(length)) 02356 for val1 in self.goal.planning_scene_diff.robot_state.multi_dof_joint_state.poses: 02357 _v145 = val1.position 02358 _x = _v145 02359 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 02360 _v146 = val1.orientation 02361 _x = _v146 02362 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 02363 length = len(self.goal.planning_scene_diff.fixed_frame_transforms) 02364 buff.write(_struct_I.pack(length)) 02365 for val1 in self.goal.planning_scene_diff.fixed_frame_transforms: 02366 _v147 = val1.header 02367 buff.write(_struct_I.pack(_v147.seq)) 02368 _v148 = _v147.stamp 02369 _x = _v148 02370 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02371 _x = _v147.frame_id 02372 length = len(_x) 02373 buff.write(struct.pack('<I%ss'%length, length, _x)) 02374 _x = val1.child_frame_id 02375 length = len(_x) 02376 buff.write(struct.pack('<I%ss'%length, length, _x)) 02377 _v149 = val1.transform 02378 _v150 = _v149.translation 02379 _x = _v150 02380 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 02381 _v151 = _v149.rotation 02382 _x = _v151 02383 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 02384 length = len(self.goal.planning_scene_diff.allowed_collision_matrix.link_names) 02385 buff.write(_struct_I.pack(length)) 02386 for val1 in self.goal.planning_scene_diff.allowed_collision_matrix.link_names: 02387 length = len(val1) 02388 buff.write(struct.pack('<I%ss'%length, length, val1)) 02389 length = len(self.goal.planning_scene_diff.allowed_collision_matrix.entries) 02390 buff.write(_struct_I.pack(length)) 02391 for val1 in self.goal.planning_scene_diff.allowed_collision_matrix.entries: 02392 length = len(val1.enabled) 02393 buff.write(_struct_I.pack(length)) 02394 pattern = '<%sB'%length 02395 buff.write(val1.enabled.tostring()) 02396 length = len(self.goal.planning_scene_diff.allowed_contacts) 02397 buff.write(_struct_I.pack(length)) 02398 for val1 in self.goal.planning_scene_diff.allowed_contacts: 02399 _x = val1.name 02400 length = len(_x) 02401 buff.write(struct.pack('<I%ss'%length, length, _x)) 02402 _v152 = val1.shape 02403 buff.write(_struct_b.pack(_v152.type)) 02404 length = len(_v152.dimensions) 02405 buff.write(_struct_I.pack(length)) 02406 pattern = '<%sd'%length 02407 buff.write(_v152.dimensions.tostring()) 02408 length = len(_v152.triangles) 02409 buff.write(_struct_I.pack(length)) 02410 pattern = '<%si'%length 02411 buff.write(_v152.triangles.tostring()) 02412 length = len(_v152.vertices) 02413 buff.write(_struct_I.pack(length)) 02414 for val3 in _v152.vertices: 02415 _x = val3 02416 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 02417 _v153 = val1.pose_stamped 02418 _v154 = _v153.header 02419 buff.write(_struct_I.pack(_v154.seq)) 02420 _v155 = _v154.stamp 02421 _x = _v155 02422 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02423 _x = _v154.frame_id 02424 length = len(_x) 02425 buff.write(struct.pack('<I%ss'%length, length, _x)) 02426 _v156 = _v153.pose 02427 _v157 = _v156.position 02428 _x = _v157 02429 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 02430 _v158 = _v156.orientation 02431 _x = _v158 02432 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 02433 length = len(val1.link_names) 02434 buff.write(_struct_I.pack(length)) 02435 for val2 in val1.link_names: 02436 length = len(val2) 02437 buff.write(struct.pack('<I%ss'%length, length, val2)) 02438 buff.write(_struct_d.pack(val1.penetration_depth)) 02439 length = len(self.goal.planning_scene_diff.link_padding) 02440 buff.write(_struct_I.pack(length)) 02441 for val1 in self.goal.planning_scene_diff.link_padding: 02442 _x = val1.link_name 02443 length = len(_x) 02444 buff.write(struct.pack('<I%ss'%length, length, _x)) 02445 buff.write(_struct_d.pack(val1.padding)) 02446 length = len(self.goal.planning_scene_diff.collision_objects) 02447 buff.write(_struct_I.pack(length)) 02448 for val1 in self.goal.planning_scene_diff.collision_objects: 02449 _v159 = val1.header 02450 buff.write(_struct_I.pack(_v159.seq)) 02451 _v160 = _v159.stamp 02452 _x = _v160 02453 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02454 _x = _v159.frame_id 02455 length = len(_x) 02456 buff.write(struct.pack('<I%ss'%length, length, _x)) 02457 _x = val1.id 02458 length = len(_x) 02459 buff.write(struct.pack('<I%ss'%length, length, _x)) 02460 buff.write(_struct_f.pack(val1.padding)) 02461 _v161 = val1.operation 02462 buff.write(_struct_b.pack(_v161.operation)) 02463 length = len(val1.shapes) 02464 buff.write(_struct_I.pack(length)) 02465 for val2 in val1.shapes: 02466 buff.write(_struct_b.pack(val2.type)) 02467 length = len(val2.dimensions) 02468 buff.write(_struct_I.pack(length)) 02469 pattern = '<%sd'%length 02470 buff.write(val2.dimensions.tostring()) 02471 length = len(val2.triangles) 02472 buff.write(_struct_I.pack(length)) 02473 pattern = '<%si'%length 02474 buff.write(val2.triangles.tostring()) 02475 length = len(val2.vertices) 02476 buff.write(_struct_I.pack(length)) 02477 for val3 in val2.vertices: 02478 _x = val3 02479 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 02480 length = len(val1.poses) 02481 buff.write(_struct_I.pack(length)) 02482 for val2 in val1.poses: 02483 _v162 = val2.position 02484 _x = _v162 02485 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 02486 _v163 = val2.orientation 02487 _x = _v163 02488 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 02489 length = len(self.goal.planning_scene_diff.attached_collision_objects) 02490 buff.write(_struct_I.pack(length)) 02491 for val1 in self.goal.planning_scene_diff.attached_collision_objects: 02492 _x = val1.link_name 02493 length = len(_x) 02494 buff.write(struct.pack('<I%ss'%length, length, _x)) 02495 _v164 = val1.object 02496 _v165 = _v164.header 02497 buff.write(_struct_I.pack(_v165.seq)) 02498 _v166 = _v165.stamp 02499 _x = _v166 02500 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02501 _x = _v165.frame_id 02502 length = len(_x) 02503 buff.write(struct.pack('<I%ss'%length, length, _x)) 02504 _x = _v164.id 02505 length = len(_x) 02506 buff.write(struct.pack('<I%ss'%length, length, _x)) 02507 buff.write(_struct_f.pack(_v164.padding)) 02508 _v167 = _v164.operation 02509 buff.write(_struct_b.pack(_v167.operation)) 02510 length = len(_v164.shapes) 02511 buff.write(_struct_I.pack(length)) 02512 for val3 in _v164.shapes: 02513 buff.write(_struct_b.pack(val3.type)) 02514 length = len(val3.dimensions) 02515 buff.write(_struct_I.pack(length)) 02516 pattern = '<%sd'%length 02517 buff.write(val3.dimensions.tostring()) 02518 length = len(val3.triangles) 02519 buff.write(_struct_I.pack(length)) 02520 pattern = '<%si'%length 02521 buff.write(val3.triangles.tostring()) 02522 length = len(val3.vertices) 02523 buff.write(_struct_I.pack(length)) 02524 for val4 in val3.vertices: 02525 _x = val4 02526 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 02527 length = len(_v164.poses) 02528 buff.write(_struct_I.pack(length)) 02529 for val3 in _v164.poses: 02530 _v168 = val3.position 02531 _x = _v168 02532 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 02533 _v169 = val3.orientation 02534 _x = _v169 02535 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 02536 length = len(val1.touch_links) 02537 buff.write(_struct_I.pack(length)) 02538 for val2 in val1.touch_links: 02539 length = len(val2) 02540 buff.write(struct.pack('<I%ss'%length, length, val2)) 02541 _x = self 02542 buff.write(_struct_3I.pack(_x.goal.planning_scene_diff.collision_map.header.seq, _x.goal.planning_scene_diff.collision_map.header.stamp.secs, _x.goal.planning_scene_diff.collision_map.header.stamp.nsecs)) 02543 _x = self.goal.planning_scene_diff.collision_map.header.frame_id 02544 length = len(_x) 02545 buff.write(struct.pack('<I%ss'%length, length, _x)) 02546 length = len(self.goal.planning_scene_diff.collision_map.boxes) 02547 buff.write(_struct_I.pack(length)) 02548 for val1 in self.goal.planning_scene_diff.collision_map.boxes: 02549 _v170 = val1.center 02550 _x = _v170 02551 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z)) 02552 _v171 = val1.extents 02553 _x = _v171 02554 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z)) 02555 _v172 = val1.axis 02556 _x = _v172 02557 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z)) 02558 buff.write(_struct_f.pack(val1.angle)) 02559 buff.write(_struct_b.pack(self.goal.motion_plan_request.workspace_parameters.workspace_region_shape.type)) 02560 length = len(self.goal.motion_plan_request.workspace_parameters.workspace_region_shape.dimensions) 02561 buff.write(_struct_I.pack(length)) 02562 pattern = '<%sd'%length 02563 buff.write(self.goal.motion_plan_request.workspace_parameters.workspace_region_shape.dimensions.tostring()) 02564 length = len(self.goal.motion_plan_request.workspace_parameters.workspace_region_shape.triangles) 02565 buff.write(_struct_I.pack(length)) 02566 pattern = '<%si'%length 02567 buff.write(self.goal.motion_plan_request.workspace_parameters.workspace_region_shape.triangles.tostring()) 02568 length = len(self.goal.motion_plan_request.workspace_parameters.workspace_region_shape.vertices) 02569 buff.write(_struct_I.pack(length)) 02570 for val1 in self.goal.motion_plan_request.workspace_parameters.workspace_region_shape.vertices: 02571 _x = val1 02572 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 02573 _x = self 02574 buff.write(_struct_3I.pack(_x.goal.motion_plan_request.workspace_parameters.workspace_region_pose.header.seq, _x.goal.motion_plan_request.workspace_parameters.workspace_region_pose.header.stamp.secs, _x.goal.motion_plan_request.workspace_parameters.workspace_region_pose.header.stamp.nsecs)) 02575 _x = self.goal.motion_plan_request.workspace_parameters.workspace_region_pose.header.frame_id 02576 length = len(_x) 02577 buff.write(struct.pack('<I%ss'%length, length, _x)) 02578 _x = self 02579 buff.write(_struct_7d3I.pack(_x.goal.motion_plan_request.workspace_parameters.workspace_region_pose.pose.position.x, _x.goal.motion_plan_request.workspace_parameters.workspace_region_pose.pose.position.y, _x.goal.motion_plan_request.workspace_parameters.workspace_region_pose.pose.position.z, _x.goal.motion_plan_request.workspace_parameters.workspace_region_pose.pose.orientation.x, _x.goal.motion_plan_request.workspace_parameters.workspace_region_pose.pose.orientation.y, _x.goal.motion_plan_request.workspace_parameters.workspace_region_pose.pose.orientation.z, _x.goal.motion_plan_request.workspace_parameters.workspace_region_pose.pose.orientation.w, _x.goal.motion_plan_request.start_state.joint_state.header.seq, _x.goal.motion_plan_request.start_state.joint_state.header.stamp.secs, _x.goal.motion_plan_request.start_state.joint_state.header.stamp.nsecs)) 02580 _x = self.goal.motion_plan_request.start_state.joint_state.header.frame_id 02581 length = len(_x) 02582 buff.write(struct.pack('<I%ss'%length, length, _x)) 02583 length = len(self.goal.motion_plan_request.start_state.joint_state.name) 02584 buff.write(_struct_I.pack(length)) 02585 for val1 in self.goal.motion_plan_request.start_state.joint_state.name: 02586 length = len(val1) 02587 buff.write(struct.pack('<I%ss'%length, length, val1)) 02588 length = len(self.goal.motion_plan_request.start_state.joint_state.position) 02589 buff.write(_struct_I.pack(length)) 02590 pattern = '<%sd'%length 02591 buff.write(self.goal.motion_plan_request.start_state.joint_state.position.tostring()) 02592 length = len(self.goal.motion_plan_request.start_state.joint_state.velocity) 02593 buff.write(_struct_I.pack(length)) 02594 pattern = '<%sd'%length 02595 buff.write(self.goal.motion_plan_request.start_state.joint_state.velocity.tostring()) 02596 length = len(self.goal.motion_plan_request.start_state.joint_state.effort) 02597 buff.write(_struct_I.pack(length)) 02598 pattern = '<%sd'%length 02599 buff.write(self.goal.motion_plan_request.start_state.joint_state.effort.tostring()) 02600 _x = self 02601 buff.write(_struct_2I.pack(_x.goal.motion_plan_request.start_state.multi_dof_joint_state.stamp.secs, _x.goal.motion_plan_request.start_state.multi_dof_joint_state.stamp.nsecs)) 02602 length = len(self.goal.motion_plan_request.start_state.multi_dof_joint_state.joint_names) 02603 buff.write(_struct_I.pack(length)) 02604 for val1 in self.goal.motion_plan_request.start_state.multi_dof_joint_state.joint_names: 02605 length = len(val1) 02606 buff.write(struct.pack('<I%ss'%length, length, val1)) 02607 length = len(self.goal.motion_plan_request.start_state.multi_dof_joint_state.frame_ids) 02608 buff.write(_struct_I.pack(length)) 02609 for val1 in self.goal.motion_plan_request.start_state.multi_dof_joint_state.frame_ids: 02610 length = len(val1) 02611 buff.write(struct.pack('<I%ss'%length, length, val1)) 02612 length = len(self.goal.motion_plan_request.start_state.multi_dof_joint_state.child_frame_ids) 02613 buff.write(_struct_I.pack(length)) 02614 for val1 in self.goal.motion_plan_request.start_state.multi_dof_joint_state.child_frame_ids: 02615 length = len(val1) 02616 buff.write(struct.pack('<I%ss'%length, length, val1)) 02617 length = len(self.goal.motion_plan_request.start_state.multi_dof_joint_state.poses) 02618 buff.write(_struct_I.pack(length)) 02619 for val1 in self.goal.motion_plan_request.start_state.multi_dof_joint_state.poses: 02620 _v173 = val1.position 02621 _x = _v173 02622 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 02623 _v174 = val1.orientation 02624 _x = _v174 02625 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 02626 length = len(self.goal.motion_plan_request.goal_constraints.joint_constraints) 02627 buff.write(_struct_I.pack(length)) 02628 for val1 in self.goal.motion_plan_request.goal_constraints.joint_constraints: 02629 _x = val1.joint_name 02630 length = len(_x) 02631 buff.write(struct.pack('<I%ss'%length, length, _x)) 02632 _x = val1 02633 buff.write(_struct_4d.pack(_x.position, _x.tolerance_above, _x.tolerance_below, _x.weight)) 02634 length = len(self.goal.motion_plan_request.goal_constraints.position_constraints) 02635 buff.write(_struct_I.pack(length)) 02636 for val1 in self.goal.motion_plan_request.goal_constraints.position_constraints: 02637 _v175 = val1.header 02638 buff.write(_struct_I.pack(_v175.seq)) 02639 _v176 = _v175.stamp 02640 _x = _v176 02641 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02642 _x = _v175.frame_id 02643 length = len(_x) 02644 buff.write(struct.pack('<I%ss'%length, length, _x)) 02645 _x = val1.link_name 02646 length = len(_x) 02647 buff.write(struct.pack('<I%ss'%length, length, _x)) 02648 _v177 = val1.target_point_offset 02649 _x = _v177 02650 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 02651 _v178 = val1.position 02652 _x = _v178 02653 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 02654 _v179 = val1.constraint_region_shape 02655 buff.write(_struct_b.pack(_v179.type)) 02656 length = len(_v179.dimensions) 02657 buff.write(_struct_I.pack(length)) 02658 pattern = '<%sd'%length 02659 buff.write(_v179.dimensions.tostring()) 02660 length = len(_v179.triangles) 02661 buff.write(_struct_I.pack(length)) 02662 pattern = '<%si'%length 02663 buff.write(_v179.triangles.tostring()) 02664 length = len(_v179.vertices) 02665 buff.write(_struct_I.pack(length)) 02666 for val3 in _v179.vertices: 02667 _x = val3 02668 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 02669 _v180 = val1.constraint_region_orientation 02670 _x = _v180 02671 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 02672 buff.write(_struct_d.pack(val1.weight)) 02673 length = len(self.goal.motion_plan_request.goal_constraints.orientation_constraints) 02674 buff.write(_struct_I.pack(length)) 02675 for val1 in self.goal.motion_plan_request.goal_constraints.orientation_constraints: 02676 _v181 = val1.header 02677 buff.write(_struct_I.pack(_v181.seq)) 02678 _v182 = _v181.stamp 02679 _x = _v182 02680 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02681 _x = _v181.frame_id 02682 length = len(_x) 02683 buff.write(struct.pack('<I%ss'%length, length, _x)) 02684 _x = val1.link_name 02685 length = len(_x) 02686 buff.write(struct.pack('<I%ss'%length, length, _x)) 02687 buff.write(_struct_i.pack(val1.type)) 02688 _v183 = val1.orientation 02689 _x = _v183 02690 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 02691 _x = val1 02692 buff.write(_struct_4d.pack(_x.absolute_roll_tolerance, _x.absolute_pitch_tolerance, _x.absolute_yaw_tolerance, _x.weight)) 02693 length = len(self.goal.motion_plan_request.goal_constraints.visibility_constraints) 02694 buff.write(_struct_I.pack(length)) 02695 for val1 in self.goal.motion_plan_request.goal_constraints.visibility_constraints: 02696 _v184 = val1.header 02697 buff.write(_struct_I.pack(_v184.seq)) 02698 _v185 = _v184.stamp 02699 _x = _v185 02700 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02701 _x = _v184.frame_id 02702 length = len(_x) 02703 buff.write(struct.pack('<I%ss'%length, length, _x)) 02704 _v186 = val1.target 02705 _v187 = _v186.header 02706 buff.write(_struct_I.pack(_v187.seq)) 02707 _v188 = _v187.stamp 02708 _x = _v188 02709 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02710 _x = _v187.frame_id 02711 length = len(_x) 02712 buff.write(struct.pack('<I%ss'%length, length, _x)) 02713 _v189 = _v186.point 02714 _x = _v189 02715 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 02716 _v190 = val1.sensor_pose 02717 _v191 = _v190.header 02718 buff.write(_struct_I.pack(_v191.seq)) 02719 _v192 = _v191.stamp 02720 _x = _v192 02721 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02722 _x = _v191.frame_id 02723 length = len(_x) 02724 buff.write(struct.pack('<I%ss'%length, length, _x)) 02725 _v193 = _v190.pose 02726 _v194 = _v193.position 02727 _x = _v194 02728 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 02729 _v195 = _v193.orientation 02730 _x = _v195 02731 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 02732 buff.write(_struct_d.pack(val1.absolute_tolerance)) 02733 length = len(self.goal.motion_plan_request.path_constraints.joint_constraints) 02734 buff.write(_struct_I.pack(length)) 02735 for val1 in self.goal.motion_plan_request.path_constraints.joint_constraints: 02736 _x = val1.joint_name 02737 length = len(_x) 02738 buff.write(struct.pack('<I%ss'%length, length, _x)) 02739 _x = val1 02740 buff.write(_struct_4d.pack(_x.position, _x.tolerance_above, _x.tolerance_below, _x.weight)) 02741 length = len(self.goal.motion_plan_request.path_constraints.position_constraints) 02742 buff.write(_struct_I.pack(length)) 02743 for val1 in self.goal.motion_plan_request.path_constraints.position_constraints: 02744 _v196 = val1.header 02745 buff.write(_struct_I.pack(_v196.seq)) 02746 _v197 = _v196.stamp 02747 _x = _v197 02748 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02749 _x = _v196.frame_id 02750 length = len(_x) 02751 buff.write(struct.pack('<I%ss'%length, length, _x)) 02752 _x = val1.link_name 02753 length = len(_x) 02754 buff.write(struct.pack('<I%ss'%length, length, _x)) 02755 _v198 = val1.target_point_offset 02756 _x = _v198 02757 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 02758 _v199 = val1.position 02759 _x = _v199 02760 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 02761 _v200 = val1.constraint_region_shape 02762 buff.write(_struct_b.pack(_v200.type)) 02763 length = len(_v200.dimensions) 02764 buff.write(_struct_I.pack(length)) 02765 pattern = '<%sd'%length 02766 buff.write(_v200.dimensions.tostring()) 02767 length = len(_v200.triangles) 02768 buff.write(_struct_I.pack(length)) 02769 pattern = '<%si'%length 02770 buff.write(_v200.triangles.tostring()) 02771 length = len(_v200.vertices) 02772 buff.write(_struct_I.pack(length)) 02773 for val3 in _v200.vertices: 02774 _x = val3 02775 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 02776 _v201 = val1.constraint_region_orientation 02777 _x = _v201 02778 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 02779 buff.write(_struct_d.pack(val1.weight)) 02780 length = len(self.goal.motion_plan_request.path_constraints.orientation_constraints) 02781 buff.write(_struct_I.pack(length)) 02782 for val1 in self.goal.motion_plan_request.path_constraints.orientation_constraints: 02783 _v202 = val1.header 02784 buff.write(_struct_I.pack(_v202.seq)) 02785 _v203 = _v202.stamp 02786 _x = _v203 02787 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02788 _x = _v202.frame_id 02789 length = len(_x) 02790 buff.write(struct.pack('<I%ss'%length, length, _x)) 02791 _x = val1.link_name 02792 length = len(_x) 02793 buff.write(struct.pack('<I%ss'%length, length, _x)) 02794 buff.write(_struct_i.pack(val1.type)) 02795 _v204 = val1.orientation 02796 _x = _v204 02797 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 02798 _x = val1 02799 buff.write(_struct_4d.pack(_x.absolute_roll_tolerance, _x.absolute_pitch_tolerance, _x.absolute_yaw_tolerance, _x.weight)) 02800 length = len(self.goal.motion_plan_request.path_constraints.visibility_constraints) 02801 buff.write(_struct_I.pack(length)) 02802 for val1 in self.goal.motion_plan_request.path_constraints.visibility_constraints: 02803 _v205 = val1.header 02804 buff.write(_struct_I.pack(_v205.seq)) 02805 _v206 = _v205.stamp 02806 _x = _v206 02807 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02808 _x = _v205.frame_id 02809 length = len(_x) 02810 buff.write(struct.pack('<I%ss'%length, length, _x)) 02811 _v207 = val1.target 02812 _v208 = _v207.header 02813 buff.write(_struct_I.pack(_v208.seq)) 02814 _v209 = _v208.stamp 02815 _x = _v209 02816 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02817 _x = _v208.frame_id 02818 length = len(_x) 02819 buff.write(struct.pack('<I%ss'%length, length, _x)) 02820 _v210 = _v207.point 02821 _x = _v210 02822 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 02823 _v211 = val1.sensor_pose 02824 _v212 = _v211.header 02825 buff.write(_struct_I.pack(_v212.seq)) 02826 _v213 = _v212.stamp 02827 _x = _v213 02828 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02829 _x = _v212.frame_id 02830 length = len(_x) 02831 buff.write(struct.pack('<I%ss'%length, length, _x)) 02832 _v214 = _v211.pose 02833 _v215 = _v214.position 02834 _x = _v215 02835 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 02836 _v216 = _v214.orientation 02837 _x = _v216 02838 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 02839 buff.write(_struct_d.pack(val1.absolute_tolerance)) 02840 _x = self.goal.motion_plan_request.planner_id 02841 length = len(_x) 02842 buff.write(struct.pack('<I%ss'%length, length, _x)) 02843 _x = self.goal.motion_plan_request.group_name 02844 length = len(_x) 02845 buff.write(struct.pack('<I%ss'%length, length, _x)) 02846 _x = self 02847 buff.write(_struct_7i.pack(_x.goal.motion_plan_request.num_planning_attempts, _x.goal.motion_plan_request.allowed_planning_time.secs, _x.goal.motion_plan_request.allowed_planning_time.nsecs, _x.goal.motion_plan_request.expected_path_duration.secs, _x.goal.motion_plan_request.expected_path_duration.nsecs, _x.goal.motion_plan_request.expected_path_dt.secs, _x.goal.motion_plan_request.expected_path_dt.nsecs)) 02848 length = len(self.goal.operations.collision_operations) 02849 buff.write(_struct_I.pack(length)) 02850 for val1 in self.goal.operations.collision_operations: 02851 _x = val1.object1 02852 length = len(_x) 02853 buff.write(struct.pack('<I%ss'%length, length, _x)) 02854 _x = val1.object2 02855 length = len(_x) 02856 buff.write(struct.pack('<I%ss'%length, length, _x)) 02857 _x = val1 02858 buff.write(_struct_di.pack(_x.penetration_distance, _x.operation)) 02859 _x = self 02860 buff.write(_struct_4B.pack(_x.goal.accept_partial_plans, _x.goal.accept_invalid_goals, _x.goal.disable_ik, _x.goal.disable_collision_monitoring)) 02861 except struct.error as se: self._check_types(se) 02862 except TypeError as te: self._check_types(te) 02863 02864 def deserialize_numpy(self, str, numpy): 02865 """ 02866 unpack serialized message in str into this message instance using numpy for array types 02867 @param str: byte array of serialized message 02868 @type str: str 02869 @param numpy: numpy python module 02870 @type numpy: module 02871 """ 02872 try: 02873 if self.header is None: 02874 self.header = std_msgs.msg._Header.Header() 02875 if self.goal_id is None: 02876 self.goal_id = actionlib_msgs.msg.GoalID() 02877 if self.goal is None: 02878 self.goal = arm_navigation_msgs.msg.MoveArmGoal() 02879 end = 0 02880 _x = self 02881 start = end 02882 end += 12 02883 (_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 02884 start = end 02885 end += 4 02886 (length,) = _struct_I.unpack(str[start:end]) 02887 start = end 02888 end += length 02889 self.header.frame_id = str[start:end] 02890 _x = self 02891 start = end 02892 end += 8 02893 (_x.goal_id.stamp.secs, _x.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end]) 02894 start = end 02895 end += 4 02896 (length,) = _struct_I.unpack(str[start:end]) 02897 start = end 02898 end += length 02899 self.goal_id.id = str[start:end] 02900 start = end 02901 end += 4 02902 (length,) = _struct_I.unpack(str[start:end]) 02903 start = end 02904 end += length 02905 self.goal.planner_service_name = str[start:end] 02906 _x = self 02907 start = end 02908 end += 12 02909 (_x.goal.planning_scene_diff.robot_state.joint_state.header.seq, _x.goal.planning_scene_diff.robot_state.joint_state.header.stamp.secs, _x.goal.planning_scene_diff.robot_state.joint_state.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 02910 start = end 02911 end += 4 02912 (length,) = _struct_I.unpack(str[start:end]) 02913 start = end 02914 end += length 02915 self.goal.planning_scene_diff.robot_state.joint_state.header.frame_id = str[start:end] 02916 start = end 02917 end += 4 02918 (length,) = _struct_I.unpack(str[start:end]) 02919 self.goal.planning_scene_diff.robot_state.joint_state.name = [] 02920 for i in range(0, length): 02921 start = end 02922 end += 4 02923 (length,) = _struct_I.unpack(str[start:end]) 02924 start = end 02925 end += length 02926 val1 = str[start:end] 02927 self.goal.planning_scene_diff.robot_state.joint_state.name.append(val1) 02928 start = end 02929 end += 4 02930 (length,) = _struct_I.unpack(str[start:end]) 02931 pattern = '<%sd'%length 02932 start = end 02933 end += struct.calcsize(pattern) 02934 self.goal.planning_scene_diff.robot_state.joint_state.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 02935 start = end 02936 end += 4 02937 (length,) = _struct_I.unpack(str[start:end]) 02938 pattern = '<%sd'%length 02939 start = end 02940 end += struct.calcsize(pattern) 02941 self.goal.planning_scene_diff.robot_state.joint_state.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 02942 start = end 02943 end += 4 02944 (length,) = _struct_I.unpack(str[start:end]) 02945 pattern = '<%sd'%length 02946 start = end 02947 end += struct.calcsize(pattern) 02948 self.goal.planning_scene_diff.robot_state.joint_state.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 02949 _x = self 02950 start = end 02951 end += 8 02952 (_x.goal.planning_scene_diff.robot_state.multi_dof_joint_state.stamp.secs, _x.goal.planning_scene_diff.robot_state.multi_dof_joint_state.stamp.nsecs,) = _struct_2I.unpack(str[start:end]) 02953 start = end 02954 end += 4 02955 (length,) = _struct_I.unpack(str[start:end]) 02956 self.goal.planning_scene_diff.robot_state.multi_dof_joint_state.joint_names = [] 02957 for i in range(0, length): 02958 start = end 02959 end += 4 02960 (length,) = _struct_I.unpack(str[start:end]) 02961 start = end 02962 end += length 02963 val1 = str[start:end] 02964 self.goal.planning_scene_diff.robot_state.multi_dof_joint_state.joint_names.append(val1) 02965 start = end 02966 end += 4 02967 (length,) = _struct_I.unpack(str[start:end]) 02968 self.goal.planning_scene_diff.robot_state.multi_dof_joint_state.frame_ids = [] 02969 for i in range(0, length): 02970 start = end 02971 end += 4 02972 (length,) = _struct_I.unpack(str[start:end]) 02973 start = end 02974 end += length 02975 val1 = str[start:end] 02976 self.goal.planning_scene_diff.robot_state.multi_dof_joint_state.frame_ids.append(val1) 02977 start = end 02978 end += 4 02979 (length,) = _struct_I.unpack(str[start:end]) 02980 self.goal.planning_scene_diff.robot_state.multi_dof_joint_state.child_frame_ids = [] 02981 for i in range(0, length): 02982 start = end 02983 end += 4 02984 (length,) = _struct_I.unpack(str[start:end]) 02985 start = end 02986 end += length 02987 val1 = str[start:end] 02988 self.goal.planning_scene_diff.robot_state.multi_dof_joint_state.child_frame_ids.append(val1) 02989 start = end 02990 end += 4 02991 (length,) = _struct_I.unpack(str[start:end]) 02992 self.goal.planning_scene_diff.robot_state.multi_dof_joint_state.poses = [] 02993 for i in range(0, length): 02994 val1 = geometry_msgs.msg.Pose() 02995 _v217 = val1.position 02996 _x = _v217 02997 start = end 02998 end += 24 02999 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 03000 _v218 = val1.orientation 03001 _x = _v218 03002 start = end 03003 end += 32 03004 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 03005 self.goal.planning_scene_diff.robot_state.multi_dof_joint_state.poses.append(val1) 03006 start = end 03007 end += 4 03008 (length,) = _struct_I.unpack(str[start:end]) 03009 self.goal.planning_scene_diff.fixed_frame_transforms = [] 03010 for i in range(0, length): 03011 val1 = geometry_msgs.msg.TransformStamped() 03012 _v219 = val1.header 03013 start = end 03014 end += 4 03015 (_v219.seq,) = _struct_I.unpack(str[start:end]) 03016 _v220 = _v219.stamp 03017 _x = _v220 03018 start = end 03019 end += 8 03020 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 03021 start = end 03022 end += 4 03023 (length,) = _struct_I.unpack(str[start:end]) 03024 start = end 03025 end += length 03026 _v219.frame_id = str[start:end] 03027 start = end 03028 end += 4 03029 (length,) = _struct_I.unpack(str[start:end]) 03030 start = end 03031 end += length 03032 val1.child_frame_id = str[start:end] 03033 _v221 = val1.transform 03034 _v222 = _v221.translation 03035 _x = _v222 03036 start = end 03037 end += 24 03038 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 03039 _v223 = _v221.rotation 03040 _x = _v223 03041 start = end 03042 end += 32 03043 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 03044 self.goal.planning_scene_diff.fixed_frame_transforms.append(val1) 03045 start = end 03046 end += 4 03047 (length,) = _struct_I.unpack(str[start:end]) 03048 self.goal.planning_scene_diff.allowed_collision_matrix.link_names = [] 03049 for i in range(0, length): 03050 start = end 03051 end += 4 03052 (length,) = _struct_I.unpack(str[start:end]) 03053 start = end 03054 end += length 03055 val1 = str[start:end] 03056 self.goal.planning_scene_diff.allowed_collision_matrix.link_names.append(val1) 03057 start = end 03058 end += 4 03059 (length,) = _struct_I.unpack(str[start:end]) 03060 self.goal.planning_scene_diff.allowed_collision_matrix.entries = [] 03061 for i in range(0, length): 03062 val1 = arm_navigation_msgs.msg.AllowedCollisionEntry() 03063 start = end 03064 end += 4 03065 (length,) = _struct_I.unpack(str[start:end]) 03066 pattern = '<%sB'%length 03067 start = end 03068 end += struct.calcsize(pattern) 03069 val1.enabled = numpy.frombuffer(str[start:end], dtype=numpy.bool, count=length) 03070 val1.enabled = map(bool, val1.enabled) 03071 self.goal.planning_scene_diff.allowed_collision_matrix.entries.append(val1) 03072 start = end 03073 end += 4 03074 (length,) = _struct_I.unpack(str[start:end]) 03075 self.goal.planning_scene_diff.allowed_contacts = [] 03076 for i in range(0, length): 03077 val1 = arm_navigation_msgs.msg.AllowedContactSpecification() 03078 start = end 03079 end += 4 03080 (length,) = _struct_I.unpack(str[start:end]) 03081 start = end 03082 end += length 03083 val1.name = str[start:end] 03084 _v224 = val1.shape 03085 start = end 03086 end += 1 03087 (_v224.type,) = _struct_b.unpack(str[start:end]) 03088 start = end 03089 end += 4 03090 (length,) = _struct_I.unpack(str[start:end]) 03091 pattern = '<%sd'%length 03092 start = end 03093 end += struct.calcsize(pattern) 03094 _v224.dimensions = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 03095 start = end 03096 end += 4 03097 (length,) = _struct_I.unpack(str[start:end]) 03098 pattern = '<%si'%length 03099 start = end 03100 end += struct.calcsize(pattern) 03101 _v224.triangles = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length) 03102 start = end 03103 end += 4 03104 (length,) = _struct_I.unpack(str[start:end]) 03105 _v224.vertices = [] 03106 for i in range(0, length): 03107 val3 = geometry_msgs.msg.Point() 03108 _x = val3 03109 start = end 03110 end += 24 03111 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 03112 _v224.vertices.append(val3) 03113 _v225 = val1.pose_stamped 03114 _v226 = _v225.header 03115 start = end 03116 end += 4 03117 (_v226.seq,) = _struct_I.unpack(str[start:end]) 03118 _v227 = _v226.stamp 03119 _x = _v227 03120 start = end 03121 end += 8 03122 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 03123 start = end 03124 end += 4 03125 (length,) = _struct_I.unpack(str[start:end]) 03126 start = end 03127 end += length 03128 _v226.frame_id = str[start:end] 03129 _v228 = _v225.pose 03130 _v229 = _v228.position 03131 _x = _v229 03132 start = end 03133 end += 24 03134 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 03135 _v230 = _v228.orientation 03136 _x = _v230 03137 start = end 03138 end += 32 03139 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 03140 start = end 03141 end += 4 03142 (length,) = _struct_I.unpack(str[start:end]) 03143 val1.link_names = [] 03144 for i in range(0, length): 03145 start = end 03146 end += 4 03147 (length,) = _struct_I.unpack(str[start:end]) 03148 start = end 03149 end += length 03150 val2 = str[start:end] 03151 val1.link_names.append(val2) 03152 start = end 03153 end += 8 03154 (val1.penetration_depth,) = _struct_d.unpack(str[start:end]) 03155 self.goal.planning_scene_diff.allowed_contacts.append(val1) 03156 start = end 03157 end += 4 03158 (length,) = _struct_I.unpack(str[start:end]) 03159 self.goal.planning_scene_diff.link_padding = [] 03160 for i in range(0, length): 03161 val1 = arm_navigation_msgs.msg.LinkPadding() 03162 start = end 03163 end += 4 03164 (length,) = _struct_I.unpack(str[start:end]) 03165 start = end 03166 end += length 03167 val1.link_name = str[start:end] 03168 start = end 03169 end += 8 03170 (val1.padding,) = _struct_d.unpack(str[start:end]) 03171 self.goal.planning_scene_diff.link_padding.append(val1) 03172 start = end 03173 end += 4 03174 (length,) = _struct_I.unpack(str[start:end]) 03175 self.goal.planning_scene_diff.collision_objects = [] 03176 for i in range(0, length): 03177 val1 = arm_navigation_msgs.msg.CollisionObject() 03178 _v231 = val1.header 03179 start = end 03180 end += 4 03181 (_v231.seq,) = _struct_I.unpack(str[start:end]) 03182 _v232 = _v231.stamp 03183 _x = _v232 03184 start = end 03185 end += 8 03186 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 03187 start = end 03188 end += 4 03189 (length,) = _struct_I.unpack(str[start:end]) 03190 start = end 03191 end += length 03192 _v231.frame_id = str[start:end] 03193 start = end 03194 end += 4 03195 (length,) = _struct_I.unpack(str[start:end]) 03196 start = end 03197 end += length 03198 val1.id = str[start:end] 03199 start = end 03200 end += 4 03201 (val1.padding,) = _struct_f.unpack(str[start:end]) 03202 _v233 = val1.operation 03203 start = end 03204 end += 1 03205 (_v233.operation,) = _struct_b.unpack(str[start:end]) 03206 start = end 03207 end += 4 03208 (length,) = _struct_I.unpack(str[start:end]) 03209 val1.shapes = [] 03210 for i in range(0, length): 03211 val2 = arm_navigation_msgs.msg.Shape() 03212 start = end 03213 end += 1 03214 (val2.type,) = _struct_b.unpack(str[start:end]) 03215 start = end 03216 end += 4 03217 (length,) = _struct_I.unpack(str[start:end]) 03218 pattern = '<%sd'%length 03219 start = end 03220 end += struct.calcsize(pattern) 03221 val2.dimensions = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 03222 start = end 03223 end += 4 03224 (length,) = _struct_I.unpack(str[start:end]) 03225 pattern = '<%si'%length 03226 start = end 03227 end += struct.calcsize(pattern) 03228 val2.triangles = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length) 03229 start = end 03230 end += 4 03231 (length,) = _struct_I.unpack(str[start:end]) 03232 val2.vertices = [] 03233 for i in range(0, length): 03234 val3 = geometry_msgs.msg.Point() 03235 _x = val3 03236 start = end 03237 end += 24 03238 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 03239 val2.vertices.append(val3) 03240 val1.shapes.append(val2) 03241 start = end 03242 end += 4 03243 (length,) = _struct_I.unpack(str[start:end]) 03244 val1.poses = [] 03245 for i in range(0, length): 03246 val2 = geometry_msgs.msg.Pose() 03247 _v234 = val2.position 03248 _x = _v234 03249 start = end 03250 end += 24 03251 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 03252 _v235 = val2.orientation 03253 _x = _v235 03254 start = end 03255 end += 32 03256 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 03257 val1.poses.append(val2) 03258 self.goal.planning_scene_diff.collision_objects.append(val1) 03259 start = end 03260 end += 4 03261 (length,) = _struct_I.unpack(str[start:end]) 03262 self.goal.planning_scene_diff.attached_collision_objects = [] 03263 for i in range(0, length): 03264 val1 = arm_navigation_msgs.msg.AttachedCollisionObject() 03265 start = end 03266 end += 4 03267 (length,) = _struct_I.unpack(str[start:end]) 03268 start = end 03269 end += length 03270 val1.link_name = str[start:end] 03271 _v236 = val1.object 03272 _v237 = _v236.header 03273 start = end 03274 end += 4 03275 (_v237.seq,) = _struct_I.unpack(str[start:end]) 03276 _v238 = _v237.stamp 03277 _x = _v238 03278 start = end 03279 end += 8 03280 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 03281 start = end 03282 end += 4 03283 (length,) = _struct_I.unpack(str[start:end]) 03284 start = end 03285 end += length 03286 _v237.frame_id = str[start:end] 03287 start = end 03288 end += 4 03289 (length,) = _struct_I.unpack(str[start:end]) 03290 start = end 03291 end += length 03292 _v236.id = str[start:end] 03293 start = end 03294 end += 4 03295 (_v236.padding,) = _struct_f.unpack(str[start:end]) 03296 _v239 = _v236.operation 03297 start = end 03298 end += 1 03299 (_v239.operation,) = _struct_b.unpack(str[start:end]) 03300 start = end 03301 end += 4 03302 (length,) = _struct_I.unpack(str[start:end]) 03303 _v236.shapes = [] 03304 for i in range(0, length): 03305 val3 = arm_navigation_msgs.msg.Shape() 03306 start = end 03307 end += 1 03308 (val3.type,) = _struct_b.unpack(str[start:end]) 03309 start = end 03310 end += 4 03311 (length,) = _struct_I.unpack(str[start:end]) 03312 pattern = '<%sd'%length 03313 start = end 03314 end += struct.calcsize(pattern) 03315 val3.dimensions = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 03316 start = end 03317 end += 4 03318 (length,) = _struct_I.unpack(str[start:end]) 03319 pattern = '<%si'%length 03320 start = end 03321 end += struct.calcsize(pattern) 03322 val3.triangles = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length) 03323 start = end 03324 end += 4 03325 (length,) = _struct_I.unpack(str[start:end]) 03326 val3.vertices = [] 03327 for i in range(0, length): 03328 val4 = geometry_msgs.msg.Point() 03329 _x = val4 03330 start = end 03331 end += 24 03332 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 03333 val3.vertices.append(val4) 03334 _v236.shapes.append(val3) 03335 start = end 03336 end += 4 03337 (length,) = _struct_I.unpack(str[start:end]) 03338 _v236.poses = [] 03339 for i in range(0, length): 03340 val3 = geometry_msgs.msg.Pose() 03341 _v240 = val3.position 03342 _x = _v240 03343 start = end 03344 end += 24 03345 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 03346 _v241 = val3.orientation 03347 _x = _v241 03348 start = end 03349 end += 32 03350 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 03351 _v236.poses.append(val3) 03352 start = end 03353 end += 4 03354 (length,) = _struct_I.unpack(str[start:end]) 03355 val1.touch_links = [] 03356 for i in range(0, length): 03357 start = end 03358 end += 4 03359 (length,) = _struct_I.unpack(str[start:end]) 03360 start = end 03361 end += length 03362 val2 = str[start:end] 03363 val1.touch_links.append(val2) 03364 self.goal.planning_scene_diff.attached_collision_objects.append(val1) 03365 _x = self 03366 start = end 03367 end += 12 03368 (_x.goal.planning_scene_diff.collision_map.header.seq, _x.goal.planning_scene_diff.collision_map.header.stamp.secs, _x.goal.planning_scene_diff.collision_map.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 03369 start = end 03370 end += 4 03371 (length,) = _struct_I.unpack(str[start:end]) 03372 start = end 03373 end += length 03374 self.goal.planning_scene_diff.collision_map.header.frame_id = str[start:end] 03375 start = end 03376 end += 4 03377 (length,) = _struct_I.unpack(str[start:end]) 03378 self.goal.planning_scene_diff.collision_map.boxes = [] 03379 for i in range(0, length): 03380 val1 = arm_navigation_msgs.msg.OrientedBoundingBox() 03381 _v242 = val1.center 03382 _x = _v242 03383 start = end 03384 end += 12 03385 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end]) 03386 _v243 = val1.extents 03387 _x = _v243 03388 start = end 03389 end += 12 03390 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end]) 03391 _v244 = val1.axis 03392 _x = _v244 03393 start = end 03394 end += 12 03395 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end]) 03396 start = end 03397 end += 4 03398 (val1.angle,) = _struct_f.unpack(str[start:end]) 03399 self.goal.planning_scene_diff.collision_map.boxes.append(val1) 03400 start = end 03401 end += 1 03402 (self.goal.motion_plan_request.workspace_parameters.workspace_region_shape.type,) = _struct_b.unpack(str[start:end]) 03403 start = end 03404 end += 4 03405 (length,) = _struct_I.unpack(str[start:end]) 03406 pattern = '<%sd'%length 03407 start = end 03408 end += struct.calcsize(pattern) 03409 self.goal.motion_plan_request.workspace_parameters.workspace_region_shape.dimensions = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 03410 start = end 03411 end += 4 03412 (length,) = _struct_I.unpack(str[start:end]) 03413 pattern = '<%si'%length 03414 start = end 03415 end += struct.calcsize(pattern) 03416 self.goal.motion_plan_request.workspace_parameters.workspace_region_shape.triangles = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length) 03417 start = end 03418 end += 4 03419 (length,) = _struct_I.unpack(str[start:end]) 03420 self.goal.motion_plan_request.workspace_parameters.workspace_region_shape.vertices = [] 03421 for i in range(0, length): 03422 val1 = geometry_msgs.msg.Point() 03423 _x = val1 03424 start = end 03425 end += 24 03426 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 03427 self.goal.motion_plan_request.workspace_parameters.workspace_region_shape.vertices.append(val1) 03428 _x = self 03429 start = end 03430 end += 12 03431 (_x.goal.motion_plan_request.workspace_parameters.workspace_region_pose.header.seq, _x.goal.motion_plan_request.workspace_parameters.workspace_region_pose.header.stamp.secs, _x.goal.motion_plan_request.workspace_parameters.workspace_region_pose.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 03432 start = end 03433 end += 4 03434 (length,) = _struct_I.unpack(str[start:end]) 03435 start = end 03436 end += length 03437 self.goal.motion_plan_request.workspace_parameters.workspace_region_pose.header.frame_id = str[start:end] 03438 _x = self 03439 start = end 03440 end += 68 03441 (_x.goal.motion_plan_request.workspace_parameters.workspace_region_pose.pose.position.x, _x.goal.motion_plan_request.workspace_parameters.workspace_region_pose.pose.position.y, _x.goal.motion_plan_request.workspace_parameters.workspace_region_pose.pose.position.z, _x.goal.motion_plan_request.workspace_parameters.workspace_region_pose.pose.orientation.x, _x.goal.motion_plan_request.workspace_parameters.workspace_region_pose.pose.orientation.y, _x.goal.motion_plan_request.workspace_parameters.workspace_region_pose.pose.orientation.z, _x.goal.motion_plan_request.workspace_parameters.workspace_region_pose.pose.orientation.w, _x.goal.motion_plan_request.start_state.joint_state.header.seq, _x.goal.motion_plan_request.start_state.joint_state.header.stamp.secs, _x.goal.motion_plan_request.start_state.joint_state.header.stamp.nsecs,) = _struct_7d3I.unpack(str[start:end]) 03442 start = end 03443 end += 4 03444 (length,) = _struct_I.unpack(str[start:end]) 03445 start = end 03446 end += length 03447 self.goal.motion_plan_request.start_state.joint_state.header.frame_id = str[start:end] 03448 start = end 03449 end += 4 03450 (length,) = _struct_I.unpack(str[start:end]) 03451 self.goal.motion_plan_request.start_state.joint_state.name = [] 03452 for i in range(0, length): 03453 start = end 03454 end += 4 03455 (length,) = _struct_I.unpack(str[start:end]) 03456 start = end 03457 end += length 03458 val1 = str[start:end] 03459 self.goal.motion_plan_request.start_state.joint_state.name.append(val1) 03460 start = end 03461 end += 4 03462 (length,) = _struct_I.unpack(str[start:end]) 03463 pattern = '<%sd'%length 03464 start = end 03465 end += struct.calcsize(pattern) 03466 self.goal.motion_plan_request.start_state.joint_state.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 03467 start = end 03468 end += 4 03469 (length,) = _struct_I.unpack(str[start:end]) 03470 pattern = '<%sd'%length 03471 start = end 03472 end += struct.calcsize(pattern) 03473 self.goal.motion_plan_request.start_state.joint_state.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 03474 start = end 03475 end += 4 03476 (length,) = _struct_I.unpack(str[start:end]) 03477 pattern = '<%sd'%length 03478 start = end 03479 end += struct.calcsize(pattern) 03480 self.goal.motion_plan_request.start_state.joint_state.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 03481 _x = self 03482 start = end 03483 end += 8 03484 (_x.goal.motion_plan_request.start_state.multi_dof_joint_state.stamp.secs, _x.goal.motion_plan_request.start_state.multi_dof_joint_state.stamp.nsecs,) = _struct_2I.unpack(str[start:end]) 03485 start = end 03486 end += 4 03487 (length,) = _struct_I.unpack(str[start:end]) 03488 self.goal.motion_plan_request.start_state.multi_dof_joint_state.joint_names = [] 03489 for i in range(0, length): 03490 start = end 03491 end += 4 03492 (length,) = _struct_I.unpack(str[start:end]) 03493 start = end 03494 end += length 03495 val1 = str[start:end] 03496 self.goal.motion_plan_request.start_state.multi_dof_joint_state.joint_names.append(val1) 03497 start = end 03498 end += 4 03499 (length,) = _struct_I.unpack(str[start:end]) 03500 self.goal.motion_plan_request.start_state.multi_dof_joint_state.frame_ids = [] 03501 for i in range(0, length): 03502 start = end 03503 end += 4 03504 (length,) = _struct_I.unpack(str[start:end]) 03505 start = end 03506 end += length 03507 val1 = str[start:end] 03508 self.goal.motion_plan_request.start_state.multi_dof_joint_state.frame_ids.append(val1) 03509 start = end 03510 end += 4 03511 (length,) = _struct_I.unpack(str[start:end]) 03512 self.goal.motion_plan_request.start_state.multi_dof_joint_state.child_frame_ids = [] 03513 for i in range(0, length): 03514 start = end 03515 end += 4 03516 (length,) = _struct_I.unpack(str[start:end]) 03517 start = end 03518 end += length 03519 val1 = str[start:end] 03520 self.goal.motion_plan_request.start_state.multi_dof_joint_state.child_frame_ids.append(val1) 03521 start = end 03522 end += 4 03523 (length,) = _struct_I.unpack(str[start:end]) 03524 self.goal.motion_plan_request.start_state.multi_dof_joint_state.poses = [] 03525 for i in range(0, length): 03526 val1 = geometry_msgs.msg.Pose() 03527 _v245 = val1.position 03528 _x = _v245 03529 start = end 03530 end += 24 03531 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 03532 _v246 = val1.orientation 03533 _x = _v246 03534 start = end 03535 end += 32 03536 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 03537 self.goal.motion_plan_request.start_state.multi_dof_joint_state.poses.append(val1) 03538 start = end 03539 end += 4 03540 (length,) = _struct_I.unpack(str[start:end]) 03541 self.goal.motion_plan_request.goal_constraints.joint_constraints = [] 03542 for i in range(0, length): 03543 val1 = arm_navigation_msgs.msg.JointConstraint() 03544 start = end 03545 end += 4 03546 (length,) = _struct_I.unpack(str[start:end]) 03547 start = end 03548 end += length 03549 val1.joint_name = str[start:end] 03550 _x = val1 03551 start = end 03552 end += 32 03553 (_x.position, _x.tolerance_above, _x.tolerance_below, _x.weight,) = _struct_4d.unpack(str[start:end]) 03554 self.goal.motion_plan_request.goal_constraints.joint_constraints.append(val1) 03555 start = end 03556 end += 4 03557 (length,) = _struct_I.unpack(str[start:end]) 03558 self.goal.motion_plan_request.goal_constraints.position_constraints = [] 03559 for i in range(0, length): 03560 val1 = arm_navigation_msgs.msg.PositionConstraint() 03561 _v247 = val1.header 03562 start = end 03563 end += 4 03564 (_v247.seq,) = _struct_I.unpack(str[start:end]) 03565 _v248 = _v247.stamp 03566 _x = _v248 03567 start = end 03568 end += 8 03569 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 03570 start = end 03571 end += 4 03572 (length,) = _struct_I.unpack(str[start:end]) 03573 start = end 03574 end += length 03575 _v247.frame_id = str[start:end] 03576 start = end 03577 end += 4 03578 (length,) = _struct_I.unpack(str[start:end]) 03579 start = end 03580 end += length 03581 val1.link_name = str[start:end] 03582 _v249 = val1.target_point_offset 03583 _x = _v249 03584 start = end 03585 end += 24 03586 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 03587 _v250 = val1.position 03588 _x = _v250 03589 start = end 03590 end += 24 03591 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 03592 _v251 = val1.constraint_region_shape 03593 start = end 03594 end += 1 03595 (_v251.type,) = _struct_b.unpack(str[start:end]) 03596 start = end 03597 end += 4 03598 (length,) = _struct_I.unpack(str[start:end]) 03599 pattern = '<%sd'%length 03600 start = end 03601 end += struct.calcsize(pattern) 03602 _v251.dimensions = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 03603 start = end 03604 end += 4 03605 (length,) = _struct_I.unpack(str[start:end]) 03606 pattern = '<%si'%length 03607 start = end 03608 end += struct.calcsize(pattern) 03609 _v251.triangles = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length) 03610 start = end 03611 end += 4 03612 (length,) = _struct_I.unpack(str[start:end]) 03613 _v251.vertices = [] 03614 for i in range(0, length): 03615 val3 = geometry_msgs.msg.Point() 03616 _x = val3 03617 start = end 03618 end += 24 03619 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 03620 _v251.vertices.append(val3) 03621 _v252 = val1.constraint_region_orientation 03622 _x = _v252 03623 start = end 03624 end += 32 03625 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 03626 start = end 03627 end += 8 03628 (val1.weight,) = _struct_d.unpack(str[start:end]) 03629 self.goal.motion_plan_request.goal_constraints.position_constraints.append(val1) 03630 start = end 03631 end += 4 03632 (length,) = _struct_I.unpack(str[start:end]) 03633 self.goal.motion_plan_request.goal_constraints.orientation_constraints = [] 03634 for i in range(0, length): 03635 val1 = arm_navigation_msgs.msg.OrientationConstraint() 03636 _v253 = val1.header 03637 start = end 03638 end += 4 03639 (_v253.seq,) = _struct_I.unpack(str[start:end]) 03640 _v254 = _v253.stamp 03641 _x = _v254 03642 start = end 03643 end += 8 03644 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 03645 start = end 03646 end += 4 03647 (length,) = _struct_I.unpack(str[start:end]) 03648 start = end 03649 end += length 03650 _v253.frame_id = str[start:end] 03651 start = end 03652 end += 4 03653 (length,) = _struct_I.unpack(str[start:end]) 03654 start = end 03655 end += length 03656 val1.link_name = str[start:end] 03657 start = end 03658 end += 4 03659 (val1.type,) = _struct_i.unpack(str[start:end]) 03660 _v255 = val1.orientation 03661 _x = _v255 03662 start = end 03663 end += 32 03664 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 03665 _x = val1 03666 start = end 03667 end += 32 03668 (_x.absolute_roll_tolerance, _x.absolute_pitch_tolerance, _x.absolute_yaw_tolerance, _x.weight,) = _struct_4d.unpack(str[start:end]) 03669 self.goal.motion_plan_request.goal_constraints.orientation_constraints.append(val1) 03670 start = end 03671 end += 4 03672 (length,) = _struct_I.unpack(str[start:end]) 03673 self.goal.motion_plan_request.goal_constraints.visibility_constraints = [] 03674 for i in range(0, length): 03675 val1 = arm_navigation_msgs.msg.VisibilityConstraint() 03676 _v256 = val1.header 03677 start = end 03678 end += 4 03679 (_v256.seq,) = _struct_I.unpack(str[start:end]) 03680 _v257 = _v256.stamp 03681 _x = _v257 03682 start = end 03683 end += 8 03684 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 03685 start = end 03686 end += 4 03687 (length,) = _struct_I.unpack(str[start:end]) 03688 start = end 03689 end += length 03690 _v256.frame_id = str[start:end] 03691 _v258 = val1.target 03692 _v259 = _v258.header 03693 start = end 03694 end += 4 03695 (_v259.seq,) = _struct_I.unpack(str[start:end]) 03696 _v260 = _v259.stamp 03697 _x = _v260 03698 start = end 03699 end += 8 03700 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 03701 start = end 03702 end += 4 03703 (length,) = _struct_I.unpack(str[start:end]) 03704 start = end 03705 end += length 03706 _v259.frame_id = str[start:end] 03707 _v261 = _v258.point 03708 _x = _v261 03709 start = end 03710 end += 24 03711 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 03712 _v262 = val1.sensor_pose 03713 _v263 = _v262.header 03714 start = end 03715 end += 4 03716 (_v263.seq,) = _struct_I.unpack(str[start:end]) 03717 _v264 = _v263.stamp 03718 _x = _v264 03719 start = end 03720 end += 8 03721 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 03722 start = end 03723 end += 4 03724 (length,) = _struct_I.unpack(str[start:end]) 03725 start = end 03726 end += length 03727 _v263.frame_id = str[start:end] 03728 _v265 = _v262.pose 03729 _v266 = _v265.position 03730 _x = _v266 03731 start = end 03732 end += 24 03733 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 03734 _v267 = _v265.orientation 03735 _x = _v267 03736 start = end 03737 end += 32 03738 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 03739 start = end 03740 end += 8 03741 (val1.absolute_tolerance,) = _struct_d.unpack(str[start:end]) 03742 self.goal.motion_plan_request.goal_constraints.visibility_constraints.append(val1) 03743 start = end 03744 end += 4 03745 (length,) = _struct_I.unpack(str[start:end]) 03746 self.goal.motion_plan_request.path_constraints.joint_constraints = [] 03747 for i in range(0, length): 03748 val1 = arm_navigation_msgs.msg.JointConstraint() 03749 start = end 03750 end += 4 03751 (length,) = _struct_I.unpack(str[start:end]) 03752 start = end 03753 end += length 03754 val1.joint_name = str[start:end] 03755 _x = val1 03756 start = end 03757 end += 32 03758 (_x.position, _x.tolerance_above, _x.tolerance_below, _x.weight,) = _struct_4d.unpack(str[start:end]) 03759 self.goal.motion_plan_request.path_constraints.joint_constraints.append(val1) 03760 start = end 03761 end += 4 03762 (length,) = _struct_I.unpack(str[start:end]) 03763 self.goal.motion_plan_request.path_constraints.position_constraints = [] 03764 for i in range(0, length): 03765 val1 = arm_navigation_msgs.msg.PositionConstraint() 03766 _v268 = val1.header 03767 start = end 03768 end += 4 03769 (_v268.seq,) = _struct_I.unpack(str[start:end]) 03770 _v269 = _v268.stamp 03771 _x = _v269 03772 start = end 03773 end += 8 03774 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 03775 start = end 03776 end += 4 03777 (length,) = _struct_I.unpack(str[start:end]) 03778 start = end 03779 end += length 03780 _v268.frame_id = str[start:end] 03781 start = end 03782 end += 4 03783 (length,) = _struct_I.unpack(str[start:end]) 03784 start = end 03785 end += length 03786 val1.link_name = str[start:end] 03787 _v270 = val1.target_point_offset 03788 _x = _v270 03789 start = end 03790 end += 24 03791 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 03792 _v271 = val1.position 03793 _x = _v271 03794 start = end 03795 end += 24 03796 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 03797 _v272 = val1.constraint_region_shape 03798 start = end 03799 end += 1 03800 (_v272.type,) = _struct_b.unpack(str[start:end]) 03801 start = end 03802 end += 4 03803 (length,) = _struct_I.unpack(str[start:end]) 03804 pattern = '<%sd'%length 03805 start = end 03806 end += struct.calcsize(pattern) 03807 _v272.dimensions = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 03808 start = end 03809 end += 4 03810 (length,) = _struct_I.unpack(str[start:end]) 03811 pattern = '<%si'%length 03812 start = end 03813 end += struct.calcsize(pattern) 03814 _v272.triangles = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length) 03815 start = end 03816 end += 4 03817 (length,) = _struct_I.unpack(str[start:end]) 03818 _v272.vertices = [] 03819 for i in range(0, length): 03820 val3 = geometry_msgs.msg.Point() 03821 _x = val3 03822 start = end 03823 end += 24 03824 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 03825 _v272.vertices.append(val3) 03826 _v273 = val1.constraint_region_orientation 03827 _x = _v273 03828 start = end 03829 end += 32 03830 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 03831 start = end 03832 end += 8 03833 (val1.weight,) = _struct_d.unpack(str[start:end]) 03834 self.goal.motion_plan_request.path_constraints.position_constraints.append(val1) 03835 start = end 03836 end += 4 03837 (length,) = _struct_I.unpack(str[start:end]) 03838 self.goal.motion_plan_request.path_constraints.orientation_constraints = [] 03839 for i in range(0, length): 03840 val1 = arm_navigation_msgs.msg.OrientationConstraint() 03841 _v274 = val1.header 03842 start = end 03843 end += 4 03844 (_v274.seq,) = _struct_I.unpack(str[start:end]) 03845 _v275 = _v274.stamp 03846 _x = _v275 03847 start = end 03848 end += 8 03849 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 03850 start = end 03851 end += 4 03852 (length,) = _struct_I.unpack(str[start:end]) 03853 start = end 03854 end += length 03855 _v274.frame_id = str[start:end] 03856 start = end 03857 end += 4 03858 (length,) = _struct_I.unpack(str[start:end]) 03859 start = end 03860 end += length 03861 val1.link_name = str[start:end] 03862 start = end 03863 end += 4 03864 (val1.type,) = _struct_i.unpack(str[start:end]) 03865 _v276 = val1.orientation 03866 _x = _v276 03867 start = end 03868 end += 32 03869 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 03870 _x = val1 03871 start = end 03872 end += 32 03873 (_x.absolute_roll_tolerance, _x.absolute_pitch_tolerance, _x.absolute_yaw_tolerance, _x.weight,) = _struct_4d.unpack(str[start:end]) 03874 self.goal.motion_plan_request.path_constraints.orientation_constraints.append(val1) 03875 start = end 03876 end += 4 03877 (length,) = _struct_I.unpack(str[start:end]) 03878 self.goal.motion_plan_request.path_constraints.visibility_constraints = [] 03879 for i in range(0, length): 03880 val1 = arm_navigation_msgs.msg.VisibilityConstraint() 03881 _v277 = val1.header 03882 start = end 03883 end += 4 03884 (_v277.seq,) = _struct_I.unpack(str[start:end]) 03885 _v278 = _v277.stamp 03886 _x = _v278 03887 start = end 03888 end += 8 03889 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 03890 start = end 03891 end += 4 03892 (length,) = _struct_I.unpack(str[start:end]) 03893 start = end 03894 end += length 03895 _v277.frame_id = str[start:end] 03896 _v279 = val1.target 03897 _v280 = _v279.header 03898 start = end 03899 end += 4 03900 (_v280.seq,) = _struct_I.unpack(str[start:end]) 03901 _v281 = _v280.stamp 03902 _x = _v281 03903 start = end 03904 end += 8 03905 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 03906 start = end 03907 end += 4 03908 (length,) = _struct_I.unpack(str[start:end]) 03909 start = end 03910 end += length 03911 _v280.frame_id = str[start:end] 03912 _v282 = _v279.point 03913 _x = _v282 03914 start = end 03915 end += 24 03916 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 03917 _v283 = val1.sensor_pose 03918 _v284 = _v283.header 03919 start = end 03920 end += 4 03921 (_v284.seq,) = _struct_I.unpack(str[start:end]) 03922 _v285 = _v284.stamp 03923 _x = _v285 03924 start = end 03925 end += 8 03926 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 03927 start = end 03928 end += 4 03929 (length,) = _struct_I.unpack(str[start:end]) 03930 start = end 03931 end += length 03932 _v284.frame_id = str[start:end] 03933 _v286 = _v283.pose 03934 _v287 = _v286.position 03935 _x = _v287 03936 start = end 03937 end += 24 03938 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 03939 _v288 = _v286.orientation 03940 _x = _v288 03941 start = end 03942 end += 32 03943 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 03944 start = end 03945 end += 8 03946 (val1.absolute_tolerance,) = _struct_d.unpack(str[start:end]) 03947 self.goal.motion_plan_request.path_constraints.visibility_constraints.append(val1) 03948 start = end 03949 end += 4 03950 (length,) = _struct_I.unpack(str[start:end]) 03951 start = end 03952 end += length 03953 self.goal.motion_plan_request.planner_id = str[start:end] 03954 start = end 03955 end += 4 03956 (length,) = _struct_I.unpack(str[start:end]) 03957 start = end 03958 end += length 03959 self.goal.motion_plan_request.group_name = str[start:end] 03960 _x = self 03961 start = end 03962 end += 28 03963 (_x.goal.motion_plan_request.num_planning_attempts, _x.goal.motion_plan_request.allowed_planning_time.secs, _x.goal.motion_plan_request.allowed_planning_time.nsecs, _x.goal.motion_plan_request.expected_path_duration.secs, _x.goal.motion_plan_request.expected_path_duration.nsecs, _x.goal.motion_plan_request.expected_path_dt.secs, _x.goal.motion_plan_request.expected_path_dt.nsecs,) = _struct_7i.unpack(str[start:end]) 03964 start = end 03965 end += 4 03966 (length,) = _struct_I.unpack(str[start:end]) 03967 self.goal.operations.collision_operations = [] 03968 for i in range(0, length): 03969 val1 = arm_navigation_msgs.msg.CollisionOperation() 03970 start = end 03971 end += 4 03972 (length,) = _struct_I.unpack(str[start:end]) 03973 start = end 03974 end += length 03975 val1.object1 = str[start:end] 03976 start = end 03977 end += 4 03978 (length,) = _struct_I.unpack(str[start:end]) 03979 start = end 03980 end += length 03981 val1.object2 = str[start:end] 03982 _x = val1 03983 start = end 03984 end += 12 03985 (_x.penetration_distance, _x.operation,) = _struct_di.unpack(str[start:end]) 03986 self.goal.operations.collision_operations.append(val1) 03987 _x = self 03988 start = end 03989 end += 4 03990 (_x.goal.accept_partial_plans, _x.goal.accept_invalid_goals, _x.goal.disable_ik, _x.goal.disable_collision_monitoring,) = _struct_4B.unpack(str[start:end]) 03991 self.goal.accept_partial_plans = bool(self.goal.accept_partial_plans) 03992 self.goal.accept_invalid_goals = bool(self.goal.accept_invalid_goals) 03993 self.goal.disable_ik = bool(self.goal.disable_ik) 03994 self.goal.disable_collision_monitoring = bool(self.goal.disable_collision_monitoring) 03995 return self 03996 except struct.error as e: 03997 raise roslib.message.DeserializationError(e) #most likely buffer underfill 03998 03999 _struct_I = roslib.message.struct_I 04000 _struct_7d3I = struct.Struct("<7d3I") 04001 _struct_b = struct.Struct("<b") 04002 _struct_d = struct.Struct("<d") 04003 _struct_f = struct.Struct("<f") 04004 _struct_di = struct.Struct("<di") 04005 _struct_7i = struct.Struct("<7i") 04006 _struct_3f = struct.Struct("<3f") 04007 _struct_i = struct.Struct("<i") 04008 _struct_3I = struct.Struct("<3I") 04009 _struct_4d = struct.Struct("<4d") 04010 _struct_4B = struct.Struct("<4B") 04011 _struct_2I = struct.Struct("<2I") 04012 _struct_3d = struct.Struct("<3d")