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