00001 """autogenerated by genmsg_py from ReactiveLiftAction.msg. Do not edit."""
00002 import roslib.message
00003 import struct
00004
00005 import trajectory_msgs.msg
00006 import object_manipulation_msgs.msg
00007 import roslib.rostime
00008 import actionlib_msgs.msg
00009 import geometry_msgs.msg
00010 import sensor_msgs.msg
00011 import std_msgs.msg
00012 import household_objects_database_msgs.msg
00013
00014 class ReactiveLiftAction(roslib.message.Message):
00015 _md5sum = "7c440c96e5e93f84d4ad14066aa4160b"
00016 _type = "object_manipulation_msgs/ReactiveLiftAction"
00017 _has_header = False
00018 _full_text = """# ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00019
00020 ReactiveLiftActionGoal action_goal
00021 ReactiveLiftActionResult action_result
00022 ReactiveLiftActionFeedback action_feedback
00023
00024 ================================================================================
00025 MSG: object_manipulation_msgs/ReactiveLiftActionGoal
00026 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00027
00028 Header header
00029 actionlib_msgs/GoalID goal_id
00030 ReactiveLiftGoal goal
00031
00032 ================================================================================
00033 MSG: std_msgs/Header
00034 # Standard metadata for higher-level stamped data types.
00035 # This is generally used to communicate timestamped data
00036 # in a particular coordinate frame.
00037 #
00038 # sequence ID: consecutively increasing ID
00039 uint32 seq
00040 #Two-integer timestamp that is expressed as:
00041 # * stamp.secs: seconds (stamp_secs) since epoch
00042 # * stamp.nsecs: nanoseconds since stamp_secs
00043 # time-handling sugar is provided by the client library
00044 time stamp
00045 #Frame this data is associated with
00046 # 0: no frame
00047 # 1: global frame
00048 string frame_id
00049
00050 ================================================================================
00051 MSG: actionlib_msgs/GoalID
00052 # The stamp should store the time at which this goal was requested.
00053 # It is used by an action server when it tries to preempt all
00054 # goals that were requested before a certain time
00055 time stamp
00056
00057 # The id provides a way to associate feedback and
00058 # result message with specific goal requests. The id
00059 # specified must be unique.
00060 string id
00061
00062
00063 ================================================================================
00064 MSG: object_manipulation_msgs/ReactiveLiftGoal
00065 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00066
00067 # the name of the arm being used
00068 string arm_name
00069
00070 # the object to be grasped
00071 GraspableObject target
00072
00073 # How the object should be lifted
00074 GripperTranslation lift
00075
00076 # the joint trajectory to use for the approach (if available)
00077 # this trajectory is expected to start at the current pose of the gripper
00078 # and end at the desired grasp pose
00079 trajectory_msgs/JointTrajectory trajectory
00080
00081 # the name of the support surface in the collision environment, if any
00082 string collision_support_surface_name
00083
00084
00085 ================================================================================
00086 MSG: object_manipulation_msgs/GraspableObject
00087 # an object that the object_manipulator can work on
00088
00089 # a graspable object can be represented in multiple ways. This message
00090 # can contain all of them. Which one is actually used is up to the receiver
00091 # of this message. When adding new representations, one must be careful that
00092 # they have reasonable lightweight defaults indicating that that particular
00093 # representation is not available.
00094
00095 # the tf frame to be used as a reference frame when combining information from
00096 # the different representations below
00097 string reference_frame_id
00098
00099 # potential recognition results from a database of models
00100 # all poses are relative to the object reference pose
00101 household_objects_database_msgs/DatabaseModelPose[] potential_models
00102
00103 # the point cloud itself
00104 sensor_msgs/PointCloud cluster
00105
00106 # a region of a PointCloud2 of interest
00107 object_manipulation_msgs/SceneRegion region
00108
00109
00110 ================================================================================
00111 MSG: household_objects_database_msgs/DatabaseModelPose
00112 # Informs that a specific model from the Model Database has been
00113 # identified at a certain location
00114
00115 # the database id of the model
00116 int32 model_id
00117
00118 # the pose that it can be found in
00119 geometry_msgs/PoseStamped pose
00120
00121 # a measure of the confidence level in this detection result
00122 float32 confidence
00123 ================================================================================
00124 MSG: geometry_msgs/PoseStamped
00125 # A Pose with reference coordinate frame and timestamp
00126 Header header
00127 Pose pose
00128
00129 ================================================================================
00130 MSG: geometry_msgs/Pose
00131 # A representation of pose in free space, composed of postion and orientation.
00132 Point position
00133 Quaternion orientation
00134
00135 ================================================================================
00136 MSG: geometry_msgs/Point
00137 # This contains the position of a point in free space
00138 float64 x
00139 float64 y
00140 float64 z
00141
00142 ================================================================================
00143 MSG: geometry_msgs/Quaternion
00144 # This represents an orientation in free space in quaternion form.
00145
00146 float64 x
00147 float64 y
00148 float64 z
00149 float64 w
00150
00151 ================================================================================
00152 MSG: sensor_msgs/PointCloud
00153 # This message holds a collection of 3d points, plus optional additional
00154 # information about each point.
00155
00156 # Time of sensor data acquisition, coordinate frame ID.
00157 Header header
00158
00159 # Array of 3d points. Each Point32 should be interpreted as a 3d point
00160 # in the frame given in the header.
00161 geometry_msgs/Point32[] points
00162
00163 # Each channel should have the same number of elements as points array,
00164 # and the data in each channel should correspond 1:1 with each point.
00165 # Channel names in common practice are listed in ChannelFloat32.msg.
00166 ChannelFloat32[] channels
00167
00168 ================================================================================
00169 MSG: geometry_msgs/Point32
00170 # This contains the position of a point in free space(with 32 bits of precision).
00171 # It is recommeded to use Point wherever possible instead of Point32.
00172 #
00173 # This recommendation is to promote interoperability.
00174 #
00175 # This message is designed to take up less space when sending
00176 # lots of points at once, as in the case of a PointCloud.
00177
00178 float32 x
00179 float32 y
00180 float32 z
00181 ================================================================================
00182 MSG: sensor_msgs/ChannelFloat32
00183 # This message is used by the PointCloud message to hold optional data
00184 # associated with each point in the cloud. The length of the values
00185 # array should be the same as the length of the points array in the
00186 # PointCloud, and each value should be associated with the corresponding
00187 # point.
00188
00189 # Channel names in existing practice include:
00190 # "u", "v" - row and column (respectively) in the left stereo image.
00191 # This is opposite to usual conventions but remains for
00192 # historical reasons. The newer PointCloud2 message has no
00193 # such problem.
00194 # "rgb" - For point clouds produced by color stereo cameras. uint8
00195 # (R,G,B) values packed into the least significant 24 bits,
00196 # in order.
00197 # "intensity" - laser or pixel intensity.
00198 # "distance"
00199
00200 # The channel name should give semantics of the channel (e.g.
00201 # "intensity" instead of "value").
00202 string name
00203
00204 # The values array should be 1-1 with the elements of the associated
00205 # PointCloud.
00206 float32[] values
00207
00208 ================================================================================
00209 MSG: object_manipulation_msgs/SceneRegion
00210 # Point cloud
00211 sensor_msgs/PointCloud2 cloud
00212
00213 # Indices for the region of interest
00214 int32[] mask
00215
00216 # One of the corresponding 2D images, if applicable
00217 sensor_msgs/Image image
00218
00219 # The disparity image, if applicable
00220 sensor_msgs/Image disparity_image
00221
00222 # Camera info for the camera that took the image
00223 sensor_msgs/CameraInfo cam_info
00224
00225 ================================================================================
00226 MSG: sensor_msgs/PointCloud2
00227 # This message holds a collection of N-dimensional points, which may
00228 # contain additional information such as normals, intensity, etc. The
00229 # point data is stored as a binary blob, its layout described by the
00230 # contents of the "fields" array.
00231
00232 # The point cloud data may be organized 2d (image-like) or 1d
00233 # (unordered). Point clouds organized as 2d images may be produced by
00234 # camera depth sensors such as stereo or time-of-flight.
00235
00236 # Time of sensor data acquisition, and the coordinate frame ID (for 3d
00237 # points).
00238 Header header
00239
00240 # 2D structure of the point cloud. If the cloud is unordered, height is
00241 # 1 and width is the length of the point cloud.
00242 uint32 height
00243 uint32 width
00244
00245 # Describes the channels and their layout in the binary data blob.
00246 PointField[] fields
00247
00248 bool is_bigendian # Is this data bigendian?
00249 uint32 point_step # Length of a point in bytes
00250 uint32 row_step # Length of a row in bytes
00251 uint8[] data # Actual point data, size is (row_step*height)
00252
00253 bool is_dense # True if there are no invalid points
00254
00255 ================================================================================
00256 MSG: sensor_msgs/PointField
00257 # This message holds the description of one point entry in the
00258 # PointCloud2 message format.
00259 uint8 INT8 = 1
00260 uint8 UINT8 = 2
00261 uint8 INT16 = 3
00262 uint8 UINT16 = 4
00263 uint8 INT32 = 5
00264 uint8 UINT32 = 6
00265 uint8 FLOAT32 = 7
00266 uint8 FLOAT64 = 8
00267
00268 string name # Name of field
00269 uint32 offset # Offset from start of point struct
00270 uint8 datatype # Datatype enumeration, see above
00271 uint32 count # How many elements in the field
00272
00273 ================================================================================
00274 MSG: sensor_msgs/Image
00275 # This message contains an uncompressed image
00276 # (0, 0) is at top-left corner of image
00277 #
00278
00279 Header header # Header timestamp should be acquisition time of image
00280 # Header frame_id should be optical frame of camera
00281 # origin of frame should be optical center of cameara
00282 # +x should point to the right in the image
00283 # +y should point down in the image
00284 # +z should point into to plane of the image
00285 # If the frame_id here and the frame_id of the CameraInfo
00286 # message associated with the image conflict
00287 # the behavior is undefined
00288
00289 uint32 height # image height, that is, number of rows
00290 uint32 width # image width, that is, number of columns
00291
00292 # The legal values for encoding are in file src/image_encodings.cpp
00293 # If you want to standardize a new string format, join
00294 # ros-users@lists.sourceforge.net and send an email proposing a new encoding.
00295
00296 string encoding # Encoding of pixels -- channel meaning, ordering, size
00297 # taken from the list of strings in src/image_encodings.cpp
00298
00299 uint8 is_bigendian # is this data bigendian?
00300 uint32 step # Full row length in bytes
00301 uint8[] data # actual matrix data, size is (step * rows)
00302
00303 ================================================================================
00304 MSG: sensor_msgs/CameraInfo
00305 # This message defines meta information for a camera. It should be in a
00306 # camera namespace on topic "camera_info" and accompanied by up to five
00307 # image topics named:
00308 #
00309 # image_raw - raw data from the camera driver, possibly Bayer encoded
00310 # image - monochrome, distorted
00311 # image_color - color, distorted
00312 # image_rect - monochrome, rectified
00313 # image_rect_color - color, rectified
00314 #
00315 # The image_pipeline contains packages (image_proc, stereo_image_proc)
00316 # for producing the four processed image topics from image_raw and
00317 # camera_info. The meaning of the camera parameters are described in
00318 # detail at http://www.ros.org/wiki/image_pipeline/CameraInfo.
00319 #
00320 # The image_geometry package provides a user-friendly interface to
00321 # common operations using this meta information. If you want to, e.g.,
00322 # project a 3d point into image coordinates, we strongly recommend
00323 # using image_geometry.
00324 #
00325 # If the camera is uncalibrated, the matrices D, K, R, P should be left
00326 # zeroed out. In particular, clients may assume that K[0] == 0.0
00327 # indicates an uncalibrated camera.
00328
00329 #######################################################################
00330 # Image acquisition info #
00331 #######################################################################
00332
00333 # Time of image acquisition, camera coordinate frame ID
00334 Header header # Header timestamp should be acquisition time of image
00335 # Header frame_id should be optical frame of camera
00336 # origin of frame should be optical center of camera
00337 # +x should point to the right in the image
00338 # +y should point down in the image
00339 # +z should point into the plane of the image
00340
00341
00342 #######################################################################
00343 # Calibration Parameters #
00344 #######################################################################
00345 # These are fixed during camera calibration. Their values will be the #
00346 # same in all messages until the camera is recalibrated. Note that #
00347 # self-calibrating systems may "recalibrate" frequently. #
00348 # #
00349 # The internal parameters can be used to warp a raw (distorted) image #
00350 # to: #
00351 # 1. An undistorted image (requires D and K) #
00352 # 2. A rectified image (requires D, K, R) #
00353 # The projection matrix P projects 3D points into the rectified image.#
00354 #######################################################################
00355
00356 # The image dimensions with which the camera was calibrated. Normally
00357 # this will be the full camera resolution in pixels.
00358 uint32 height
00359 uint32 width
00360
00361 # The distortion model used. Supported models are listed in
00362 # sensor_msgs/distortion_models.h. For most cameras, "plumb_bob" - a
00363 # simple model of radial and tangential distortion - is sufficent.
00364 string distortion_model
00365
00366 # The distortion parameters, size depending on the distortion model.
00367 # For "plumb_bob", the 5 parameters are: (k1, k2, t1, t2, k3).
00368 float64[] D
00369
00370 # Intrinsic camera matrix for the raw (distorted) images.
00371 # [fx 0 cx]
00372 # K = [ 0 fy cy]
00373 # [ 0 0 1]
00374 # Projects 3D points in the camera coordinate frame to 2D pixel
00375 # coordinates using the focal lengths (fx, fy) and principal point
00376 # (cx, cy).
00377 float64[9] K # 3x3 row-major matrix
00378
00379 # Rectification matrix (stereo cameras only)
00380 # A rotation matrix aligning the camera coordinate system to the ideal
00381 # stereo image plane so that epipolar lines in both stereo images are
00382 # parallel.
00383 float64[9] R # 3x3 row-major matrix
00384
00385 # Projection/camera matrix
00386 # [fx' 0 cx' Tx]
00387 # P = [ 0 fy' cy' Ty]
00388 # [ 0 0 1 0]
00389 # By convention, this matrix specifies the intrinsic (camera) matrix
00390 # of the processed (rectified) image. That is, the left 3x3 portion
00391 # is the normal camera intrinsic matrix for the rectified image.
00392 # It projects 3D points in the camera coordinate frame to 2D pixel
00393 # coordinates using the focal lengths (fx', fy') and principal point
00394 # (cx', cy') - these may differ from the values in K.
00395 # For monocular cameras, Tx = Ty = 0. Normally, monocular cameras will
00396 # also have R = the identity and P[1:3,1:3] = K.
00397 # For a stereo pair, the fourth column [Tx Ty 0]' is related to the
00398 # position of the optical center of the second camera in the first
00399 # camera's frame. We assume Tz = 0 so both cameras are in the same
00400 # stereo image plane. The first camera always has Tx = Ty = 0. For
00401 # the right (second) camera of a horizontal stereo pair, Ty = 0 and
00402 # Tx = -fx' * B, where B is the baseline between the cameras.
00403 # Given a 3D point [X Y Z]', the projection (x, y) of the point onto
00404 # the rectified image is given by:
00405 # [u v w]' = P * [X Y Z 1]'
00406 # x = u / w
00407 # y = v / w
00408 # This holds for both images of a stereo pair.
00409 float64[12] P # 3x4 row-major matrix
00410
00411
00412 #######################################################################
00413 # Operational Parameters #
00414 #######################################################################
00415 # These define the image region actually captured by the camera #
00416 # driver. Although they affect the geometry of the output image, they #
00417 # may be changed freely without recalibrating the camera. #
00418 #######################################################################
00419
00420 # Binning refers here to any camera setting which combines rectangular
00421 # neighborhoods of pixels into larger "super-pixels." It reduces the
00422 # resolution of the output image to
00423 # (width / binning_x) x (height / binning_y).
00424 # The default values binning_x = binning_y = 0 is considered the same
00425 # as binning_x = binning_y = 1 (no subsampling).
00426 uint32 binning_x
00427 uint32 binning_y
00428
00429 # Region of interest (subwindow of full camera resolution), given in
00430 # full resolution (unbinned) image coordinates. A particular ROI
00431 # always denotes the same window of pixels on the camera sensor,
00432 # regardless of binning settings.
00433 # The default setting of roi (all values 0) is considered the same as
00434 # full resolution (roi.width = width, roi.height = height).
00435 RegionOfInterest roi
00436
00437 ================================================================================
00438 MSG: sensor_msgs/RegionOfInterest
00439 # This message is used to specify a region of interest within an image.
00440 #
00441 # When used to specify the ROI setting of the camera when the image was
00442 # taken, the height and width fields should either match the height and
00443 # width fields for the associated image; or height = width = 0
00444 # indicates that the full resolution image was captured.
00445
00446 uint32 x_offset # Leftmost pixel of the ROI
00447 # (0 if the ROI includes the left edge of the image)
00448 uint32 y_offset # Topmost pixel of the ROI
00449 # (0 if the ROI includes the top edge of the image)
00450 uint32 height # Height of ROI
00451 uint32 width # Width of ROI
00452
00453 # True if a distinct rectified ROI should be calculated from the "raw"
00454 # ROI in this message. Typically this should be False if the full image
00455 # is captured (ROI not used), and True if a subwindow is captured (ROI
00456 # used).
00457 bool do_rectify
00458
00459 ================================================================================
00460 MSG: object_manipulation_msgs/GripperTranslation
00461 # defines a translation for the gripper, used in pickup or place tasks
00462 # for example for lifting an object off a table or approaching the table for placing
00463
00464 # the direction of the translation
00465 geometry_msgs/Vector3Stamped direction
00466
00467 # the desired translation distance
00468 float32 desired_distance
00469
00470 # the min distance that must be considered feasible before the
00471 # grasp is even attempted
00472 float32 min_distance
00473 ================================================================================
00474 MSG: geometry_msgs/Vector3Stamped
00475 # This represents a Vector3 with reference coordinate frame and timestamp
00476 Header header
00477 Vector3 vector
00478
00479 ================================================================================
00480 MSG: geometry_msgs/Vector3
00481 # This represents a vector in free space.
00482
00483 float64 x
00484 float64 y
00485 float64 z
00486 ================================================================================
00487 MSG: trajectory_msgs/JointTrajectory
00488 Header header
00489 string[] joint_names
00490 JointTrajectoryPoint[] points
00491 ================================================================================
00492 MSG: trajectory_msgs/JointTrajectoryPoint
00493 float64[] positions
00494 float64[] velocities
00495 float64[] accelerations
00496 duration time_from_start
00497 ================================================================================
00498 MSG: object_manipulation_msgs/ReactiveLiftActionResult
00499 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00500
00501 Header header
00502 actionlib_msgs/GoalStatus status
00503 ReactiveLiftResult result
00504
00505 ================================================================================
00506 MSG: actionlib_msgs/GoalStatus
00507 GoalID goal_id
00508 uint8 status
00509 uint8 PENDING = 0 # The goal has yet to be processed by the action server
00510 uint8 ACTIVE = 1 # The goal is currently being processed by the action server
00511 uint8 PREEMPTED = 2 # The goal received a cancel request after it started executing
00512 # and has since completed its execution (Terminal State)
00513 uint8 SUCCEEDED = 3 # The goal was achieved successfully by the action server (Terminal State)
00514 uint8 ABORTED = 4 # The goal was aborted during execution by the action server due
00515 # to some failure (Terminal State)
00516 uint8 REJECTED = 5 # The goal was rejected by the action server without being processed,
00517 # because the goal was unattainable or invalid (Terminal State)
00518 uint8 PREEMPTING = 6 # The goal received a cancel request after it started executing
00519 # and has not yet completed execution
00520 uint8 RECALLING = 7 # The goal received a cancel request before it started executing,
00521 # but the action server has not yet confirmed that the goal is canceled
00522 uint8 RECALLED = 8 # The goal received a cancel request before it started executing
00523 # and was successfully cancelled (Terminal State)
00524 uint8 LOST = 9 # An action client can determine that a goal is LOST. This should not be
00525 # sent over the wire by an action server
00526
00527 #Allow for the user to associate a string with GoalStatus for debugging
00528 string text
00529
00530
00531 ================================================================================
00532 MSG: object_manipulation_msgs/ReactiveLiftResult
00533 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00534
00535 # The result of the lift attempt
00536 ManipulationResult manipulation_result
00537
00538
00539 ================================================================================
00540 MSG: object_manipulation_msgs/ManipulationResult
00541 # Result codes for manipulation tasks
00542
00543 # task completed as expected
00544 # generally means you can proceed as planned
00545 int32 SUCCESS = 1
00546
00547 # task not possible (e.g. out of reach or obstacles in the way)
00548 # generally means that the world was not disturbed, so you can try another task
00549 int32 UNFEASIBLE = -1
00550
00551 # task was thought possible, but failed due to unexpected events during execution
00552 # it is likely that the world was disturbed, so you are encouraged to refresh
00553 # your sensed world model before proceeding to another task
00554 int32 FAILED = -2
00555
00556 # a lower level error prevented task completion (e.g. joint controller not responding)
00557 # generally requires human attention
00558 int32 ERROR = -3
00559
00560 # means that at some point during execution we ended up in a state that the collision-aware
00561 # arm navigation module will not move out of. The world was likely not disturbed, but you
00562 # probably need a new collision map to move the arm out of the stuck position
00563 int32 ARM_MOVEMENT_PREVENTED = -4
00564
00565 # specific to grasp actions
00566 # the object was grasped successfully, but the lift attempt could not achieve the minimum lift distance requested
00567 # it is likely that the collision environment will see collisions between the hand/object and the support surface
00568 int32 LIFT_FAILED = -5
00569
00570 # specific to place actions
00571 # the object was placed successfully, but the retreat attempt could not achieve the minimum retreat distance requested
00572 # it is likely that the collision environment will see collisions between the hand and the object
00573 int32 RETREAT_FAILED = -6
00574
00575 # indicates that somewhere along the line a human said "wait, stop, this is bad, go back and do something else"
00576 int32 CANCELLED = -7
00577
00578 # the actual value of this error code
00579 int32 value
00580
00581 ================================================================================
00582 MSG: object_manipulation_msgs/ReactiveLiftActionFeedback
00583 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00584
00585 Header header
00586 actionlib_msgs/GoalStatus status
00587 ReactiveLiftFeedback feedback
00588
00589 ================================================================================
00590 MSG: object_manipulation_msgs/ReactiveLiftFeedback
00591 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00592
00593 # Which phase the lift is in
00594
00595 ManipulationPhase manipulation_phase
00596
00597
00598
00599 ================================================================================
00600 MSG: object_manipulation_msgs/ManipulationPhase
00601 int32 CHECKING_FEASIBILITY = 0
00602 int32 MOVING_TO_PREGRASP = 1
00603 int32 MOVING_TO_GRASP = 2
00604 int32 CLOSING = 3
00605 int32 ADJUSTING_GRASP = 4
00606 int32 LIFTING = 5
00607 int32 MOVING_WITH_OBJECT = 6
00608 int32 MOVING_TO_PLACE = 7
00609 int32 PLACING = 8
00610 int32 OPENING = 9
00611 int32 RETREATING = 10
00612 int32 MOVING_WITHOUT_OBJECT = 11
00613 int32 SHAKING = 12
00614 int32 SUCCEEDED = 13
00615 int32 FAILED = 14
00616 int32 ABORTED = 15
00617 int32 HOLDING_OBJECT = 16
00618
00619 int32 phase
00620 """
00621 __slots__ = ['action_goal','action_result','action_feedback']
00622 _slot_types = ['object_manipulation_msgs/ReactiveLiftActionGoal','object_manipulation_msgs/ReactiveLiftActionResult','object_manipulation_msgs/ReactiveLiftActionFeedback']
00623
00624 def __init__(self, *args, **kwds):
00625 """
00626 Constructor. Any message fields that are implicitly/explicitly
00627 set to None will be assigned a default value. The recommend
00628 use is keyword arguments as this is more robust to future message
00629 changes. You cannot mix in-order arguments and keyword arguments.
00630
00631 The available fields are:
00632 action_goal,action_result,action_feedback
00633
00634 @param args: complete set of field values, in .msg order
00635 @param kwds: use keyword arguments corresponding to message field names
00636 to set specific fields.
00637 """
00638 if args or kwds:
00639 super(ReactiveLiftAction, self).__init__(*args, **kwds)
00640 #message fields cannot be None, assign default values for those that are
00641 if self.action_goal is None:
00642 self.action_goal = object_manipulation_msgs.msg.ReactiveLiftActionGoal()
00643 if self.action_result is None:
00644 self.action_result = object_manipulation_msgs.msg.ReactiveLiftActionResult()
00645 if self.action_feedback is None:
00646 self.action_feedback = object_manipulation_msgs.msg.ReactiveLiftActionFeedback()
00647 else:
00648 self.action_goal = object_manipulation_msgs.msg.ReactiveLiftActionGoal()
00649 self.action_result = object_manipulation_msgs.msg.ReactiveLiftActionResult()
00650 self.action_feedback = object_manipulation_msgs.msg.ReactiveLiftActionFeedback()
00651
00652 def _get_types(self):
00653 """
00654 internal API method
00655 """
00656 return self._slot_types
00657
00658 def serialize(self, buff):
00659 """
00660 serialize message into buffer
00661 @param buff: buffer
00662 @type buff: StringIO
00663 """
00664 try:
00665 _x = self
00666 buff.write(_struct_3I.pack(_x.action_goal.header.seq, _x.action_goal.header.stamp.secs, _x.action_goal.header.stamp.nsecs))
00667 _x = self.action_goal.header.frame_id
00668 length = len(_x)
00669 buff.write(struct.pack('<I%ss'%length, length, _x))
00670 _x = self
00671 buff.write(_struct_2I.pack(_x.action_goal.goal_id.stamp.secs, _x.action_goal.goal_id.stamp.nsecs))
00672 _x = self.action_goal.goal_id.id
00673 length = len(_x)
00674 buff.write(struct.pack('<I%ss'%length, length, _x))
00675 _x = self.action_goal.goal.arm_name
00676 length = len(_x)
00677 buff.write(struct.pack('<I%ss'%length, length, _x))
00678 _x = self.action_goal.goal.target.reference_frame_id
00679 length = len(_x)
00680 buff.write(struct.pack('<I%ss'%length, length, _x))
00681 length = len(self.action_goal.goal.target.potential_models)
00682 buff.write(_struct_I.pack(length))
00683 for val1 in self.action_goal.goal.target.potential_models:
00684 buff.write(_struct_i.pack(val1.model_id))
00685 _v1 = val1.pose
00686 _v2 = _v1.header
00687 buff.write(_struct_I.pack(_v2.seq))
00688 _v3 = _v2.stamp
00689 _x = _v3
00690 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00691 _x = _v2.frame_id
00692 length = len(_x)
00693 buff.write(struct.pack('<I%ss'%length, length, _x))
00694 _v4 = _v1.pose
00695 _v5 = _v4.position
00696 _x = _v5
00697 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00698 _v6 = _v4.orientation
00699 _x = _v6
00700 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00701 buff.write(_struct_f.pack(val1.confidence))
00702 _x = self
00703 buff.write(_struct_3I.pack(_x.action_goal.goal.target.cluster.header.seq, _x.action_goal.goal.target.cluster.header.stamp.secs, _x.action_goal.goal.target.cluster.header.stamp.nsecs))
00704 _x = self.action_goal.goal.target.cluster.header.frame_id
00705 length = len(_x)
00706 buff.write(struct.pack('<I%ss'%length, length, _x))
00707 length = len(self.action_goal.goal.target.cluster.points)
00708 buff.write(_struct_I.pack(length))
00709 for val1 in self.action_goal.goal.target.cluster.points:
00710 _x = val1
00711 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z))
00712 length = len(self.action_goal.goal.target.cluster.channels)
00713 buff.write(_struct_I.pack(length))
00714 for val1 in self.action_goal.goal.target.cluster.channels:
00715 _x = val1.name
00716 length = len(_x)
00717 buff.write(struct.pack('<I%ss'%length, length, _x))
00718 length = len(val1.values)
00719 buff.write(_struct_I.pack(length))
00720 pattern = '<%sf'%length
00721 buff.write(struct.pack(pattern, *val1.values))
00722 _x = self
00723 buff.write(_struct_3I.pack(_x.action_goal.goal.target.region.cloud.header.seq, _x.action_goal.goal.target.region.cloud.header.stamp.secs, _x.action_goal.goal.target.region.cloud.header.stamp.nsecs))
00724 _x = self.action_goal.goal.target.region.cloud.header.frame_id
00725 length = len(_x)
00726 buff.write(struct.pack('<I%ss'%length, length, _x))
00727 _x = self
00728 buff.write(_struct_2I.pack(_x.action_goal.goal.target.region.cloud.height, _x.action_goal.goal.target.region.cloud.width))
00729 length = len(self.action_goal.goal.target.region.cloud.fields)
00730 buff.write(_struct_I.pack(length))
00731 for val1 in self.action_goal.goal.target.region.cloud.fields:
00732 _x = val1.name
00733 length = len(_x)
00734 buff.write(struct.pack('<I%ss'%length, length, _x))
00735 _x = val1
00736 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count))
00737 _x = self
00738 buff.write(_struct_B2I.pack(_x.action_goal.goal.target.region.cloud.is_bigendian, _x.action_goal.goal.target.region.cloud.point_step, _x.action_goal.goal.target.region.cloud.row_step))
00739 _x = self.action_goal.goal.target.region.cloud.data
00740 length = len(_x)
00741 # - if encoded as a list instead, serialize as bytes instead of string
00742 if type(_x) in [list, tuple]:
00743 buff.write(struct.pack('<I%sB'%length, length, *_x))
00744 else:
00745 buff.write(struct.pack('<I%ss'%length, length, _x))
00746 buff.write(_struct_B.pack(self.action_goal.goal.target.region.cloud.is_dense))
00747 length = len(self.action_goal.goal.target.region.mask)
00748 buff.write(_struct_I.pack(length))
00749 pattern = '<%si'%length
00750 buff.write(struct.pack(pattern, *self.action_goal.goal.target.region.mask))
00751 _x = self
00752 buff.write(_struct_3I.pack(_x.action_goal.goal.target.region.image.header.seq, _x.action_goal.goal.target.region.image.header.stamp.secs, _x.action_goal.goal.target.region.image.header.stamp.nsecs))
00753 _x = self.action_goal.goal.target.region.image.header.frame_id
00754 length = len(_x)
00755 buff.write(struct.pack('<I%ss'%length, length, _x))
00756 _x = self
00757 buff.write(_struct_2I.pack(_x.action_goal.goal.target.region.image.height, _x.action_goal.goal.target.region.image.width))
00758 _x = self.action_goal.goal.target.region.image.encoding
00759 length = len(_x)
00760 buff.write(struct.pack('<I%ss'%length, length, _x))
00761 _x = self
00762 buff.write(_struct_BI.pack(_x.action_goal.goal.target.region.image.is_bigendian, _x.action_goal.goal.target.region.image.step))
00763 _x = self.action_goal.goal.target.region.image.data
00764 length = len(_x)
00765 # - if encoded as a list instead, serialize as bytes instead of string
00766 if type(_x) in [list, tuple]:
00767 buff.write(struct.pack('<I%sB'%length, length, *_x))
00768 else:
00769 buff.write(struct.pack('<I%ss'%length, length, _x))
00770 _x = self
00771 buff.write(_struct_3I.pack(_x.action_goal.goal.target.region.disparity_image.header.seq, _x.action_goal.goal.target.region.disparity_image.header.stamp.secs, _x.action_goal.goal.target.region.disparity_image.header.stamp.nsecs))
00772 _x = self.action_goal.goal.target.region.disparity_image.header.frame_id
00773 length = len(_x)
00774 buff.write(struct.pack('<I%ss'%length, length, _x))
00775 _x = self
00776 buff.write(_struct_2I.pack(_x.action_goal.goal.target.region.disparity_image.height, _x.action_goal.goal.target.region.disparity_image.width))
00777 _x = self.action_goal.goal.target.region.disparity_image.encoding
00778 length = len(_x)
00779 buff.write(struct.pack('<I%ss'%length, length, _x))
00780 _x = self
00781 buff.write(_struct_BI.pack(_x.action_goal.goal.target.region.disparity_image.is_bigendian, _x.action_goal.goal.target.region.disparity_image.step))
00782 _x = self.action_goal.goal.target.region.disparity_image.data
00783 length = len(_x)
00784 # - if encoded as a list instead, serialize as bytes instead of string
00785 if type(_x) in [list, tuple]:
00786 buff.write(struct.pack('<I%sB'%length, length, *_x))
00787 else:
00788 buff.write(struct.pack('<I%ss'%length, length, _x))
00789 _x = self
00790 buff.write(_struct_3I.pack(_x.action_goal.goal.target.region.cam_info.header.seq, _x.action_goal.goal.target.region.cam_info.header.stamp.secs, _x.action_goal.goal.target.region.cam_info.header.stamp.nsecs))
00791 _x = self.action_goal.goal.target.region.cam_info.header.frame_id
00792 length = len(_x)
00793 buff.write(struct.pack('<I%ss'%length, length, _x))
00794 _x = self
00795 buff.write(_struct_2I.pack(_x.action_goal.goal.target.region.cam_info.height, _x.action_goal.goal.target.region.cam_info.width))
00796 _x = self.action_goal.goal.target.region.cam_info.distortion_model
00797 length = len(_x)
00798 buff.write(struct.pack('<I%ss'%length, length, _x))
00799 length = len(self.action_goal.goal.target.region.cam_info.D)
00800 buff.write(_struct_I.pack(length))
00801 pattern = '<%sd'%length
00802 buff.write(struct.pack(pattern, *self.action_goal.goal.target.region.cam_info.D))
00803 buff.write(_struct_9d.pack(*self.action_goal.goal.target.region.cam_info.K))
00804 buff.write(_struct_9d.pack(*self.action_goal.goal.target.region.cam_info.R))
00805 buff.write(_struct_12d.pack(*self.action_goal.goal.target.region.cam_info.P))
00806 _x = self
00807 buff.write(_struct_6IB3I.pack(_x.action_goal.goal.target.region.cam_info.binning_x, _x.action_goal.goal.target.region.cam_info.binning_y, _x.action_goal.goal.target.region.cam_info.roi.x_offset, _x.action_goal.goal.target.region.cam_info.roi.y_offset, _x.action_goal.goal.target.region.cam_info.roi.height, _x.action_goal.goal.target.region.cam_info.roi.width, _x.action_goal.goal.target.region.cam_info.roi.do_rectify, _x.action_goal.goal.lift.direction.header.seq, _x.action_goal.goal.lift.direction.header.stamp.secs, _x.action_goal.goal.lift.direction.header.stamp.nsecs))
00808 _x = self.action_goal.goal.lift.direction.header.frame_id
00809 length = len(_x)
00810 buff.write(struct.pack('<I%ss'%length, length, _x))
00811 _x = self
00812 buff.write(_struct_3d2f3I.pack(_x.action_goal.goal.lift.direction.vector.x, _x.action_goal.goal.lift.direction.vector.y, _x.action_goal.goal.lift.direction.vector.z, _x.action_goal.goal.lift.desired_distance, _x.action_goal.goal.lift.min_distance, _x.action_goal.goal.trajectory.header.seq, _x.action_goal.goal.trajectory.header.stamp.secs, _x.action_goal.goal.trajectory.header.stamp.nsecs))
00813 _x = self.action_goal.goal.trajectory.header.frame_id
00814 length = len(_x)
00815 buff.write(struct.pack('<I%ss'%length, length, _x))
00816 length = len(self.action_goal.goal.trajectory.joint_names)
00817 buff.write(_struct_I.pack(length))
00818 for val1 in self.action_goal.goal.trajectory.joint_names:
00819 length = len(val1)
00820 buff.write(struct.pack('<I%ss'%length, length, val1))
00821 length = len(self.action_goal.goal.trajectory.points)
00822 buff.write(_struct_I.pack(length))
00823 for val1 in self.action_goal.goal.trajectory.points:
00824 length = len(val1.positions)
00825 buff.write(_struct_I.pack(length))
00826 pattern = '<%sd'%length
00827 buff.write(struct.pack(pattern, *val1.positions))
00828 length = len(val1.velocities)
00829 buff.write(_struct_I.pack(length))
00830 pattern = '<%sd'%length
00831 buff.write(struct.pack(pattern, *val1.velocities))
00832 length = len(val1.accelerations)
00833 buff.write(_struct_I.pack(length))
00834 pattern = '<%sd'%length
00835 buff.write(struct.pack(pattern, *val1.accelerations))
00836 _v7 = val1.time_from_start
00837 _x = _v7
00838 buff.write(_struct_2i.pack(_x.secs, _x.nsecs))
00839 _x = self.action_goal.goal.collision_support_surface_name
00840 length = len(_x)
00841 buff.write(struct.pack('<I%ss'%length, length, _x))
00842 _x = self
00843 buff.write(_struct_3I.pack(_x.action_result.header.seq, _x.action_result.header.stamp.secs, _x.action_result.header.stamp.nsecs))
00844 _x = self.action_result.header.frame_id
00845 length = len(_x)
00846 buff.write(struct.pack('<I%ss'%length, length, _x))
00847 _x = self
00848 buff.write(_struct_2I.pack(_x.action_result.status.goal_id.stamp.secs, _x.action_result.status.goal_id.stamp.nsecs))
00849 _x = self.action_result.status.goal_id.id
00850 length = len(_x)
00851 buff.write(struct.pack('<I%ss'%length, length, _x))
00852 buff.write(_struct_B.pack(self.action_result.status.status))
00853 _x = self.action_result.status.text
00854 length = len(_x)
00855 buff.write(struct.pack('<I%ss'%length, length, _x))
00856 _x = self
00857 buff.write(_struct_i3I.pack(_x.action_result.result.manipulation_result.value, _x.action_feedback.header.seq, _x.action_feedback.header.stamp.secs, _x.action_feedback.header.stamp.nsecs))
00858 _x = self.action_feedback.header.frame_id
00859 length = len(_x)
00860 buff.write(struct.pack('<I%ss'%length, length, _x))
00861 _x = self
00862 buff.write(_struct_2I.pack(_x.action_feedback.status.goal_id.stamp.secs, _x.action_feedback.status.goal_id.stamp.nsecs))
00863 _x = self.action_feedback.status.goal_id.id
00864 length = len(_x)
00865 buff.write(struct.pack('<I%ss'%length, length, _x))
00866 buff.write(_struct_B.pack(self.action_feedback.status.status))
00867 _x = self.action_feedback.status.text
00868 length = len(_x)
00869 buff.write(struct.pack('<I%ss'%length, length, _x))
00870 buff.write(_struct_i.pack(self.action_feedback.feedback.manipulation_phase.phase))
00871 except struct.error, se: self._check_types(se)
00872 except TypeError, te: self._check_types(te)
00873
00874 def deserialize(self, str):
00875 """
00876 unpack serialized message in str into this message instance
00877 @param str: byte array of serialized message
00878 @type str: str
00879 """
00880 try:
00881 if self.action_goal is None:
00882 self.action_goal = object_manipulation_msgs.msg.ReactiveLiftActionGoal()
00883 if self.action_result is None:
00884 self.action_result = object_manipulation_msgs.msg.ReactiveLiftActionResult()
00885 if self.action_feedback is None:
00886 self.action_feedback = object_manipulation_msgs.msg.ReactiveLiftActionFeedback()
00887 end = 0
00888 _x = self
00889 start = end
00890 end += 12
00891 (_x.action_goal.header.seq, _x.action_goal.header.stamp.secs, _x.action_goal.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00892 start = end
00893 end += 4
00894 (length,) = _struct_I.unpack(str[start:end])
00895 start = end
00896 end += length
00897 self.action_goal.header.frame_id = str[start:end]
00898 _x = self
00899 start = end
00900 end += 8
00901 (_x.action_goal.goal_id.stamp.secs, _x.action_goal.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
00902 start = end
00903 end += 4
00904 (length,) = _struct_I.unpack(str[start:end])
00905 start = end
00906 end += length
00907 self.action_goal.goal_id.id = str[start:end]
00908 start = end
00909 end += 4
00910 (length,) = _struct_I.unpack(str[start:end])
00911 start = end
00912 end += length
00913 self.action_goal.goal.arm_name = str[start:end]
00914 start = end
00915 end += 4
00916 (length,) = _struct_I.unpack(str[start:end])
00917 start = end
00918 end += length
00919 self.action_goal.goal.target.reference_frame_id = str[start:end]
00920 start = end
00921 end += 4
00922 (length,) = _struct_I.unpack(str[start:end])
00923 self.action_goal.goal.target.potential_models = []
00924 for i in xrange(0, length):
00925 val1 = household_objects_database_msgs.msg.DatabaseModelPose()
00926 start = end
00927 end += 4
00928 (val1.model_id,) = _struct_i.unpack(str[start:end])
00929 _v8 = val1.pose
00930 _v9 = _v8.header
00931 start = end
00932 end += 4
00933 (_v9.seq,) = _struct_I.unpack(str[start:end])
00934 _v10 = _v9.stamp
00935 _x = _v10
00936 start = end
00937 end += 8
00938 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00939 start = end
00940 end += 4
00941 (length,) = _struct_I.unpack(str[start:end])
00942 start = end
00943 end += length
00944 _v9.frame_id = str[start:end]
00945 _v11 = _v8.pose
00946 _v12 = _v11.position
00947 _x = _v12
00948 start = end
00949 end += 24
00950 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00951 _v13 = _v11.orientation
00952 _x = _v13
00953 start = end
00954 end += 32
00955 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
00956 start = end
00957 end += 4
00958 (val1.confidence,) = _struct_f.unpack(str[start:end])
00959 self.action_goal.goal.target.potential_models.append(val1)
00960 _x = self
00961 start = end
00962 end += 12
00963 (_x.action_goal.goal.target.cluster.header.seq, _x.action_goal.goal.target.cluster.header.stamp.secs, _x.action_goal.goal.target.cluster.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00964 start = end
00965 end += 4
00966 (length,) = _struct_I.unpack(str[start:end])
00967 start = end
00968 end += length
00969 self.action_goal.goal.target.cluster.header.frame_id = str[start:end]
00970 start = end
00971 end += 4
00972 (length,) = _struct_I.unpack(str[start:end])
00973 self.action_goal.goal.target.cluster.points = []
00974 for i in xrange(0, length):
00975 val1 = geometry_msgs.msg.Point32()
00976 _x = val1
00977 start = end
00978 end += 12
00979 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end])
00980 self.action_goal.goal.target.cluster.points.append(val1)
00981 start = end
00982 end += 4
00983 (length,) = _struct_I.unpack(str[start:end])
00984 self.action_goal.goal.target.cluster.channels = []
00985 for i in xrange(0, length):
00986 val1 = sensor_msgs.msg.ChannelFloat32()
00987 start = end
00988 end += 4
00989 (length,) = _struct_I.unpack(str[start:end])
00990 start = end
00991 end += length
00992 val1.name = str[start:end]
00993 start = end
00994 end += 4
00995 (length,) = _struct_I.unpack(str[start:end])
00996 pattern = '<%sf'%length
00997 start = end
00998 end += struct.calcsize(pattern)
00999 val1.values = struct.unpack(pattern, str[start:end])
01000 self.action_goal.goal.target.cluster.channels.append(val1)
01001 _x = self
01002 start = end
01003 end += 12
01004 (_x.action_goal.goal.target.region.cloud.header.seq, _x.action_goal.goal.target.region.cloud.header.stamp.secs, _x.action_goal.goal.target.region.cloud.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01005 start = end
01006 end += 4
01007 (length,) = _struct_I.unpack(str[start:end])
01008 start = end
01009 end += length
01010 self.action_goal.goal.target.region.cloud.header.frame_id = str[start:end]
01011 _x = self
01012 start = end
01013 end += 8
01014 (_x.action_goal.goal.target.region.cloud.height, _x.action_goal.goal.target.region.cloud.width,) = _struct_2I.unpack(str[start:end])
01015 start = end
01016 end += 4
01017 (length,) = _struct_I.unpack(str[start:end])
01018 self.action_goal.goal.target.region.cloud.fields = []
01019 for i in xrange(0, length):
01020 val1 = sensor_msgs.msg.PointField()
01021 start = end
01022 end += 4
01023 (length,) = _struct_I.unpack(str[start:end])
01024 start = end
01025 end += length
01026 val1.name = str[start:end]
01027 _x = val1
01028 start = end
01029 end += 9
01030 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end])
01031 self.action_goal.goal.target.region.cloud.fields.append(val1)
01032 _x = self
01033 start = end
01034 end += 9
01035 (_x.action_goal.goal.target.region.cloud.is_bigendian, _x.action_goal.goal.target.region.cloud.point_step, _x.action_goal.goal.target.region.cloud.row_step,) = _struct_B2I.unpack(str[start:end])
01036 self.action_goal.goal.target.region.cloud.is_bigendian = bool(self.action_goal.goal.target.region.cloud.is_bigendian)
01037 start = end
01038 end += 4
01039 (length,) = _struct_I.unpack(str[start:end])
01040 start = end
01041 end += length
01042 self.action_goal.goal.target.region.cloud.data = str[start:end]
01043 start = end
01044 end += 1
01045 (self.action_goal.goal.target.region.cloud.is_dense,) = _struct_B.unpack(str[start:end])
01046 self.action_goal.goal.target.region.cloud.is_dense = bool(self.action_goal.goal.target.region.cloud.is_dense)
01047 start = end
01048 end += 4
01049 (length,) = _struct_I.unpack(str[start:end])
01050 pattern = '<%si'%length
01051 start = end
01052 end += struct.calcsize(pattern)
01053 self.action_goal.goal.target.region.mask = struct.unpack(pattern, str[start:end])
01054 _x = self
01055 start = end
01056 end += 12
01057 (_x.action_goal.goal.target.region.image.header.seq, _x.action_goal.goal.target.region.image.header.stamp.secs, _x.action_goal.goal.target.region.image.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01058 start = end
01059 end += 4
01060 (length,) = _struct_I.unpack(str[start:end])
01061 start = end
01062 end += length
01063 self.action_goal.goal.target.region.image.header.frame_id = str[start:end]
01064 _x = self
01065 start = end
01066 end += 8
01067 (_x.action_goal.goal.target.region.image.height, _x.action_goal.goal.target.region.image.width,) = _struct_2I.unpack(str[start:end])
01068 start = end
01069 end += 4
01070 (length,) = _struct_I.unpack(str[start:end])
01071 start = end
01072 end += length
01073 self.action_goal.goal.target.region.image.encoding = str[start:end]
01074 _x = self
01075 start = end
01076 end += 5
01077 (_x.action_goal.goal.target.region.image.is_bigendian, _x.action_goal.goal.target.region.image.step,) = _struct_BI.unpack(str[start:end])
01078 start = end
01079 end += 4
01080 (length,) = _struct_I.unpack(str[start:end])
01081 start = end
01082 end += length
01083 self.action_goal.goal.target.region.image.data = str[start:end]
01084 _x = self
01085 start = end
01086 end += 12
01087 (_x.action_goal.goal.target.region.disparity_image.header.seq, _x.action_goal.goal.target.region.disparity_image.header.stamp.secs, _x.action_goal.goal.target.region.disparity_image.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01088 start = end
01089 end += 4
01090 (length,) = _struct_I.unpack(str[start:end])
01091 start = end
01092 end += length
01093 self.action_goal.goal.target.region.disparity_image.header.frame_id = str[start:end]
01094 _x = self
01095 start = end
01096 end += 8
01097 (_x.action_goal.goal.target.region.disparity_image.height, _x.action_goal.goal.target.region.disparity_image.width,) = _struct_2I.unpack(str[start:end])
01098 start = end
01099 end += 4
01100 (length,) = _struct_I.unpack(str[start:end])
01101 start = end
01102 end += length
01103 self.action_goal.goal.target.region.disparity_image.encoding = str[start:end]
01104 _x = self
01105 start = end
01106 end += 5
01107 (_x.action_goal.goal.target.region.disparity_image.is_bigendian, _x.action_goal.goal.target.region.disparity_image.step,) = _struct_BI.unpack(str[start:end])
01108 start = end
01109 end += 4
01110 (length,) = _struct_I.unpack(str[start:end])
01111 start = end
01112 end += length
01113 self.action_goal.goal.target.region.disparity_image.data = str[start:end]
01114 _x = self
01115 start = end
01116 end += 12
01117 (_x.action_goal.goal.target.region.cam_info.header.seq, _x.action_goal.goal.target.region.cam_info.header.stamp.secs, _x.action_goal.goal.target.region.cam_info.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01118 start = end
01119 end += 4
01120 (length,) = _struct_I.unpack(str[start:end])
01121 start = end
01122 end += length
01123 self.action_goal.goal.target.region.cam_info.header.frame_id = str[start:end]
01124 _x = self
01125 start = end
01126 end += 8
01127 (_x.action_goal.goal.target.region.cam_info.height, _x.action_goal.goal.target.region.cam_info.width,) = _struct_2I.unpack(str[start:end])
01128 start = end
01129 end += 4
01130 (length,) = _struct_I.unpack(str[start:end])
01131 start = end
01132 end += length
01133 self.action_goal.goal.target.region.cam_info.distortion_model = 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.target.region.cam_info.D = struct.unpack(pattern, str[start:end])
01141 start = end
01142 end += 72
01143 self.action_goal.goal.target.region.cam_info.K = _struct_9d.unpack(str[start:end])
01144 start = end
01145 end += 72
01146 self.action_goal.goal.target.region.cam_info.R = _struct_9d.unpack(str[start:end])
01147 start = end
01148 end += 96
01149 self.action_goal.goal.target.region.cam_info.P = _struct_12d.unpack(str[start:end])
01150 _x = self
01151 start = end
01152 end += 37
01153 (_x.action_goal.goal.target.region.cam_info.binning_x, _x.action_goal.goal.target.region.cam_info.binning_y, _x.action_goal.goal.target.region.cam_info.roi.x_offset, _x.action_goal.goal.target.region.cam_info.roi.y_offset, _x.action_goal.goal.target.region.cam_info.roi.height, _x.action_goal.goal.target.region.cam_info.roi.width, _x.action_goal.goal.target.region.cam_info.roi.do_rectify, _x.action_goal.goal.lift.direction.header.seq, _x.action_goal.goal.lift.direction.header.stamp.secs, _x.action_goal.goal.lift.direction.header.stamp.nsecs,) = _struct_6IB3I.unpack(str[start:end])
01154 self.action_goal.goal.target.region.cam_info.roi.do_rectify = bool(self.action_goal.goal.target.region.cam_info.roi.do_rectify)
01155 start = end
01156 end += 4
01157 (length,) = _struct_I.unpack(str[start:end])
01158 start = end
01159 end += length
01160 self.action_goal.goal.lift.direction.header.frame_id = str[start:end]
01161 _x = self
01162 start = end
01163 end += 44
01164 (_x.action_goal.goal.lift.direction.vector.x, _x.action_goal.goal.lift.direction.vector.y, _x.action_goal.goal.lift.direction.vector.z, _x.action_goal.goal.lift.desired_distance, _x.action_goal.goal.lift.min_distance, _x.action_goal.goal.trajectory.header.seq, _x.action_goal.goal.trajectory.header.stamp.secs, _x.action_goal.goal.trajectory.header.stamp.nsecs,) = _struct_3d2f3I.unpack(str[start:end])
01165 start = end
01166 end += 4
01167 (length,) = _struct_I.unpack(str[start:end])
01168 start = end
01169 end += length
01170 self.action_goal.goal.trajectory.header.frame_id = str[start:end]
01171 start = end
01172 end += 4
01173 (length,) = _struct_I.unpack(str[start:end])
01174 self.action_goal.goal.trajectory.joint_names = []
01175 for i in xrange(0, length):
01176 start = end
01177 end += 4
01178 (length,) = _struct_I.unpack(str[start:end])
01179 start = end
01180 end += length
01181 val1 = str[start:end]
01182 self.action_goal.goal.trajectory.joint_names.append(val1)
01183 start = end
01184 end += 4
01185 (length,) = _struct_I.unpack(str[start:end])
01186 self.action_goal.goal.trajectory.points = []
01187 for i in xrange(0, length):
01188 val1 = trajectory_msgs.msg.JointTrajectoryPoint()
01189 start = end
01190 end += 4
01191 (length,) = _struct_I.unpack(str[start:end])
01192 pattern = '<%sd'%length
01193 start = end
01194 end += struct.calcsize(pattern)
01195 val1.positions = struct.unpack(pattern, str[start:end])
01196 start = end
01197 end += 4
01198 (length,) = _struct_I.unpack(str[start:end])
01199 pattern = '<%sd'%length
01200 start = end
01201 end += struct.calcsize(pattern)
01202 val1.velocities = struct.unpack(pattern, str[start:end])
01203 start = end
01204 end += 4
01205 (length,) = _struct_I.unpack(str[start:end])
01206 pattern = '<%sd'%length
01207 start = end
01208 end += struct.calcsize(pattern)
01209 val1.accelerations = struct.unpack(pattern, str[start:end])
01210 _v14 = val1.time_from_start
01211 _x = _v14
01212 start = end
01213 end += 8
01214 (_x.secs, _x.nsecs,) = _struct_2i.unpack(str[start:end])
01215 self.action_goal.goal.trajectory.points.append(val1)
01216 start = end
01217 end += 4
01218 (length,) = _struct_I.unpack(str[start:end])
01219 start = end
01220 end += length
01221 self.action_goal.goal.collision_support_surface_name = str[start:end]
01222 _x = self
01223 start = end
01224 end += 12
01225 (_x.action_result.header.seq, _x.action_result.header.stamp.secs, _x.action_result.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01226 start = end
01227 end += 4
01228 (length,) = _struct_I.unpack(str[start:end])
01229 start = end
01230 end += length
01231 self.action_result.header.frame_id = str[start:end]
01232 _x = self
01233 start = end
01234 end += 8
01235 (_x.action_result.status.goal_id.stamp.secs, _x.action_result.status.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
01236 start = end
01237 end += 4
01238 (length,) = _struct_I.unpack(str[start:end])
01239 start = end
01240 end += length
01241 self.action_result.status.goal_id.id = str[start:end]
01242 start = end
01243 end += 1
01244 (self.action_result.status.status,) = _struct_B.unpack(str[start:end])
01245 start = end
01246 end += 4
01247 (length,) = _struct_I.unpack(str[start:end])
01248 start = end
01249 end += length
01250 self.action_result.status.text = str[start:end]
01251 _x = self
01252 start = end
01253 end += 16
01254 (_x.action_result.result.manipulation_result.value, _x.action_feedback.header.seq, _x.action_feedback.header.stamp.secs, _x.action_feedback.header.stamp.nsecs,) = _struct_i3I.unpack(str[start:end])
01255 start = end
01256 end += 4
01257 (length,) = _struct_I.unpack(str[start:end])
01258 start = end
01259 end += length
01260 self.action_feedback.header.frame_id = str[start:end]
01261 _x = self
01262 start = end
01263 end += 8
01264 (_x.action_feedback.status.goal_id.stamp.secs, _x.action_feedback.status.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
01265 start = end
01266 end += 4
01267 (length,) = _struct_I.unpack(str[start:end])
01268 start = end
01269 end += length
01270 self.action_feedback.status.goal_id.id = str[start:end]
01271 start = end
01272 end += 1
01273 (self.action_feedback.status.status,) = _struct_B.unpack(str[start:end])
01274 start = end
01275 end += 4
01276 (length,) = _struct_I.unpack(str[start:end])
01277 start = end
01278 end += length
01279 self.action_feedback.status.text = str[start:end]
01280 start = end
01281 end += 4
01282 (self.action_feedback.feedback.manipulation_phase.phase,) = _struct_i.unpack(str[start:end])
01283 return self
01284 except struct.error, e:
01285 raise roslib.message.DeserializationError(e) #most likely buffer underfill
01286
01287
01288 def serialize_numpy(self, buff, numpy):
01289 """
01290 serialize message with numpy array types into buffer
01291 @param buff: buffer
01292 @type buff: StringIO
01293 @param numpy: numpy python module
01294 @type numpy module
01295 """
01296 try:
01297 _x = self
01298 buff.write(_struct_3I.pack(_x.action_goal.header.seq, _x.action_goal.header.stamp.secs, _x.action_goal.header.stamp.nsecs))
01299 _x = self.action_goal.header.frame_id
01300 length = len(_x)
01301 buff.write(struct.pack('<I%ss'%length, length, _x))
01302 _x = self
01303 buff.write(_struct_2I.pack(_x.action_goal.goal_id.stamp.secs, _x.action_goal.goal_id.stamp.nsecs))
01304 _x = self.action_goal.goal_id.id
01305 length = len(_x)
01306 buff.write(struct.pack('<I%ss'%length, length, _x))
01307 _x = self.action_goal.goal.arm_name
01308 length = len(_x)
01309 buff.write(struct.pack('<I%ss'%length, length, _x))
01310 _x = self.action_goal.goal.target.reference_frame_id
01311 length = len(_x)
01312 buff.write(struct.pack('<I%ss'%length, length, _x))
01313 length = len(self.action_goal.goal.target.potential_models)
01314 buff.write(_struct_I.pack(length))
01315 for val1 in self.action_goal.goal.target.potential_models:
01316 buff.write(_struct_i.pack(val1.model_id))
01317 _v15 = val1.pose
01318 _v16 = _v15.header
01319 buff.write(_struct_I.pack(_v16.seq))
01320 _v17 = _v16.stamp
01321 _x = _v17
01322 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01323 _x = _v16.frame_id
01324 length = len(_x)
01325 buff.write(struct.pack('<I%ss'%length, length, _x))
01326 _v18 = _v15.pose
01327 _v19 = _v18.position
01328 _x = _v19
01329 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01330 _v20 = _v18.orientation
01331 _x = _v20
01332 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01333 buff.write(_struct_f.pack(val1.confidence))
01334 _x = self
01335 buff.write(_struct_3I.pack(_x.action_goal.goal.target.cluster.header.seq, _x.action_goal.goal.target.cluster.header.stamp.secs, _x.action_goal.goal.target.cluster.header.stamp.nsecs))
01336 _x = self.action_goal.goal.target.cluster.header.frame_id
01337 length = len(_x)
01338 buff.write(struct.pack('<I%ss'%length, length, _x))
01339 length = len(self.action_goal.goal.target.cluster.points)
01340 buff.write(_struct_I.pack(length))
01341 for val1 in self.action_goal.goal.target.cluster.points:
01342 _x = val1
01343 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z))
01344 length = len(self.action_goal.goal.target.cluster.channels)
01345 buff.write(_struct_I.pack(length))
01346 for val1 in self.action_goal.goal.target.cluster.channels:
01347 _x = val1.name
01348 length = len(_x)
01349 buff.write(struct.pack('<I%ss'%length, length, _x))
01350 length = len(val1.values)
01351 buff.write(_struct_I.pack(length))
01352 pattern = '<%sf'%length
01353 buff.write(val1.values.tostring())
01354 _x = self
01355 buff.write(_struct_3I.pack(_x.action_goal.goal.target.region.cloud.header.seq, _x.action_goal.goal.target.region.cloud.header.stamp.secs, _x.action_goal.goal.target.region.cloud.header.stamp.nsecs))
01356 _x = self.action_goal.goal.target.region.cloud.header.frame_id
01357 length = len(_x)
01358 buff.write(struct.pack('<I%ss'%length, length, _x))
01359 _x = self
01360 buff.write(_struct_2I.pack(_x.action_goal.goal.target.region.cloud.height, _x.action_goal.goal.target.region.cloud.width))
01361 length = len(self.action_goal.goal.target.region.cloud.fields)
01362 buff.write(_struct_I.pack(length))
01363 for val1 in self.action_goal.goal.target.region.cloud.fields:
01364 _x = val1.name
01365 length = len(_x)
01366 buff.write(struct.pack('<I%ss'%length, length, _x))
01367 _x = val1
01368 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count))
01369 _x = self
01370 buff.write(_struct_B2I.pack(_x.action_goal.goal.target.region.cloud.is_bigendian, _x.action_goal.goal.target.region.cloud.point_step, _x.action_goal.goal.target.region.cloud.row_step))
01371 _x = self.action_goal.goal.target.region.cloud.data
01372 length = len(_x)
01373 # - if encoded as a list instead, serialize as bytes instead of string
01374 if type(_x) in [list, tuple]:
01375 buff.write(struct.pack('<I%sB'%length, length, *_x))
01376 else:
01377 buff.write(struct.pack('<I%ss'%length, length, _x))
01378 buff.write(_struct_B.pack(self.action_goal.goal.target.region.cloud.is_dense))
01379 length = len(self.action_goal.goal.target.region.mask)
01380 buff.write(_struct_I.pack(length))
01381 pattern = '<%si'%length
01382 buff.write(self.action_goal.goal.target.region.mask.tostring())
01383 _x = self
01384 buff.write(_struct_3I.pack(_x.action_goal.goal.target.region.image.header.seq, _x.action_goal.goal.target.region.image.header.stamp.secs, _x.action_goal.goal.target.region.image.header.stamp.nsecs))
01385 _x = self.action_goal.goal.target.region.image.header.frame_id
01386 length = len(_x)
01387 buff.write(struct.pack('<I%ss'%length, length, _x))
01388 _x = self
01389 buff.write(_struct_2I.pack(_x.action_goal.goal.target.region.image.height, _x.action_goal.goal.target.region.image.width))
01390 _x = self.action_goal.goal.target.region.image.encoding
01391 length = len(_x)
01392 buff.write(struct.pack('<I%ss'%length, length, _x))
01393 _x = self
01394 buff.write(_struct_BI.pack(_x.action_goal.goal.target.region.image.is_bigendian, _x.action_goal.goal.target.region.image.step))
01395 _x = self.action_goal.goal.target.region.image.data
01396 length = len(_x)
01397 # - if encoded as a list instead, serialize as bytes instead of string
01398 if type(_x) in [list, tuple]:
01399 buff.write(struct.pack('<I%sB'%length, length, *_x))
01400 else:
01401 buff.write(struct.pack('<I%ss'%length, length, _x))
01402 _x = self
01403 buff.write(_struct_3I.pack(_x.action_goal.goal.target.region.disparity_image.header.seq, _x.action_goal.goal.target.region.disparity_image.header.stamp.secs, _x.action_goal.goal.target.region.disparity_image.header.stamp.nsecs))
01404 _x = self.action_goal.goal.target.region.disparity_image.header.frame_id
01405 length = len(_x)
01406 buff.write(struct.pack('<I%ss'%length, length, _x))
01407 _x = self
01408 buff.write(_struct_2I.pack(_x.action_goal.goal.target.region.disparity_image.height, _x.action_goal.goal.target.region.disparity_image.width))
01409 _x = self.action_goal.goal.target.region.disparity_image.encoding
01410 length = len(_x)
01411 buff.write(struct.pack('<I%ss'%length, length, _x))
01412 _x = self
01413 buff.write(_struct_BI.pack(_x.action_goal.goal.target.region.disparity_image.is_bigendian, _x.action_goal.goal.target.region.disparity_image.step))
01414 _x = self.action_goal.goal.target.region.disparity_image.data
01415 length = len(_x)
01416 # - if encoded as a list instead, serialize as bytes instead of string
01417 if type(_x) in [list, tuple]:
01418 buff.write(struct.pack('<I%sB'%length, length, *_x))
01419 else:
01420 buff.write(struct.pack('<I%ss'%length, length, _x))
01421 _x = self
01422 buff.write(_struct_3I.pack(_x.action_goal.goal.target.region.cam_info.header.seq, _x.action_goal.goal.target.region.cam_info.header.stamp.secs, _x.action_goal.goal.target.region.cam_info.header.stamp.nsecs))
01423 _x = self.action_goal.goal.target.region.cam_info.header.frame_id
01424 length = len(_x)
01425 buff.write(struct.pack('<I%ss'%length, length, _x))
01426 _x = self
01427 buff.write(_struct_2I.pack(_x.action_goal.goal.target.region.cam_info.height, _x.action_goal.goal.target.region.cam_info.width))
01428 _x = self.action_goal.goal.target.region.cam_info.distortion_model
01429 length = len(_x)
01430 buff.write(struct.pack('<I%ss'%length, length, _x))
01431 length = len(self.action_goal.goal.target.region.cam_info.D)
01432 buff.write(_struct_I.pack(length))
01433 pattern = '<%sd'%length
01434 buff.write(self.action_goal.goal.target.region.cam_info.D.tostring())
01435 buff.write(self.action_goal.goal.target.region.cam_info.K.tostring())
01436 buff.write(self.action_goal.goal.target.region.cam_info.R.tostring())
01437 buff.write(self.action_goal.goal.target.region.cam_info.P.tostring())
01438 _x = self
01439 buff.write(_struct_6IB3I.pack(_x.action_goal.goal.target.region.cam_info.binning_x, _x.action_goal.goal.target.region.cam_info.binning_y, _x.action_goal.goal.target.region.cam_info.roi.x_offset, _x.action_goal.goal.target.region.cam_info.roi.y_offset, _x.action_goal.goal.target.region.cam_info.roi.height, _x.action_goal.goal.target.region.cam_info.roi.width, _x.action_goal.goal.target.region.cam_info.roi.do_rectify, _x.action_goal.goal.lift.direction.header.seq, _x.action_goal.goal.lift.direction.header.stamp.secs, _x.action_goal.goal.lift.direction.header.stamp.nsecs))
01440 _x = self.action_goal.goal.lift.direction.header.frame_id
01441 length = len(_x)
01442 buff.write(struct.pack('<I%ss'%length, length, _x))
01443 _x = self
01444 buff.write(_struct_3d2f3I.pack(_x.action_goal.goal.lift.direction.vector.x, _x.action_goal.goal.lift.direction.vector.y, _x.action_goal.goal.lift.direction.vector.z, _x.action_goal.goal.lift.desired_distance, _x.action_goal.goal.lift.min_distance, _x.action_goal.goal.trajectory.header.seq, _x.action_goal.goal.trajectory.header.stamp.secs, _x.action_goal.goal.trajectory.header.stamp.nsecs))
01445 _x = self.action_goal.goal.trajectory.header.frame_id
01446 length = len(_x)
01447 buff.write(struct.pack('<I%ss'%length, length, _x))
01448 length = len(self.action_goal.goal.trajectory.joint_names)
01449 buff.write(_struct_I.pack(length))
01450 for val1 in self.action_goal.goal.trajectory.joint_names:
01451 length = len(val1)
01452 buff.write(struct.pack('<I%ss'%length, length, val1))
01453 length = len(self.action_goal.goal.trajectory.points)
01454 buff.write(_struct_I.pack(length))
01455 for val1 in self.action_goal.goal.trajectory.points:
01456 length = len(val1.positions)
01457 buff.write(_struct_I.pack(length))
01458 pattern = '<%sd'%length
01459 buff.write(val1.positions.tostring())
01460 length = len(val1.velocities)
01461 buff.write(_struct_I.pack(length))
01462 pattern = '<%sd'%length
01463 buff.write(val1.velocities.tostring())
01464 length = len(val1.accelerations)
01465 buff.write(_struct_I.pack(length))
01466 pattern = '<%sd'%length
01467 buff.write(val1.accelerations.tostring())
01468 _v21 = val1.time_from_start
01469 _x = _v21
01470 buff.write(_struct_2i.pack(_x.secs, _x.nsecs))
01471 _x = self.action_goal.goal.collision_support_surface_name
01472 length = len(_x)
01473 buff.write(struct.pack('<I%ss'%length, length, _x))
01474 _x = self
01475 buff.write(_struct_3I.pack(_x.action_result.header.seq, _x.action_result.header.stamp.secs, _x.action_result.header.stamp.nsecs))
01476 _x = self.action_result.header.frame_id
01477 length = len(_x)
01478 buff.write(struct.pack('<I%ss'%length, length, _x))
01479 _x = self
01480 buff.write(_struct_2I.pack(_x.action_result.status.goal_id.stamp.secs, _x.action_result.status.goal_id.stamp.nsecs))
01481 _x = self.action_result.status.goal_id.id
01482 length = len(_x)
01483 buff.write(struct.pack('<I%ss'%length, length, _x))
01484 buff.write(_struct_B.pack(self.action_result.status.status))
01485 _x = self.action_result.status.text
01486 length = len(_x)
01487 buff.write(struct.pack('<I%ss'%length, length, _x))
01488 _x = self
01489 buff.write(_struct_i3I.pack(_x.action_result.result.manipulation_result.value, _x.action_feedback.header.seq, _x.action_feedback.header.stamp.secs, _x.action_feedback.header.stamp.nsecs))
01490 _x = self.action_feedback.header.frame_id
01491 length = len(_x)
01492 buff.write(struct.pack('<I%ss'%length, length, _x))
01493 _x = self
01494 buff.write(_struct_2I.pack(_x.action_feedback.status.goal_id.stamp.secs, _x.action_feedback.status.goal_id.stamp.nsecs))
01495 _x = self.action_feedback.status.goal_id.id
01496 length = len(_x)
01497 buff.write(struct.pack('<I%ss'%length, length, _x))
01498 buff.write(_struct_B.pack(self.action_feedback.status.status))
01499 _x = self.action_feedback.status.text
01500 length = len(_x)
01501 buff.write(struct.pack('<I%ss'%length, length, _x))
01502 buff.write(_struct_i.pack(self.action_feedback.feedback.manipulation_phase.phase))
01503 except struct.error, se: self._check_types(se)
01504 except TypeError, te: self._check_types(te)
01505
01506 def deserialize_numpy(self, str, numpy):
01507 """
01508 unpack serialized message in str into this message instance using numpy for array types
01509 @param str: byte array of serialized message
01510 @type str: str
01511 @param numpy: numpy python module
01512 @type numpy: module
01513 """
01514 try:
01515 if self.action_goal is None:
01516 self.action_goal = object_manipulation_msgs.msg.ReactiveLiftActionGoal()
01517 if self.action_result is None:
01518 self.action_result = object_manipulation_msgs.msg.ReactiveLiftActionResult()
01519 if self.action_feedback is None:
01520 self.action_feedback = object_manipulation_msgs.msg.ReactiveLiftActionFeedback()
01521 end = 0
01522 _x = self
01523 start = end
01524 end += 12
01525 (_x.action_goal.header.seq, _x.action_goal.header.stamp.secs, _x.action_goal.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01526 start = end
01527 end += 4
01528 (length,) = _struct_I.unpack(str[start:end])
01529 start = end
01530 end += length
01531 self.action_goal.header.frame_id = str[start:end]
01532 _x = self
01533 start = end
01534 end += 8
01535 (_x.action_goal.goal_id.stamp.secs, _x.action_goal.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
01536 start = end
01537 end += 4
01538 (length,) = _struct_I.unpack(str[start:end])
01539 start = end
01540 end += length
01541 self.action_goal.goal_id.id = str[start:end]
01542 start = end
01543 end += 4
01544 (length,) = _struct_I.unpack(str[start:end])
01545 start = end
01546 end += length
01547 self.action_goal.goal.arm_name = str[start:end]
01548 start = end
01549 end += 4
01550 (length,) = _struct_I.unpack(str[start:end])
01551 start = end
01552 end += length
01553 self.action_goal.goal.target.reference_frame_id = str[start:end]
01554 start = end
01555 end += 4
01556 (length,) = _struct_I.unpack(str[start:end])
01557 self.action_goal.goal.target.potential_models = []
01558 for i in xrange(0, length):
01559 val1 = household_objects_database_msgs.msg.DatabaseModelPose()
01560 start = end
01561 end += 4
01562 (val1.model_id,) = _struct_i.unpack(str[start:end])
01563 _v22 = val1.pose
01564 _v23 = _v22.header
01565 start = end
01566 end += 4
01567 (_v23.seq,) = _struct_I.unpack(str[start:end])
01568 _v24 = _v23.stamp
01569 _x = _v24
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 _v23.frame_id = str[start:end]
01579 _v25 = _v22.pose
01580 _v26 = _v25.position
01581 _x = _v26
01582 start = end
01583 end += 24
01584 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01585 _v27 = _v25.orientation
01586 _x = _v27
01587 start = end
01588 end += 32
01589 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01590 start = end
01591 end += 4
01592 (val1.confidence,) = _struct_f.unpack(str[start:end])
01593 self.action_goal.goal.target.potential_models.append(val1)
01594 _x = self
01595 start = end
01596 end += 12
01597 (_x.action_goal.goal.target.cluster.header.seq, _x.action_goal.goal.target.cluster.header.stamp.secs, _x.action_goal.goal.target.cluster.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01598 start = end
01599 end += 4
01600 (length,) = _struct_I.unpack(str[start:end])
01601 start = end
01602 end += length
01603 self.action_goal.goal.target.cluster.header.frame_id = str[start:end]
01604 start = end
01605 end += 4
01606 (length,) = _struct_I.unpack(str[start:end])
01607 self.action_goal.goal.target.cluster.points = []
01608 for i in xrange(0, length):
01609 val1 = geometry_msgs.msg.Point32()
01610 _x = val1
01611 start = end
01612 end += 12
01613 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end])
01614 self.action_goal.goal.target.cluster.points.append(val1)
01615 start = end
01616 end += 4
01617 (length,) = _struct_I.unpack(str[start:end])
01618 self.action_goal.goal.target.cluster.channels = []
01619 for i in xrange(0, length):
01620 val1 = sensor_msgs.msg.ChannelFloat32()
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 start = end
01628 end += 4
01629 (length,) = _struct_I.unpack(str[start:end])
01630 pattern = '<%sf'%length
01631 start = end
01632 end += struct.calcsize(pattern)
01633 val1.values = numpy.frombuffer(str[start:end], dtype=numpy.float32, count=length)
01634 self.action_goal.goal.target.cluster.channels.append(val1)
01635 _x = self
01636 start = end
01637 end += 12
01638 (_x.action_goal.goal.target.region.cloud.header.seq, _x.action_goal.goal.target.region.cloud.header.stamp.secs, _x.action_goal.goal.target.region.cloud.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01639 start = end
01640 end += 4
01641 (length,) = _struct_I.unpack(str[start:end])
01642 start = end
01643 end += length
01644 self.action_goal.goal.target.region.cloud.header.frame_id = str[start:end]
01645 _x = self
01646 start = end
01647 end += 8
01648 (_x.action_goal.goal.target.region.cloud.height, _x.action_goal.goal.target.region.cloud.width,) = _struct_2I.unpack(str[start:end])
01649 start = end
01650 end += 4
01651 (length,) = _struct_I.unpack(str[start:end])
01652 self.action_goal.goal.target.region.cloud.fields = []
01653 for i in xrange(0, length):
01654 val1 = sensor_msgs.msg.PointField()
01655 start = end
01656 end += 4
01657 (length,) = _struct_I.unpack(str[start:end])
01658 start = end
01659 end += length
01660 val1.name = str[start:end]
01661 _x = val1
01662 start = end
01663 end += 9
01664 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end])
01665 self.action_goal.goal.target.region.cloud.fields.append(val1)
01666 _x = self
01667 start = end
01668 end += 9
01669 (_x.action_goal.goal.target.region.cloud.is_bigendian, _x.action_goal.goal.target.region.cloud.point_step, _x.action_goal.goal.target.region.cloud.row_step,) = _struct_B2I.unpack(str[start:end])
01670 self.action_goal.goal.target.region.cloud.is_bigendian = bool(self.action_goal.goal.target.region.cloud.is_bigendian)
01671 start = end
01672 end += 4
01673 (length,) = _struct_I.unpack(str[start:end])
01674 start = end
01675 end += length
01676 self.action_goal.goal.target.region.cloud.data = str[start:end]
01677 start = end
01678 end += 1
01679 (self.action_goal.goal.target.region.cloud.is_dense,) = _struct_B.unpack(str[start:end])
01680 self.action_goal.goal.target.region.cloud.is_dense = bool(self.action_goal.goal.target.region.cloud.is_dense)
01681 start = end
01682 end += 4
01683 (length,) = _struct_I.unpack(str[start:end])
01684 pattern = '<%si'%length
01685 start = end
01686 end += struct.calcsize(pattern)
01687 self.action_goal.goal.target.region.mask = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
01688 _x = self
01689 start = end
01690 end += 12
01691 (_x.action_goal.goal.target.region.image.header.seq, _x.action_goal.goal.target.region.image.header.stamp.secs, _x.action_goal.goal.target.region.image.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01692 start = end
01693 end += 4
01694 (length,) = _struct_I.unpack(str[start:end])
01695 start = end
01696 end += length
01697 self.action_goal.goal.target.region.image.header.frame_id = str[start:end]
01698 _x = self
01699 start = end
01700 end += 8
01701 (_x.action_goal.goal.target.region.image.height, _x.action_goal.goal.target.region.image.width,) = _struct_2I.unpack(str[start:end])
01702 start = end
01703 end += 4
01704 (length,) = _struct_I.unpack(str[start:end])
01705 start = end
01706 end += length
01707 self.action_goal.goal.target.region.image.encoding = str[start:end]
01708 _x = self
01709 start = end
01710 end += 5
01711 (_x.action_goal.goal.target.region.image.is_bigendian, _x.action_goal.goal.target.region.image.step,) = _struct_BI.unpack(str[start:end])
01712 start = end
01713 end += 4
01714 (length,) = _struct_I.unpack(str[start:end])
01715 start = end
01716 end += length
01717 self.action_goal.goal.target.region.image.data = str[start:end]
01718 _x = self
01719 start = end
01720 end += 12
01721 (_x.action_goal.goal.target.region.disparity_image.header.seq, _x.action_goal.goal.target.region.disparity_image.header.stamp.secs, _x.action_goal.goal.target.region.disparity_image.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01722 start = end
01723 end += 4
01724 (length,) = _struct_I.unpack(str[start:end])
01725 start = end
01726 end += length
01727 self.action_goal.goal.target.region.disparity_image.header.frame_id = str[start:end]
01728 _x = self
01729 start = end
01730 end += 8
01731 (_x.action_goal.goal.target.region.disparity_image.height, _x.action_goal.goal.target.region.disparity_image.width,) = _struct_2I.unpack(str[start:end])
01732 start = end
01733 end += 4
01734 (length,) = _struct_I.unpack(str[start:end])
01735 start = end
01736 end += length
01737 self.action_goal.goal.target.region.disparity_image.encoding = str[start:end]
01738 _x = self
01739 start = end
01740 end += 5
01741 (_x.action_goal.goal.target.region.disparity_image.is_bigendian, _x.action_goal.goal.target.region.disparity_image.step,) = _struct_BI.unpack(str[start:end])
01742 start = end
01743 end += 4
01744 (length,) = _struct_I.unpack(str[start:end])
01745 start = end
01746 end += length
01747 self.action_goal.goal.target.region.disparity_image.data = str[start:end]
01748 _x = self
01749 start = end
01750 end += 12
01751 (_x.action_goal.goal.target.region.cam_info.header.seq, _x.action_goal.goal.target.region.cam_info.header.stamp.secs, _x.action_goal.goal.target.region.cam_info.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01752 start = end
01753 end += 4
01754 (length,) = _struct_I.unpack(str[start:end])
01755 start = end
01756 end += length
01757 self.action_goal.goal.target.region.cam_info.header.frame_id = str[start:end]
01758 _x = self
01759 start = end
01760 end += 8
01761 (_x.action_goal.goal.target.region.cam_info.height, _x.action_goal.goal.target.region.cam_info.width,) = _struct_2I.unpack(str[start:end])
01762 start = end
01763 end += 4
01764 (length,) = _struct_I.unpack(str[start:end])
01765 start = end
01766 end += length
01767 self.action_goal.goal.target.region.cam_info.distortion_model = str[start:end]
01768 start = end
01769 end += 4
01770 (length,) = _struct_I.unpack(str[start:end])
01771 pattern = '<%sd'%length
01772 start = end
01773 end += struct.calcsize(pattern)
01774 self.action_goal.goal.target.region.cam_info.D = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01775 start = end
01776 end += 72
01777 self.action_goal.goal.target.region.cam_info.K = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9)
01778 start = end
01779 end += 72
01780 self.action_goal.goal.target.region.cam_info.R = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9)
01781 start = end
01782 end += 96
01783 self.action_goal.goal.target.region.cam_info.P = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=12)
01784 _x = self
01785 start = end
01786 end += 37
01787 (_x.action_goal.goal.target.region.cam_info.binning_x, _x.action_goal.goal.target.region.cam_info.binning_y, _x.action_goal.goal.target.region.cam_info.roi.x_offset, _x.action_goal.goal.target.region.cam_info.roi.y_offset, _x.action_goal.goal.target.region.cam_info.roi.height, _x.action_goal.goal.target.region.cam_info.roi.width, _x.action_goal.goal.target.region.cam_info.roi.do_rectify, _x.action_goal.goal.lift.direction.header.seq, _x.action_goal.goal.lift.direction.header.stamp.secs, _x.action_goal.goal.lift.direction.header.stamp.nsecs,) = _struct_6IB3I.unpack(str[start:end])
01788 self.action_goal.goal.target.region.cam_info.roi.do_rectify = bool(self.action_goal.goal.target.region.cam_info.roi.do_rectify)
01789 start = end
01790 end += 4
01791 (length,) = _struct_I.unpack(str[start:end])
01792 start = end
01793 end += length
01794 self.action_goal.goal.lift.direction.header.frame_id = str[start:end]
01795 _x = self
01796 start = end
01797 end += 44
01798 (_x.action_goal.goal.lift.direction.vector.x, _x.action_goal.goal.lift.direction.vector.y, _x.action_goal.goal.lift.direction.vector.z, _x.action_goal.goal.lift.desired_distance, _x.action_goal.goal.lift.min_distance, _x.action_goal.goal.trajectory.header.seq, _x.action_goal.goal.trajectory.header.stamp.secs, _x.action_goal.goal.trajectory.header.stamp.nsecs,) = _struct_3d2f3I.unpack(str[start:end])
01799 start = end
01800 end += 4
01801 (length,) = _struct_I.unpack(str[start:end])
01802 start = end
01803 end += length
01804 self.action_goal.goal.trajectory.header.frame_id = str[start:end]
01805 start = end
01806 end += 4
01807 (length,) = _struct_I.unpack(str[start:end])
01808 self.action_goal.goal.trajectory.joint_names = []
01809 for i in xrange(0, length):
01810 start = end
01811 end += 4
01812 (length,) = _struct_I.unpack(str[start:end])
01813 start = end
01814 end += length
01815 val1 = str[start:end]
01816 self.action_goal.goal.trajectory.joint_names.append(val1)
01817 start = end
01818 end += 4
01819 (length,) = _struct_I.unpack(str[start:end])
01820 self.action_goal.goal.trajectory.points = []
01821 for i in xrange(0, length):
01822 val1 = trajectory_msgs.msg.JointTrajectoryPoint()
01823 start = end
01824 end += 4
01825 (length,) = _struct_I.unpack(str[start:end])
01826 pattern = '<%sd'%length
01827 start = end
01828 end += struct.calcsize(pattern)
01829 val1.positions = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01830 start = end
01831 end += 4
01832 (length,) = _struct_I.unpack(str[start:end])
01833 pattern = '<%sd'%length
01834 start = end
01835 end += struct.calcsize(pattern)
01836 val1.velocities = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01837 start = end
01838 end += 4
01839 (length,) = _struct_I.unpack(str[start:end])
01840 pattern = '<%sd'%length
01841 start = end
01842 end += struct.calcsize(pattern)
01843 val1.accelerations = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01844 _v28 = val1.time_from_start
01845 _x = _v28
01846 start = end
01847 end += 8
01848 (_x.secs, _x.nsecs,) = _struct_2i.unpack(str[start:end])
01849 self.action_goal.goal.trajectory.points.append(val1)
01850 start = end
01851 end += 4
01852 (length,) = _struct_I.unpack(str[start:end])
01853 start = end
01854 end += length
01855 self.action_goal.goal.collision_support_surface_name = str[start:end]
01856 _x = self
01857 start = end
01858 end += 12
01859 (_x.action_result.header.seq, _x.action_result.header.stamp.secs, _x.action_result.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01860 start = end
01861 end += 4
01862 (length,) = _struct_I.unpack(str[start:end])
01863 start = end
01864 end += length
01865 self.action_result.header.frame_id = str[start:end]
01866 _x = self
01867 start = end
01868 end += 8
01869 (_x.action_result.status.goal_id.stamp.secs, _x.action_result.status.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
01870 start = end
01871 end += 4
01872 (length,) = _struct_I.unpack(str[start:end])
01873 start = end
01874 end += length
01875 self.action_result.status.goal_id.id = str[start:end]
01876 start = end
01877 end += 1
01878 (self.action_result.status.status,) = _struct_B.unpack(str[start:end])
01879 start = end
01880 end += 4
01881 (length,) = _struct_I.unpack(str[start:end])
01882 start = end
01883 end += length
01884 self.action_result.status.text = str[start:end]
01885 _x = self
01886 start = end
01887 end += 16
01888 (_x.action_result.result.manipulation_result.value, _x.action_feedback.header.seq, _x.action_feedback.header.stamp.secs, _x.action_feedback.header.stamp.nsecs,) = _struct_i3I.unpack(str[start:end])
01889 start = end
01890 end += 4
01891 (length,) = _struct_I.unpack(str[start:end])
01892 start = end
01893 end += length
01894 self.action_feedback.header.frame_id = str[start:end]
01895 _x = self
01896 start = end
01897 end += 8
01898 (_x.action_feedback.status.goal_id.stamp.secs, _x.action_feedback.status.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
01899 start = end
01900 end += 4
01901 (length,) = _struct_I.unpack(str[start:end])
01902 start = end
01903 end += length
01904 self.action_feedback.status.goal_id.id = str[start:end]
01905 start = end
01906 end += 1
01907 (self.action_feedback.status.status,) = _struct_B.unpack(str[start:end])
01908 start = end
01909 end += 4
01910 (length,) = _struct_I.unpack(str[start:end])
01911 start = end
01912 end += length
01913 self.action_feedback.status.text = str[start:end]
01914 start = end
01915 end += 4
01916 (self.action_feedback.feedback.manipulation_phase.phase,) = _struct_i.unpack(str[start:end])
01917 return self
01918 except struct.error, e:
01919 raise roslib.message.DeserializationError(e) #most likely buffer underfill
01920
01921 _struct_I = roslib.message.struct_I
01922 _struct_IBI = struct.Struct("<IBI")
01923 _struct_6IB3I = struct.Struct("<6IB3I")
01924 _struct_B = struct.Struct("<B")
01925 _struct_12d = struct.Struct("<12d")
01926 _struct_i3I = struct.Struct("<i3I")
01927 _struct_f = struct.Struct("<f")
01928 _struct_i = struct.Struct("<i")
01929 _struct_BI = struct.Struct("<BI")
01930 _struct_3f = struct.Struct("<3f")
01931 _struct_2i = struct.Struct("<2i")
01932 _struct_3I = struct.Struct("<3I")
01933 _struct_9d = struct.Struct("<9d")
01934 _struct_B2I = struct.Struct("<B2I")
01935 _struct_3d2f3I = struct.Struct("<3d2f3I")
01936 _struct_4d = struct.Struct("<4d")
01937 _struct_2I = struct.Struct("<2I")
01938 _struct_3d = struct.Struct("<3d")