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
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
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)
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)
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")