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