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