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