$search
00001 """autogenerated by genmsg_py from PlaceAction.msg. Do not edit.""" 00002 import roslib.message 00003 import struct 00004 00005 import arm_navigation_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 household_objects_database_msgs.msg 00012 import std_msgs.msg 00013 00014 class PlaceAction(roslib.message.Message): 00015 _md5sum = "ab3cf2087cd7ecb514df855fc4069d22" 00016 _type = "object_manipulation_msgs/PlaceAction" 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 PlaceActionGoal action_goal 00021 PlaceActionResult action_result 00022 PlaceActionFeedback action_feedback 00023 00024 ================================================================================ 00025 MSG: object_manipulation_msgs/PlaceActionGoal 00026 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ====== 00027 00028 Header header 00029 actionlib_msgs/GoalID goal_id 00030 PlaceGoal 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/PlaceGoal 00065 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ====== 00066 # An action for placing an object 00067 00068 # which arm to be used for grasping 00069 string arm_name 00070 00071 # a list of possible transformations for placing the object (place_trans) 00072 # the final pose of the wrist for placement (place_wrist_pose) is as follows: 00073 # place_wrist_pose = place_trans * grasp_pose 00074 # the frame_id for wrist_trans is defined here, and 00075 # should be the same for all place_locations 00076 geometry_msgs/PoseStamped[] place_locations 00077 00078 # the grasp that has been executed on this object 00079 # (contains the grasp_pose referred to above) 00080 Grasp grasp 00081 00082 # how far the retreat should ideally be away from the place location 00083 float32 desired_retreat_distance 00084 00085 # the min distance between the retreat and the place location that must actually be feasible 00086 # for the place not to be rejected 00087 float32 min_retreat_distance 00088 00089 # how the place location should be approached 00090 # the frame_id that this lift is specified in MUST be either the robot_frame 00091 # or the gripper_frame specified in your hand description file 00092 GripperTranslation approach 00093 00094 # the name that the target object has in the collision map 00095 # can be left empty if no name is available 00096 string collision_object_name 00097 00098 # the name that the support surface (e.g. table) has in the collision map 00099 # can be left empty if no name is available 00100 string collision_support_surface_name 00101 00102 # whether collisions between the gripper and the support surface should be acceptable 00103 # during move from pre-place to place and during retreat. Collisions when moving to the 00104 # pre-place location are still not allowed even if this is set to true. 00105 bool allow_gripper_support_collision 00106 00107 # whether reactive placing based on tactile sensors should be used 00108 bool use_reactive_place 00109 00110 # how much the object should be padded by when deciding if the grasp 00111 # location is freasible or not 00112 float64 place_padding 00113 00114 # set this to true if you only want to query the manipulation pipeline as to what 00115 # place locations it thinks are feasible, without actually executing them. If this is set to 00116 # true, the atempted_location_results field of the result will be populated, but no arm 00117 # movement will be attempted 00118 bool only_perform_feasibility_test 00119 00120 # OPTIONAL (These will not have to be filled out most of the time) 00121 # constraints to be imposed on every point in the motion of the arm 00122 arm_navigation_msgs/Constraints path_constraints 00123 00124 # OPTIONAL (These will not have to be filled out most of the time) 00125 # additional collision operations to be used for every arm movement performed 00126 # during placing. Note that these will be added on top of (and thus overide) other 00127 # collision operations that the grasping pipeline deems necessary. Should be used 00128 # with care and only if special behaviors are desired. 00129 arm_navigation_msgs/OrderedCollisionOperations additional_collision_operations 00130 00131 # OPTIONAL (These will not have to be filled out most of the time) 00132 # additional link paddings to be used for every arm movement performed 00133 # during placing. Note that these will be added on top of (and thus overide) other 00134 # link paddings that the grasping pipeline deems necessary. Should be used 00135 # with care and only if special behaviors are desired. 00136 arm_navigation_msgs/LinkPadding[] additional_link_padding 00137 00138 00139 ================================================================================ 00140 MSG: geometry_msgs/PoseStamped 00141 # A Pose with reference coordinate frame and timestamp 00142 Header header 00143 Pose pose 00144 00145 ================================================================================ 00146 MSG: geometry_msgs/Pose 00147 # A representation of pose in free space, composed of postion and orientation. 00148 Point position 00149 Quaternion orientation 00150 00151 ================================================================================ 00152 MSG: geometry_msgs/Point 00153 # This contains the position of a point in free space 00154 float64 x 00155 float64 y 00156 float64 z 00157 00158 ================================================================================ 00159 MSG: geometry_msgs/Quaternion 00160 # This represents an orientation in free space in quaternion form. 00161 00162 float64 x 00163 float64 y 00164 float64 z 00165 float64 w 00166 00167 ================================================================================ 00168 MSG: object_manipulation_msgs/Grasp 00169 00170 # The internal posture of the hand for the pre-grasp 00171 # only positions are used 00172 sensor_msgs/JointState pre_grasp_posture 00173 00174 # The internal posture of the hand for the grasp 00175 # positions and efforts are used 00176 sensor_msgs/JointState grasp_posture 00177 00178 # The position of the end-effector for the grasp relative to a reference frame 00179 # (that is always specified elsewhere, not in this message) 00180 geometry_msgs/Pose grasp_pose 00181 00182 # The estimated probability of success for this grasp 00183 float64 success_probability 00184 00185 # Debug flag to indicate that this grasp would be the best in its cluster 00186 bool cluster_rep 00187 00188 # how far the pre-grasp should ideally be away from the grasp 00189 float32 desired_approach_distance 00190 00191 # how much distance between pre-grasp and grasp must actually be feasible 00192 # for the grasp not to be rejected 00193 float32 min_approach_distance 00194 00195 # an optional list of obstacles that we have semantic information about 00196 # and that we expect might move in the course of executing this grasp 00197 # the grasp planner is expected to make sure they move in an OK way; during 00198 # execution, grasp executors will not check for collisions against these objects 00199 GraspableObject[] moved_obstacles 00200 00201 ================================================================================ 00202 MSG: sensor_msgs/JointState 00203 # This is a message that holds data to describe the state of a set of torque controlled joints. 00204 # 00205 # The state of each joint (revolute or prismatic) is defined by: 00206 # * the position of the joint (rad or m), 00207 # * the velocity of the joint (rad/s or m/s) and 00208 # * the effort that is applied in the joint (Nm or N). 00209 # 00210 # Each joint is uniquely identified by its name 00211 # The header specifies the time at which the joint states were recorded. All the joint states 00212 # in one message have to be recorded at the same time. 00213 # 00214 # This message consists of a multiple arrays, one for each part of the joint state. 00215 # The goal is to make each of the fields optional. When e.g. your joints have no 00216 # effort associated with them, you can leave the effort array empty. 00217 # 00218 # All arrays in this message should have the same size, or be empty. 00219 # This is the only way to uniquely associate the joint name with the correct 00220 # states. 00221 00222 00223 Header header 00224 00225 string[] name 00226 float64[] position 00227 float64[] velocity 00228 float64[] effort 00229 00230 ================================================================================ 00231 MSG: object_manipulation_msgs/GraspableObject 00232 # an object that the object_manipulator can work on 00233 00234 # a graspable object can be represented in multiple ways. This message 00235 # can contain all of them. Which one is actually used is up to the receiver 00236 # of this message. When adding new representations, one must be careful that 00237 # they have reasonable lightweight defaults indicating that that particular 00238 # representation is not available. 00239 00240 # the tf frame to be used as a reference frame when combining information from 00241 # the different representations below 00242 string reference_frame_id 00243 00244 # potential recognition results from a database of models 00245 # all poses are relative to the object reference pose 00246 household_objects_database_msgs/DatabaseModelPose[] potential_models 00247 00248 # the point cloud itself 00249 sensor_msgs/PointCloud cluster 00250 00251 # a region of a PointCloud2 of interest 00252 object_manipulation_msgs/SceneRegion region 00253 00254 # the name that this object has in the collision environment 00255 string collision_name 00256 ================================================================================ 00257 MSG: household_objects_database_msgs/DatabaseModelPose 00258 # Informs that a specific model from the Model Database has been 00259 # identified at a certain location 00260 00261 # the database id of the model 00262 int32 model_id 00263 00264 # the pose that it can be found in 00265 geometry_msgs/PoseStamped pose 00266 00267 # a measure of the confidence level in this detection result 00268 float32 confidence 00269 00270 # the name of the object detector that generated this detection result 00271 string detector_name 00272 00273 ================================================================================ 00274 MSG: sensor_msgs/PointCloud 00275 # This message holds a collection of 3d points, plus optional additional 00276 # information about each point. 00277 00278 # Time of sensor data acquisition, coordinate frame ID. 00279 Header header 00280 00281 # Array of 3d points. Each Point32 should be interpreted as a 3d point 00282 # in the frame given in the header. 00283 geometry_msgs/Point32[] points 00284 00285 # Each channel should have the same number of elements as points array, 00286 # and the data in each channel should correspond 1:1 with each point. 00287 # Channel names in common practice are listed in ChannelFloat32.msg. 00288 ChannelFloat32[] channels 00289 00290 ================================================================================ 00291 MSG: geometry_msgs/Point32 00292 # This contains the position of a point in free space(with 32 bits of precision). 00293 # It is recommeded to use Point wherever possible instead of Point32. 00294 # 00295 # This recommendation is to promote interoperability. 00296 # 00297 # This message is designed to take up less space when sending 00298 # lots of points at once, as in the case of a PointCloud. 00299 00300 float32 x 00301 float32 y 00302 float32 z 00303 ================================================================================ 00304 MSG: sensor_msgs/ChannelFloat32 00305 # This message is used by the PointCloud message to hold optional data 00306 # associated with each point in the cloud. The length of the values 00307 # array should be the same as the length of the points array in the 00308 # PointCloud, and each value should be associated with the corresponding 00309 # point. 00310 00311 # Channel names in existing practice include: 00312 # "u", "v" - row and column (respectively) in the left stereo image. 00313 # This is opposite to usual conventions but remains for 00314 # historical reasons. The newer PointCloud2 message has no 00315 # such problem. 00316 # "rgb" - For point clouds produced by color stereo cameras. uint8 00317 # (R,G,B) values packed into the least significant 24 bits, 00318 # in order. 00319 # "intensity" - laser or pixel intensity. 00320 # "distance" 00321 00322 # The channel name should give semantics of the channel (e.g. 00323 # "intensity" instead of "value"). 00324 string name 00325 00326 # The values array should be 1-1 with the elements of the associated 00327 # PointCloud. 00328 float32[] values 00329 00330 ================================================================================ 00331 MSG: object_manipulation_msgs/SceneRegion 00332 # Point cloud 00333 sensor_msgs/PointCloud2 cloud 00334 00335 # Indices for the region of interest 00336 int32[] mask 00337 00338 # One of the corresponding 2D images, if applicable 00339 sensor_msgs/Image image 00340 00341 # The disparity image, if applicable 00342 sensor_msgs/Image disparity_image 00343 00344 # Camera info for the camera that took the image 00345 sensor_msgs/CameraInfo cam_info 00346 00347 # a 3D region of interest for grasp planning 00348 geometry_msgs/PoseStamped roi_box_pose 00349 geometry_msgs/Vector3 roi_box_dims 00350 00351 ================================================================================ 00352 MSG: sensor_msgs/PointCloud2 00353 # This message holds a collection of N-dimensional points, which may 00354 # contain additional information such as normals, intensity, etc. The 00355 # point data is stored as a binary blob, its layout described by the 00356 # contents of the "fields" array. 00357 00358 # The point cloud data may be organized 2d (image-like) or 1d 00359 # (unordered). Point clouds organized as 2d images may be produced by 00360 # camera depth sensors such as stereo or time-of-flight. 00361 00362 # Time of sensor data acquisition, and the coordinate frame ID (for 3d 00363 # points). 00364 Header header 00365 00366 # 2D structure of the point cloud. If the cloud is unordered, height is 00367 # 1 and width is the length of the point cloud. 00368 uint32 height 00369 uint32 width 00370 00371 # Describes the channels and their layout in the binary data blob. 00372 PointField[] fields 00373 00374 bool is_bigendian # Is this data bigendian? 00375 uint32 point_step # Length of a point in bytes 00376 uint32 row_step # Length of a row in bytes 00377 uint8[] data # Actual point data, size is (row_step*height) 00378 00379 bool is_dense # True if there are no invalid points 00380 00381 ================================================================================ 00382 MSG: sensor_msgs/PointField 00383 # This message holds the description of one point entry in the 00384 # PointCloud2 message format. 00385 uint8 INT8 = 1 00386 uint8 UINT8 = 2 00387 uint8 INT16 = 3 00388 uint8 UINT16 = 4 00389 uint8 INT32 = 5 00390 uint8 UINT32 = 6 00391 uint8 FLOAT32 = 7 00392 uint8 FLOAT64 = 8 00393 00394 string name # Name of field 00395 uint32 offset # Offset from start of point struct 00396 uint8 datatype # Datatype enumeration, see above 00397 uint32 count # How many elements in the field 00398 00399 ================================================================================ 00400 MSG: sensor_msgs/Image 00401 # This message contains an uncompressed image 00402 # (0, 0) is at top-left corner of image 00403 # 00404 00405 Header header # Header timestamp should be acquisition time of image 00406 # Header frame_id should be optical frame of camera 00407 # origin of frame should be optical center of cameara 00408 # +x should point to the right in the image 00409 # +y should point down in the image 00410 # +z should point into to plane of the image 00411 # If the frame_id here and the frame_id of the CameraInfo 00412 # message associated with the image conflict 00413 # the behavior is undefined 00414 00415 uint32 height # image height, that is, number of rows 00416 uint32 width # image width, that is, number of columns 00417 00418 # The legal values for encoding are in file src/image_encodings.cpp 00419 # If you want to standardize a new string format, join 00420 # ros-users@lists.sourceforge.net and send an email proposing a new encoding. 00421 00422 string encoding # Encoding of pixels -- channel meaning, ordering, size 00423 # taken from the list of strings in src/image_encodings.cpp 00424 00425 uint8 is_bigendian # is this data bigendian? 00426 uint32 step # Full row length in bytes 00427 uint8[] data # actual matrix data, size is (step * rows) 00428 00429 ================================================================================ 00430 MSG: sensor_msgs/CameraInfo 00431 # This message defines meta information for a camera. It should be in a 00432 # camera namespace on topic "camera_info" and accompanied by up to five 00433 # image topics named: 00434 # 00435 # image_raw - raw data from the camera driver, possibly Bayer encoded 00436 # image - monochrome, distorted 00437 # image_color - color, distorted 00438 # image_rect - monochrome, rectified 00439 # image_rect_color - color, rectified 00440 # 00441 # The image_pipeline contains packages (image_proc, stereo_image_proc) 00442 # for producing the four processed image topics from image_raw and 00443 # camera_info. The meaning of the camera parameters are described in 00444 # detail at http://www.ros.org/wiki/image_pipeline/CameraInfo. 00445 # 00446 # The image_geometry package provides a user-friendly interface to 00447 # common operations using this meta information. If you want to, e.g., 00448 # project a 3d point into image coordinates, we strongly recommend 00449 # using image_geometry. 00450 # 00451 # If the camera is uncalibrated, the matrices D, K, R, P should be left 00452 # zeroed out. In particular, clients may assume that K[0] == 0.0 00453 # indicates an uncalibrated camera. 00454 00455 ####################################################################### 00456 # Image acquisition info # 00457 ####################################################################### 00458 00459 # Time of image acquisition, camera coordinate frame ID 00460 Header header # Header timestamp should be acquisition time of image 00461 # Header frame_id should be optical frame of camera 00462 # origin of frame should be optical center of camera 00463 # +x should point to the right in the image 00464 # +y should point down in the image 00465 # +z should point into the plane of the image 00466 00467 00468 ####################################################################### 00469 # Calibration Parameters # 00470 ####################################################################### 00471 # These are fixed during camera calibration. Their values will be the # 00472 # same in all messages until the camera is recalibrated. Note that # 00473 # self-calibrating systems may "recalibrate" frequently. # 00474 # # 00475 # The internal parameters can be used to warp a raw (distorted) image # 00476 # to: # 00477 # 1. An undistorted image (requires D and K) # 00478 # 2. A rectified image (requires D, K, R) # 00479 # The projection matrix P projects 3D points into the rectified image.# 00480 ####################################################################### 00481 00482 # The image dimensions with which the camera was calibrated. Normally 00483 # this will be the full camera resolution in pixels. 00484 uint32 height 00485 uint32 width 00486 00487 # The distortion model used. Supported models are listed in 00488 # sensor_msgs/distortion_models.h. For most cameras, "plumb_bob" - a 00489 # simple model of radial and tangential distortion - is sufficent. 00490 string distortion_model 00491 00492 # The distortion parameters, size depending on the distortion model. 00493 # For "plumb_bob", the 5 parameters are: (k1, k2, t1, t2, k3). 00494 float64[] D 00495 00496 # Intrinsic camera matrix for the raw (distorted) images. 00497 # [fx 0 cx] 00498 # K = [ 0 fy cy] 00499 # [ 0 0 1] 00500 # Projects 3D points in the camera coordinate frame to 2D pixel 00501 # coordinates using the focal lengths (fx, fy) and principal point 00502 # (cx, cy). 00503 float64[9] K # 3x3 row-major matrix 00504 00505 # Rectification matrix (stereo cameras only) 00506 # A rotation matrix aligning the camera coordinate system to the ideal 00507 # stereo image plane so that epipolar lines in both stereo images are 00508 # parallel. 00509 float64[9] R # 3x3 row-major matrix 00510 00511 # Projection/camera matrix 00512 # [fx' 0 cx' Tx] 00513 # P = [ 0 fy' cy' Ty] 00514 # [ 0 0 1 0] 00515 # By convention, this matrix specifies the intrinsic (camera) matrix 00516 # of the processed (rectified) image. That is, the left 3x3 portion 00517 # is the normal camera intrinsic matrix for the rectified image. 00518 # It projects 3D points in the camera coordinate frame to 2D pixel 00519 # coordinates using the focal lengths (fx', fy') and principal point 00520 # (cx', cy') - these may differ from the values in K. 00521 # For monocular cameras, Tx = Ty = 0. Normally, monocular cameras will 00522 # also have R = the identity and P[1:3,1:3] = K. 00523 # For a stereo pair, the fourth column [Tx Ty 0]' is related to the 00524 # position of the optical center of the second camera in the first 00525 # camera's frame. We assume Tz = 0 so both cameras are in the same 00526 # stereo image plane. The first camera always has Tx = Ty = 0. For 00527 # the right (second) camera of a horizontal stereo pair, Ty = 0 and 00528 # Tx = -fx' * B, where B is the baseline between the cameras. 00529 # Given a 3D point [X Y Z]', the projection (x, y) of the point onto 00530 # the rectified image is given by: 00531 # [u v w]' = P * [X Y Z 1]' 00532 # x = u / w 00533 # y = v / w 00534 # This holds for both images of a stereo pair. 00535 float64[12] P # 3x4 row-major matrix 00536 00537 00538 ####################################################################### 00539 # Operational Parameters # 00540 ####################################################################### 00541 # These define the image region actually captured by the camera # 00542 # driver. Although they affect the geometry of the output image, they # 00543 # may be changed freely without recalibrating the camera. # 00544 ####################################################################### 00545 00546 # Binning refers here to any camera setting which combines rectangular 00547 # neighborhoods of pixels into larger "super-pixels." It reduces the 00548 # resolution of the output image to 00549 # (width / binning_x) x (height / binning_y). 00550 # The default values binning_x = binning_y = 0 is considered the same 00551 # as binning_x = binning_y = 1 (no subsampling). 00552 uint32 binning_x 00553 uint32 binning_y 00554 00555 # Region of interest (subwindow of full camera resolution), given in 00556 # full resolution (unbinned) image coordinates. A particular ROI 00557 # always denotes the same window of pixels on the camera sensor, 00558 # regardless of binning settings. 00559 # The default setting of roi (all values 0) is considered the same as 00560 # full resolution (roi.width = width, roi.height = height). 00561 RegionOfInterest roi 00562 00563 ================================================================================ 00564 MSG: sensor_msgs/RegionOfInterest 00565 # This message is used to specify a region of interest within an image. 00566 # 00567 # When used to specify the ROI setting of the camera when the image was 00568 # taken, the height and width fields should either match the height and 00569 # width fields for the associated image; or height = width = 0 00570 # indicates that the full resolution image was captured. 00571 00572 uint32 x_offset # Leftmost pixel of the ROI 00573 # (0 if the ROI includes the left edge of the image) 00574 uint32 y_offset # Topmost pixel of the ROI 00575 # (0 if the ROI includes the top edge of the image) 00576 uint32 height # Height of ROI 00577 uint32 width # Width of ROI 00578 00579 # True if a distinct rectified ROI should be calculated from the "raw" 00580 # ROI in this message. Typically this should be False if the full image 00581 # is captured (ROI not used), and True if a subwindow is captured (ROI 00582 # used). 00583 bool do_rectify 00584 00585 ================================================================================ 00586 MSG: geometry_msgs/Vector3 00587 # This represents a vector in free space. 00588 00589 float64 x 00590 float64 y 00591 float64 z 00592 ================================================================================ 00593 MSG: object_manipulation_msgs/GripperTranslation 00594 # defines a translation for the gripper, used in pickup or place tasks 00595 # for example for lifting an object off a table or approaching the table for placing 00596 00597 # the direction of the translation 00598 geometry_msgs/Vector3Stamped direction 00599 00600 # the desired translation distance 00601 float32 desired_distance 00602 00603 # the min distance that must be considered feasible before the 00604 # grasp is even attempted 00605 float32 min_distance 00606 ================================================================================ 00607 MSG: geometry_msgs/Vector3Stamped 00608 # This represents a Vector3 with reference coordinate frame and timestamp 00609 Header header 00610 Vector3 vector 00611 00612 ================================================================================ 00613 MSG: arm_navigation_msgs/Constraints 00614 # This message contains a list of motion planning constraints. 00615 00616 arm_navigation_msgs/JointConstraint[] joint_constraints 00617 arm_navigation_msgs/PositionConstraint[] position_constraints 00618 arm_navigation_msgs/OrientationConstraint[] orientation_constraints 00619 arm_navigation_msgs/VisibilityConstraint[] visibility_constraints 00620 00621 ================================================================================ 00622 MSG: arm_navigation_msgs/JointConstraint 00623 # Constrain the position of a joint to be within a certain bound 00624 string joint_name 00625 00626 # the bound to be achieved is [position - tolerance_below, position + tolerance_above] 00627 float64 position 00628 float64 tolerance_above 00629 float64 tolerance_below 00630 00631 # A weighting factor for this constraint 00632 float64 weight 00633 ================================================================================ 00634 MSG: arm_navigation_msgs/PositionConstraint 00635 # This message contains the definition of a position constraint. 00636 Header header 00637 00638 # The robot link this constraint refers to 00639 string link_name 00640 00641 # The offset (in the link frame) for the target point on the link we are planning for 00642 geometry_msgs/Point target_point_offset 00643 00644 # The nominal/target position for the point we are planning for 00645 geometry_msgs/Point position 00646 00647 # The shape of the bounded region that constrains the position of the end-effector 00648 # This region is always centered at the position defined above 00649 arm_navigation_msgs/Shape constraint_region_shape 00650 00651 # The orientation of the bounded region that constrains the position of the end-effector. 00652 # This allows the specification of non-axis aligned constraints 00653 geometry_msgs/Quaternion constraint_region_orientation 00654 00655 # Constraint weighting factor - a weight for this constraint 00656 float64 weight 00657 00658 ================================================================================ 00659 MSG: arm_navigation_msgs/Shape 00660 byte SPHERE=0 00661 byte BOX=1 00662 byte CYLINDER=2 00663 byte MESH=3 00664 00665 byte type 00666 00667 00668 #### define sphere, box, cylinder #### 00669 # the origin of each shape is considered at the shape's center 00670 00671 # for sphere 00672 # radius := dimensions[0] 00673 00674 # for cylinder 00675 # radius := dimensions[0] 00676 # length := dimensions[1] 00677 # the length is along the Z axis 00678 00679 # for box 00680 # size_x := dimensions[0] 00681 # size_y := dimensions[1] 00682 # size_z := dimensions[2] 00683 float64[] dimensions 00684 00685 00686 #### define mesh #### 00687 00688 # list of triangles; triangle k is defined by tre vertices located 00689 # at indices triangles[3k], triangles[3k+1], triangles[3k+2] 00690 int32[] triangles 00691 geometry_msgs/Point[] vertices 00692 00693 ================================================================================ 00694 MSG: arm_navigation_msgs/OrientationConstraint 00695 # This message contains the definition of an orientation constraint. 00696 Header header 00697 00698 # The robot link this constraint refers to 00699 string link_name 00700 00701 # The type of the constraint 00702 int32 type 00703 int32 LINK_FRAME=0 00704 int32 HEADER_FRAME=1 00705 00706 # The desired orientation of the robot link specified as a quaternion 00707 geometry_msgs/Quaternion orientation 00708 00709 # optional RPY error tolerances specified if 00710 float64 absolute_roll_tolerance 00711 float64 absolute_pitch_tolerance 00712 float64 absolute_yaw_tolerance 00713 00714 # Constraint weighting factor - a weight for this constraint 00715 float64 weight 00716 00717 ================================================================================ 00718 MSG: arm_navigation_msgs/VisibilityConstraint 00719 # This message contains the definition of a visibility constraint. 00720 Header header 00721 00722 # The point stamped target that needs to be kept within view of the sensor 00723 geometry_msgs/PointStamped target 00724 00725 # The local pose of the frame in which visibility is to be maintained 00726 # The frame id should represent the robot link to which the sensor is attached 00727 # The visual axis of the sensor is assumed to be along the X axis of this frame 00728 geometry_msgs/PoseStamped sensor_pose 00729 00730 # The deviation (in radians) that will be tolerated 00731 # Constraint error will be measured as the solid angle between the 00732 # X axis of the frame defined above and the vector between the origin 00733 # of the frame defined above and the target location 00734 float64 absolute_tolerance 00735 00736 00737 ================================================================================ 00738 MSG: geometry_msgs/PointStamped 00739 # This represents a Point with reference coordinate frame and timestamp 00740 Header header 00741 Point point 00742 00743 ================================================================================ 00744 MSG: arm_navigation_msgs/OrderedCollisionOperations 00745 # A set of collision operations that will be performed in the order they are specified 00746 CollisionOperation[] collision_operations 00747 ================================================================================ 00748 MSG: arm_navigation_msgs/CollisionOperation 00749 # A definition of a collision operation 00750 # E.g. ("gripper",COLLISION_SET_ALL,ENABLE) will enable collisions 00751 # between the gripper and all objects in the collision space 00752 00753 string object1 00754 string object2 00755 string COLLISION_SET_ALL="all" 00756 string COLLISION_SET_OBJECTS="objects" 00757 string COLLISION_SET_ATTACHED_OBJECTS="attached" 00758 00759 # The penetration distance to which collisions are allowed. This is 0.0 by default. 00760 float64 penetration_distance 00761 00762 # Flag that determines whether collisions will be enabled or disabled for the pair of objects specified above 00763 int32 operation 00764 int32 DISABLE=0 00765 int32 ENABLE=1 00766 00767 ================================================================================ 00768 MSG: arm_navigation_msgs/LinkPadding 00769 #name for the link 00770 string link_name 00771 00772 # padding to apply to the link 00773 float64 padding 00774 00775 ================================================================================ 00776 MSG: object_manipulation_msgs/PlaceActionResult 00777 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ====== 00778 00779 Header header 00780 actionlib_msgs/GoalStatus status 00781 PlaceResult result 00782 00783 ================================================================================ 00784 MSG: actionlib_msgs/GoalStatus 00785 GoalID goal_id 00786 uint8 status 00787 uint8 PENDING = 0 # The goal has yet to be processed by the action server 00788 uint8 ACTIVE = 1 # The goal is currently being processed by the action server 00789 uint8 PREEMPTED = 2 # The goal received a cancel request after it started executing 00790 # and has since completed its execution (Terminal State) 00791 uint8 SUCCEEDED = 3 # The goal was achieved successfully by the action server (Terminal State) 00792 uint8 ABORTED = 4 # The goal was aborted during execution by the action server due 00793 # to some failure (Terminal State) 00794 uint8 REJECTED = 5 # The goal was rejected by the action server without being processed, 00795 # because the goal was unattainable or invalid (Terminal State) 00796 uint8 PREEMPTING = 6 # The goal received a cancel request after it started executing 00797 # and has not yet completed execution 00798 uint8 RECALLING = 7 # The goal received a cancel request before it started executing, 00799 # but the action server has not yet confirmed that the goal is canceled 00800 uint8 RECALLED = 8 # The goal received a cancel request before it started executing 00801 # and was successfully cancelled (Terminal State) 00802 uint8 LOST = 9 # An action client can determine that a goal is LOST. This should not be 00803 # sent over the wire by an action server 00804 00805 #Allow for the user to associate a string with GoalStatus for debugging 00806 string text 00807 00808 00809 ================================================================================ 00810 MSG: object_manipulation_msgs/PlaceResult 00811 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ====== 00812 00813 # The result of the pickup attempt 00814 ManipulationResult manipulation_result 00815 00816 # The successful place location, if any 00817 geometry_msgs/PoseStamped place_location 00818 00819 # the list of attempted locations, in the order in which they were attempted 00820 # the successful one should be the last one in this list 00821 geometry_msgs/PoseStamped[] attempted_locations 00822 00823 # the outcomes of the attempted locations, in the same order as attempted_locations 00824 PlaceLocationResult[] attempted_location_results 00825 00826 00827 ================================================================================ 00828 MSG: object_manipulation_msgs/ManipulationResult 00829 # Result codes for manipulation tasks 00830 00831 # task completed as expected 00832 # generally means you can proceed as planned 00833 int32 SUCCESS = 1 00834 00835 # task not possible (e.g. out of reach or obstacles in the way) 00836 # generally means that the world was not disturbed, so you can try another task 00837 int32 UNFEASIBLE = -1 00838 00839 # task was thought possible, but failed due to unexpected events during execution 00840 # it is likely that the world was disturbed, so you are encouraged to refresh 00841 # your sensed world model before proceeding to another task 00842 int32 FAILED = -2 00843 00844 # a lower level error prevented task completion (e.g. joint controller not responding) 00845 # generally requires human attention 00846 int32 ERROR = -3 00847 00848 # means that at some point during execution we ended up in a state that the collision-aware 00849 # arm navigation module will not move out of. The world was likely not disturbed, but you 00850 # probably need a new collision map to move the arm out of the stuck position 00851 int32 ARM_MOVEMENT_PREVENTED = -4 00852 00853 # specific to grasp actions 00854 # the object was grasped successfully, but the lift attempt could not achieve the minimum lift distance requested 00855 # it is likely that the collision environment will see collisions between the hand/object and the support surface 00856 int32 LIFT_FAILED = -5 00857 00858 # specific to place actions 00859 # the object was placed successfully, but the retreat attempt could not achieve the minimum retreat distance requested 00860 # it is likely that the collision environment will see collisions between the hand and the object 00861 int32 RETREAT_FAILED = -6 00862 00863 # indicates that somewhere along the line a human said "wait, stop, this is bad, go back and do something else" 00864 int32 CANCELLED = -7 00865 00866 # the actual value of this error code 00867 int32 value 00868 00869 ================================================================================ 00870 MSG: object_manipulation_msgs/PlaceLocationResult 00871 int32 SUCCESS = 1 00872 int32 PLACE_OUT_OF_REACH = 2 00873 int32 PLACE_IN_COLLISION = 3 00874 int32 PLACE_UNFEASIBLE = 4 00875 int32 PREPLACE_OUT_OF_REACH = 5 00876 int32 PREPLACE_IN_COLLISION = 6 00877 int32 PREPLACE_UNFEASIBLE = 7 00878 int32 RETREAT_OUT_OF_REACH = 8 00879 int32 RETREAT_IN_COLLISION = 9 00880 int32 RETREAT_UNFEASIBLE = 10 00881 int32 MOVE_ARM_FAILED = 11 00882 int32 PLACE_FAILED = 12 00883 int32 RETREAT_FAILED = 13 00884 int32 result_code 00885 00886 # whether the state of the world was disturbed by this attempt. generally, this flag 00887 # shows if another task can be attempted, or a new sensed world model is recommeded 00888 # before proceeding 00889 bool continuation_possible 00890 00891 ================================================================================ 00892 MSG: object_manipulation_msgs/PlaceActionFeedback 00893 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ====== 00894 00895 Header header 00896 actionlib_msgs/GoalStatus status 00897 PlaceFeedback feedback 00898 00899 ================================================================================ 00900 MSG: object_manipulation_msgs/PlaceFeedback 00901 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ====== 00902 00903 # The number of the place location currently being attempted 00904 int32 current_location 00905 00906 # The total number of locations that will be attempted 00907 int32 total_locations 00908 00909 00910 """ 00911 __slots__ = ['action_goal','action_result','action_feedback'] 00912 _slot_types = ['object_manipulation_msgs/PlaceActionGoal','object_manipulation_msgs/PlaceActionResult','object_manipulation_msgs/PlaceActionFeedback'] 00913 00914 def __init__(self, *args, **kwds): 00915 """ 00916 Constructor. Any message fields that are implicitly/explicitly 00917 set to None will be assigned a default value. The recommend 00918 use is keyword arguments as this is more robust to future message 00919 changes. You cannot mix in-order arguments and keyword arguments. 00920 00921 The available fields are: 00922 action_goal,action_result,action_feedback 00923 00924 @param args: complete set of field values, in .msg order 00925 @param kwds: use keyword arguments corresponding to message field names 00926 to set specific fields. 00927 """ 00928 if args or kwds: 00929 super(PlaceAction, self).__init__(*args, **kwds) 00930 #message fields cannot be None, assign default values for those that are 00931 if self.action_goal is None: 00932 self.action_goal = object_manipulation_msgs.msg.PlaceActionGoal() 00933 if self.action_result is None: 00934 self.action_result = object_manipulation_msgs.msg.PlaceActionResult() 00935 if self.action_feedback is None: 00936 self.action_feedback = object_manipulation_msgs.msg.PlaceActionFeedback() 00937 else: 00938 self.action_goal = object_manipulation_msgs.msg.PlaceActionGoal() 00939 self.action_result = object_manipulation_msgs.msg.PlaceActionResult() 00940 self.action_feedback = object_manipulation_msgs.msg.PlaceActionFeedback() 00941 00942 def _get_types(self): 00943 """ 00944 internal API method 00945 """ 00946 return self._slot_types 00947 00948 def serialize(self, buff): 00949 """ 00950 serialize message into buffer 00951 @param buff: buffer 00952 @type buff: StringIO 00953 """ 00954 try: 00955 _x = self 00956 buff.write(_struct_3I.pack(_x.action_goal.header.seq, _x.action_goal.header.stamp.secs, _x.action_goal.header.stamp.nsecs)) 00957 _x = self.action_goal.header.frame_id 00958 length = len(_x) 00959 buff.write(struct.pack('<I%ss'%length, length, _x)) 00960 _x = self 00961 buff.write(_struct_2I.pack(_x.action_goal.goal_id.stamp.secs, _x.action_goal.goal_id.stamp.nsecs)) 00962 _x = self.action_goal.goal_id.id 00963 length = len(_x) 00964 buff.write(struct.pack('<I%ss'%length, length, _x)) 00965 _x = self.action_goal.goal.arm_name 00966 length = len(_x) 00967 buff.write(struct.pack('<I%ss'%length, length, _x)) 00968 length = len(self.action_goal.goal.place_locations) 00969 buff.write(_struct_I.pack(length)) 00970 for val1 in self.action_goal.goal.place_locations: 00971 _v1 = val1.header 00972 buff.write(_struct_I.pack(_v1.seq)) 00973 _v2 = _v1.stamp 00974 _x = _v2 00975 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00976 _x = _v1.frame_id 00977 length = len(_x) 00978 buff.write(struct.pack('<I%ss'%length, length, _x)) 00979 _v3 = val1.pose 00980 _v4 = _v3.position 00981 _x = _v4 00982 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00983 _v5 = _v3.orientation 00984 _x = _v5 00985 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 00986 _x = self 00987 buff.write(_struct_3I.pack(_x.action_goal.goal.grasp.pre_grasp_posture.header.seq, _x.action_goal.goal.grasp.pre_grasp_posture.header.stamp.secs, _x.action_goal.goal.grasp.pre_grasp_posture.header.stamp.nsecs)) 00988 _x = self.action_goal.goal.grasp.pre_grasp_posture.header.frame_id 00989 length = len(_x) 00990 buff.write(struct.pack('<I%ss'%length, length, _x)) 00991 length = len(self.action_goal.goal.grasp.pre_grasp_posture.name) 00992 buff.write(_struct_I.pack(length)) 00993 for val1 in self.action_goal.goal.grasp.pre_grasp_posture.name: 00994 length = len(val1) 00995 buff.write(struct.pack('<I%ss'%length, length, val1)) 00996 length = len(self.action_goal.goal.grasp.pre_grasp_posture.position) 00997 buff.write(_struct_I.pack(length)) 00998 pattern = '<%sd'%length 00999 buff.write(struct.pack(pattern, *self.action_goal.goal.grasp.pre_grasp_posture.position)) 01000 length = len(self.action_goal.goal.grasp.pre_grasp_posture.velocity) 01001 buff.write(_struct_I.pack(length)) 01002 pattern = '<%sd'%length 01003 buff.write(struct.pack(pattern, *self.action_goal.goal.grasp.pre_grasp_posture.velocity)) 01004 length = len(self.action_goal.goal.grasp.pre_grasp_posture.effort) 01005 buff.write(_struct_I.pack(length)) 01006 pattern = '<%sd'%length 01007 buff.write(struct.pack(pattern, *self.action_goal.goal.grasp.pre_grasp_posture.effort)) 01008 _x = self 01009 buff.write(_struct_3I.pack(_x.action_goal.goal.grasp.grasp_posture.header.seq, _x.action_goal.goal.grasp.grasp_posture.header.stamp.secs, _x.action_goal.goal.grasp.grasp_posture.header.stamp.nsecs)) 01010 _x = self.action_goal.goal.grasp.grasp_posture.header.frame_id 01011 length = len(_x) 01012 buff.write(struct.pack('<I%ss'%length, length, _x)) 01013 length = len(self.action_goal.goal.grasp.grasp_posture.name) 01014 buff.write(_struct_I.pack(length)) 01015 for val1 in self.action_goal.goal.grasp.grasp_posture.name: 01016 length = len(val1) 01017 buff.write(struct.pack('<I%ss'%length, length, val1)) 01018 length = len(self.action_goal.goal.grasp.grasp_posture.position) 01019 buff.write(_struct_I.pack(length)) 01020 pattern = '<%sd'%length 01021 buff.write(struct.pack(pattern, *self.action_goal.goal.grasp.grasp_posture.position)) 01022 length = len(self.action_goal.goal.grasp.grasp_posture.velocity) 01023 buff.write(_struct_I.pack(length)) 01024 pattern = '<%sd'%length 01025 buff.write(struct.pack(pattern, *self.action_goal.goal.grasp.grasp_posture.velocity)) 01026 length = len(self.action_goal.goal.grasp.grasp_posture.effort) 01027 buff.write(_struct_I.pack(length)) 01028 pattern = '<%sd'%length 01029 buff.write(struct.pack(pattern, *self.action_goal.goal.grasp.grasp_posture.effort)) 01030 _x = self 01031 buff.write(_struct_8dB2f.pack(_x.action_goal.goal.grasp.grasp_pose.position.x, _x.action_goal.goal.grasp.grasp_pose.position.y, _x.action_goal.goal.grasp.grasp_pose.position.z, _x.action_goal.goal.grasp.grasp_pose.orientation.x, _x.action_goal.goal.grasp.grasp_pose.orientation.y, _x.action_goal.goal.grasp.grasp_pose.orientation.z, _x.action_goal.goal.grasp.grasp_pose.orientation.w, _x.action_goal.goal.grasp.success_probability, _x.action_goal.goal.grasp.cluster_rep, _x.action_goal.goal.grasp.desired_approach_distance, _x.action_goal.goal.grasp.min_approach_distance)) 01032 length = len(self.action_goal.goal.grasp.moved_obstacles) 01033 buff.write(_struct_I.pack(length)) 01034 for val1 in self.action_goal.goal.grasp.moved_obstacles: 01035 _x = val1.reference_frame_id 01036 length = len(_x) 01037 buff.write(struct.pack('<I%ss'%length, length, _x)) 01038 length = len(val1.potential_models) 01039 buff.write(_struct_I.pack(length)) 01040 for val2 in val1.potential_models: 01041 buff.write(_struct_i.pack(val2.model_id)) 01042 _v6 = val2.pose 01043 _v7 = _v6.header 01044 buff.write(_struct_I.pack(_v7.seq)) 01045 _v8 = _v7.stamp 01046 _x = _v8 01047 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01048 _x = _v7.frame_id 01049 length = len(_x) 01050 buff.write(struct.pack('<I%ss'%length, length, _x)) 01051 _v9 = _v6.pose 01052 _v10 = _v9.position 01053 _x = _v10 01054 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 01055 _v11 = _v9.orientation 01056 _x = _v11 01057 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 01058 buff.write(_struct_f.pack(val2.confidence)) 01059 _x = val2.detector_name 01060 length = len(_x) 01061 buff.write(struct.pack('<I%ss'%length, length, _x)) 01062 _v12 = val1.cluster 01063 _v13 = _v12.header 01064 buff.write(_struct_I.pack(_v13.seq)) 01065 _v14 = _v13.stamp 01066 _x = _v14 01067 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01068 _x = _v13.frame_id 01069 length = len(_x) 01070 buff.write(struct.pack('<I%ss'%length, length, _x)) 01071 length = len(_v12.points) 01072 buff.write(_struct_I.pack(length)) 01073 for val3 in _v12.points: 01074 _x = val3 01075 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z)) 01076 length = len(_v12.channels) 01077 buff.write(_struct_I.pack(length)) 01078 for val3 in _v12.channels: 01079 _x = val3.name 01080 length = len(_x) 01081 buff.write(struct.pack('<I%ss'%length, length, _x)) 01082 length = len(val3.values) 01083 buff.write(_struct_I.pack(length)) 01084 pattern = '<%sf'%length 01085 buff.write(struct.pack(pattern, *val3.values)) 01086 _v15 = val1.region 01087 _v16 = _v15.cloud 01088 _v17 = _v16.header 01089 buff.write(_struct_I.pack(_v17.seq)) 01090 _v18 = _v17.stamp 01091 _x = _v18 01092 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01093 _x = _v17.frame_id 01094 length = len(_x) 01095 buff.write(struct.pack('<I%ss'%length, length, _x)) 01096 _x = _v16 01097 buff.write(_struct_2I.pack(_x.height, _x.width)) 01098 length = len(_v16.fields) 01099 buff.write(_struct_I.pack(length)) 01100 for val4 in _v16.fields: 01101 _x = val4.name 01102 length = len(_x) 01103 buff.write(struct.pack('<I%ss'%length, length, _x)) 01104 _x = val4 01105 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count)) 01106 _x = _v16 01107 buff.write(_struct_B2I.pack(_x.is_bigendian, _x.point_step, _x.row_step)) 01108 _x = _v16.data 01109 length = len(_x) 01110 # - if encoded as a list instead, serialize as bytes instead of string 01111 if type(_x) in [list, tuple]: 01112 buff.write(struct.pack('<I%sB'%length, length, *_x)) 01113 else: 01114 buff.write(struct.pack('<I%ss'%length, length, _x)) 01115 buff.write(_struct_B.pack(_v16.is_dense)) 01116 length = len(_v15.mask) 01117 buff.write(_struct_I.pack(length)) 01118 pattern = '<%si'%length 01119 buff.write(struct.pack(pattern, *_v15.mask)) 01120 _v19 = _v15.image 01121 _v20 = _v19.header 01122 buff.write(_struct_I.pack(_v20.seq)) 01123 _v21 = _v20.stamp 01124 _x = _v21 01125 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01126 _x = _v20.frame_id 01127 length = len(_x) 01128 buff.write(struct.pack('<I%ss'%length, length, _x)) 01129 _x = _v19 01130 buff.write(_struct_2I.pack(_x.height, _x.width)) 01131 _x = _v19.encoding 01132 length = len(_x) 01133 buff.write(struct.pack('<I%ss'%length, length, _x)) 01134 _x = _v19 01135 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step)) 01136 _x = _v19.data 01137 length = len(_x) 01138 # - if encoded as a list instead, serialize as bytes instead of string 01139 if type(_x) in [list, tuple]: 01140 buff.write(struct.pack('<I%sB'%length, length, *_x)) 01141 else: 01142 buff.write(struct.pack('<I%ss'%length, length, _x)) 01143 _v22 = _v15.disparity_image 01144 _v23 = _v22.header 01145 buff.write(_struct_I.pack(_v23.seq)) 01146 _v24 = _v23.stamp 01147 _x = _v24 01148 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01149 _x = _v23.frame_id 01150 length = len(_x) 01151 buff.write(struct.pack('<I%ss'%length, length, _x)) 01152 _x = _v22 01153 buff.write(_struct_2I.pack(_x.height, _x.width)) 01154 _x = _v22.encoding 01155 length = len(_x) 01156 buff.write(struct.pack('<I%ss'%length, length, _x)) 01157 _x = _v22 01158 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step)) 01159 _x = _v22.data 01160 length = len(_x) 01161 # - if encoded as a list instead, serialize as bytes instead of string 01162 if type(_x) in [list, tuple]: 01163 buff.write(struct.pack('<I%sB'%length, length, *_x)) 01164 else: 01165 buff.write(struct.pack('<I%ss'%length, length, _x)) 01166 _v25 = _v15.cam_info 01167 _v26 = _v25.header 01168 buff.write(_struct_I.pack(_v26.seq)) 01169 _v27 = _v26.stamp 01170 _x = _v27 01171 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01172 _x = _v26.frame_id 01173 length = len(_x) 01174 buff.write(struct.pack('<I%ss'%length, length, _x)) 01175 _x = _v25 01176 buff.write(_struct_2I.pack(_x.height, _x.width)) 01177 _x = _v25.distortion_model 01178 length = len(_x) 01179 buff.write(struct.pack('<I%ss'%length, length, _x)) 01180 length = len(_v25.D) 01181 buff.write(_struct_I.pack(length)) 01182 pattern = '<%sd'%length 01183 buff.write(struct.pack(pattern, *_v25.D)) 01184 buff.write(_struct_9d.pack(*_v25.K)) 01185 buff.write(_struct_9d.pack(*_v25.R)) 01186 buff.write(_struct_12d.pack(*_v25.P)) 01187 _x = _v25 01188 buff.write(_struct_2I.pack(_x.binning_x, _x.binning_y)) 01189 _v28 = _v25.roi 01190 _x = _v28 01191 buff.write(_struct_4IB.pack(_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify)) 01192 _v29 = _v15.roi_box_pose 01193 _v30 = _v29.header 01194 buff.write(_struct_I.pack(_v30.seq)) 01195 _v31 = _v30.stamp 01196 _x = _v31 01197 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01198 _x = _v30.frame_id 01199 length = len(_x) 01200 buff.write(struct.pack('<I%ss'%length, length, _x)) 01201 _v32 = _v29.pose 01202 _v33 = _v32.position 01203 _x = _v33 01204 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 01205 _v34 = _v32.orientation 01206 _x = _v34 01207 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 01208 _v35 = _v15.roi_box_dims 01209 _x = _v35 01210 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 01211 _x = val1.collision_name 01212 length = len(_x) 01213 buff.write(struct.pack('<I%ss'%length, length, _x)) 01214 _x = self 01215 buff.write(_struct_2f3I.pack(_x.action_goal.goal.desired_retreat_distance, _x.action_goal.goal.min_retreat_distance, _x.action_goal.goal.approach.direction.header.seq, _x.action_goal.goal.approach.direction.header.stamp.secs, _x.action_goal.goal.approach.direction.header.stamp.nsecs)) 01216 _x = self.action_goal.goal.approach.direction.header.frame_id 01217 length = len(_x) 01218 buff.write(struct.pack('<I%ss'%length, length, _x)) 01219 _x = self 01220 buff.write(_struct_3d2f.pack(_x.action_goal.goal.approach.direction.vector.x, _x.action_goal.goal.approach.direction.vector.y, _x.action_goal.goal.approach.direction.vector.z, _x.action_goal.goal.approach.desired_distance, _x.action_goal.goal.approach.min_distance)) 01221 _x = self.action_goal.goal.collision_object_name 01222 length = len(_x) 01223 buff.write(struct.pack('<I%ss'%length, length, _x)) 01224 _x = self.action_goal.goal.collision_support_surface_name 01225 length = len(_x) 01226 buff.write(struct.pack('<I%ss'%length, length, _x)) 01227 _x = self 01228 buff.write(_struct_2BdB.pack(_x.action_goal.goal.allow_gripper_support_collision, _x.action_goal.goal.use_reactive_place, _x.action_goal.goal.place_padding, _x.action_goal.goal.only_perform_feasibility_test)) 01229 length = len(self.action_goal.goal.path_constraints.joint_constraints) 01230 buff.write(_struct_I.pack(length)) 01231 for val1 in self.action_goal.goal.path_constraints.joint_constraints: 01232 _x = val1.joint_name 01233 length = len(_x) 01234 buff.write(struct.pack('<I%ss'%length, length, _x)) 01235 _x = val1 01236 buff.write(_struct_4d.pack(_x.position, _x.tolerance_above, _x.tolerance_below, _x.weight)) 01237 length = len(self.action_goal.goal.path_constraints.position_constraints) 01238 buff.write(_struct_I.pack(length)) 01239 for val1 in self.action_goal.goal.path_constraints.position_constraints: 01240 _v36 = val1.header 01241 buff.write(_struct_I.pack(_v36.seq)) 01242 _v37 = _v36.stamp 01243 _x = _v37 01244 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01245 _x = _v36.frame_id 01246 length = len(_x) 01247 buff.write(struct.pack('<I%ss'%length, length, _x)) 01248 _x = val1.link_name 01249 length = len(_x) 01250 buff.write(struct.pack('<I%ss'%length, length, _x)) 01251 _v38 = val1.target_point_offset 01252 _x = _v38 01253 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 01254 _v39 = val1.position 01255 _x = _v39 01256 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 01257 _v40 = val1.constraint_region_shape 01258 buff.write(_struct_b.pack(_v40.type)) 01259 length = len(_v40.dimensions) 01260 buff.write(_struct_I.pack(length)) 01261 pattern = '<%sd'%length 01262 buff.write(struct.pack(pattern, *_v40.dimensions)) 01263 length = len(_v40.triangles) 01264 buff.write(_struct_I.pack(length)) 01265 pattern = '<%si'%length 01266 buff.write(struct.pack(pattern, *_v40.triangles)) 01267 length = len(_v40.vertices) 01268 buff.write(_struct_I.pack(length)) 01269 for val3 in _v40.vertices: 01270 _x = val3 01271 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 01272 _v41 = val1.constraint_region_orientation 01273 _x = _v41 01274 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 01275 buff.write(_struct_d.pack(val1.weight)) 01276 length = len(self.action_goal.goal.path_constraints.orientation_constraints) 01277 buff.write(_struct_I.pack(length)) 01278 for val1 in self.action_goal.goal.path_constraints.orientation_constraints: 01279 _v42 = val1.header 01280 buff.write(_struct_I.pack(_v42.seq)) 01281 _v43 = _v42.stamp 01282 _x = _v43 01283 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01284 _x = _v42.frame_id 01285 length = len(_x) 01286 buff.write(struct.pack('<I%ss'%length, length, _x)) 01287 _x = val1.link_name 01288 length = len(_x) 01289 buff.write(struct.pack('<I%ss'%length, length, _x)) 01290 buff.write(_struct_i.pack(val1.type)) 01291 _v44 = val1.orientation 01292 _x = _v44 01293 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 01294 _x = val1 01295 buff.write(_struct_4d.pack(_x.absolute_roll_tolerance, _x.absolute_pitch_tolerance, _x.absolute_yaw_tolerance, _x.weight)) 01296 length = len(self.action_goal.goal.path_constraints.visibility_constraints) 01297 buff.write(_struct_I.pack(length)) 01298 for val1 in self.action_goal.goal.path_constraints.visibility_constraints: 01299 _v45 = val1.header 01300 buff.write(_struct_I.pack(_v45.seq)) 01301 _v46 = _v45.stamp 01302 _x = _v46 01303 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01304 _x = _v45.frame_id 01305 length = len(_x) 01306 buff.write(struct.pack('<I%ss'%length, length, _x)) 01307 _v47 = val1.target 01308 _v48 = _v47.header 01309 buff.write(_struct_I.pack(_v48.seq)) 01310 _v49 = _v48.stamp 01311 _x = _v49 01312 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01313 _x = _v48.frame_id 01314 length = len(_x) 01315 buff.write(struct.pack('<I%ss'%length, length, _x)) 01316 _v50 = _v47.point 01317 _x = _v50 01318 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 01319 _v51 = val1.sensor_pose 01320 _v52 = _v51.header 01321 buff.write(_struct_I.pack(_v52.seq)) 01322 _v53 = _v52.stamp 01323 _x = _v53 01324 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01325 _x = _v52.frame_id 01326 length = len(_x) 01327 buff.write(struct.pack('<I%ss'%length, length, _x)) 01328 _v54 = _v51.pose 01329 _v55 = _v54.position 01330 _x = _v55 01331 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 01332 _v56 = _v54.orientation 01333 _x = _v56 01334 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 01335 buff.write(_struct_d.pack(val1.absolute_tolerance)) 01336 length = len(self.action_goal.goal.additional_collision_operations.collision_operations) 01337 buff.write(_struct_I.pack(length)) 01338 for val1 in self.action_goal.goal.additional_collision_operations.collision_operations: 01339 _x = val1.object1 01340 length = len(_x) 01341 buff.write(struct.pack('<I%ss'%length, length, _x)) 01342 _x = val1.object2 01343 length = len(_x) 01344 buff.write(struct.pack('<I%ss'%length, length, _x)) 01345 _x = val1 01346 buff.write(_struct_di.pack(_x.penetration_distance, _x.operation)) 01347 length = len(self.action_goal.goal.additional_link_padding) 01348 buff.write(_struct_I.pack(length)) 01349 for val1 in self.action_goal.goal.additional_link_padding: 01350 _x = val1.link_name 01351 length = len(_x) 01352 buff.write(struct.pack('<I%ss'%length, length, _x)) 01353 buff.write(_struct_d.pack(val1.padding)) 01354 _x = self 01355 buff.write(_struct_3I.pack(_x.action_result.header.seq, _x.action_result.header.stamp.secs, _x.action_result.header.stamp.nsecs)) 01356 _x = self.action_result.header.frame_id 01357 length = len(_x) 01358 buff.write(struct.pack('<I%ss'%length, length, _x)) 01359 _x = self 01360 buff.write(_struct_2I.pack(_x.action_result.status.goal_id.stamp.secs, _x.action_result.status.goal_id.stamp.nsecs)) 01361 _x = self.action_result.status.goal_id.id 01362 length = len(_x) 01363 buff.write(struct.pack('<I%ss'%length, length, _x)) 01364 buff.write(_struct_B.pack(self.action_result.status.status)) 01365 _x = self.action_result.status.text 01366 length = len(_x) 01367 buff.write(struct.pack('<I%ss'%length, length, _x)) 01368 _x = self 01369 buff.write(_struct_i3I.pack(_x.action_result.result.manipulation_result.value, _x.action_result.result.place_location.header.seq, _x.action_result.result.place_location.header.stamp.secs, _x.action_result.result.place_location.header.stamp.nsecs)) 01370 _x = self.action_result.result.place_location.header.frame_id 01371 length = len(_x) 01372 buff.write(struct.pack('<I%ss'%length, length, _x)) 01373 _x = self 01374 buff.write(_struct_7d.pack(_x.action_result.result.place_location.pose.position.x, _x.action_result.result.place_location.pose.position.y, _x.action_result.result.place_location.pose.position.z, _x.action_result.result.place_location.pose.orientation.x, _x.action_result.result.place_location.pose.orientation.y, _x.action_result.result.place_location.pose.orientation.z, _x.action_result.result.place_location.pose.orientation.w)) 01375 length = len(self.action_result.result.attempted_locations) 01376 buff.write(_struct_I.pack(length)) 01377 for val1 in self.action_result.result.attempted_locations: 01378 _v57 = val1.header 01379 buff.write(_struct_I.pack(_v57.seq)) 01380 _v58 = _v57.stamp 01381 _x = _v58 01382 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01383 _x = _v57.frame_id 01384 length = len(_x) 01385 buff.write(struct.pack('<I%ss'%length, length, _x)) 01386 _v59 = val1.pose 01387 _v60 = _v59.position 01388 _x = _v60 01389 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 01390 _v61 = _v59.orientation 01391 _x = _v61 01392 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 01393 length = len(self.action_result.result.attempted_location_results) 01394 buff.write(_struct_I.pack(length)) 01395 for val1 in self.action_result.result.attempted_location_results: 01396 _x = val1 01397 buff.write(_struct_iB.pack(_x.result_code, _x.continuation_possible)) 01398 _x = self 01399 buff.write(_struct_3I.pack(_x.action_feedback.header.seq, _x.action_feedback.header.stamp.secs, _x.action_feedback.header.stamp.nsecs)) 01400 _x = self.action_feedback.header.frame_id 01401 length = len(_x) 01402 buff.write(struct.pack('<I%ss'%length, length, _x)) 01403 _x = self 01404 buff.write(_struct_2I.pack(_x.action_feedback.status.goal_id.stamp.secs, _x.action_feedback.status.goal_id.stamp.nsecs)) 01405 _x = self.action_feedback.status.goal_id.id 01406 length = len(_x) 01407 buff.write(struct.pack('<I%ss'%length, length, _x)) 01408 buff.write(_struct_B.pack(self.action_feedback.status.status)) 01409 _x = self.action_feedback.status.text 01410 length = len(_x) 01411 buff.write(struct.pack('<I%ss'%length, length, _x)) 01412 _x = self 01413 buff.write(_struct_2i.pack(_x.action_feedback.feedback.current_location, _x.action_feedback.feedback.total_locations)) 01414 except struct.error as se: self._check_types(se) 01415 except TypeError as te: self._check_types(te) 01416 01417 def deserialize(self, str): 01418 """ 01419 unpack serialized message in str into this message instance 01420 @param str: byte array of serialized message 01421 @type str: str 01422 """ 01423 try: 01424 if self.action_goal is None: 01425 self.action_goal = object_manipulation_msgs.msg.PlaceActionGoal() 01426 if self.action_result is None: 01427 self.action_result = object_manipulation_msgs.msg.PlaceActionResult() 01428 if self.action_feedback is None: 01429 self.action_feedback = object_manipulation_msgs.msg.PlaceActionFeedback() 01430 end = 0 01431 _x = self 01432 start = end 01433 end += 12 01434 (_x.action_goal.header.seq, _x.action_goal.header.stamp.secs, _x.action_goal.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 01435 start = end 01436 end += 4 01437 (length,) = _struct_I.unpack(str[start:end]) 01438 start = end 01439 end += length 01440 self.action_goal.header.frame_id = str[start:end] 01441 _x = self 01442 start = end 01443 end += 8 01444 (_x.action_goal.goal_id.stamp.secs, _x.action_goal.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end]) 01445 start = end 01446 end += 4 01447 (length,) = _struct_I.unpack(str[start:end]) 01448 start = end 01449 end += length 01450 self.action_goal.goal_id.id = str[start:end] 01451 start = end 01452 end += 4 01453 (length,) = _struct_I.unpack(str[start:end]) 01454 start = end 01455 end += length 01456 self.action_goal.goal.arm_name = str[start:end] 01457 start = end 01458 end += 4 01459 (length,) = _struct_I.unpack(str[start:end]) 01460 self.action_goal.goal.place_locations = [] 01461 for i in range(0, length): 01462 val1 = geometry_msgs.msg.PoseStamped() 01463 _v62 = val1.header 01464 start = end 01465 end += 4 01466 (_v62.seq,) = _struct_I.unpack(str[start:end]) 01467 _v63 = _v62.stamp 01468 _x = _v63 01469 start = end 01470 end += 8 01471 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01472 start = end 01473 end += 4 01474 (length,) = _struct_I.unpack(str[start:end]) 01475 start = end 01476 end += length 01477 _v62.frame_id = str[start:end] 01478 _v64 = val1.pose 01479 _v65 = _v64.position 01480 _x = _v65 01481 start = end 01482 end += 24 01483 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 01484 _v66 = _v64.orientation 01485 _x = _v66 01486 start = end 01487 end += 32 01488 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 01489 self.action_goal.goal.place_locations.append(val1) 01490 _x = self 01491 start = end 01492 end += 12 01493 (_x.action_goal.goal.grasp.pre_grasp_posture.header.seq, _x.action_goal.goal.grasp.pre_grasp_posture.header.stamp.secs, _x.action_goal.goal.grasp.pre_grasp_posture.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 01494 start = end 01495 end += 4 01496 (length,) = _struct_I.unpack(str[start:end]) 01497 start = end 01498 end += length 01499 self.action_goal.goal.grasp.pre_grasp_posture.header.frame_id = str[start:end] 01500 start = end 01501 end += 4 01502 (length,) = _struct_I.unpack(str[start:end]) 01503 self.action_goal.goal.grasp.pre_grasp_posture.name = [] 01504 for i in range(0, length): 01505 start = end 01506 end += 4 01507 (length,) = _struct_I.unpack(str[start:end]) 01508 start = end 01509 end += length 01510 val1 = str[start:end] 01511 self.action_goal.goal.grasp.pre_grasp_posture.name.append(val1) 01512 start = end 01513 end += 4 01514 (length,) = _struct_I.unpack(str[start:end]) 01515 pattern = '<%sd'%length 01516 start = end 01517 end += struct.calcsize(pattern) 01518 self.action_goal.goal.grasp.pre_grasp_posture.position = struct.unpack(pattern, str[start:end]) 01519 start = end 01520 end += 4 01521 (length,) = _struct_I.unpack(str[start:end]) 01522 pattern = '<%sd'%length 01523 start = end 01524 end += struct.calcsize(pattern) 01525 self.action_goal.goal.grasp.pre_grasp_posture.velocity = struct.unpack(pattern, str[start:end]) 01526 start = end 01527 end += 4 01528 (length,) = _struct_I.unpack(str[start:end]) 01529 pattern = '<%sd'%length 01530 start = end 01531 end += struct.calcsize(pattern) 01532 self.action_goal.goal.grasp.pre_grasp_posture.effort = struct.unpack(pattern, str[start:end]) 01533 _x = self 01534 start = end 01535 end += 12 01536 (_x.action_goal.goal.grasp.grasp_posture.header.seq, _x.action_goal.goal.grasp.grasp_posture.header.stamp.secs, _x.action_goal.goal.grasp.grasp_posture.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 01537 start = end 01538 end += 4 01539 (length,) = _struct_I.unpack(str[start:end]) 01540 start = end 01541 end += length 01542 self.action_goal.goal.grasp.grasp_posture.header.frame_id = str[start:end] 01543 start = end 01544 end += 4 01545 (length,) = _struct_I.unpack(str[start:end]) 01546 self.action_goal.goal.grasp.grasp_posture.name = [] 01547 for i in range(0, length): 01548 start = end 01549 end += 4 01550 (length,) = _struct_I.unpack(str[start:end]) 01551 start = end 01552 end += length 01553 val1 = str[start:end] 01554 self.action_goal.goal.grasp.grasp_posture.name.append(val1) 01555 start = end 01556 end += 4 01557 (length,) = _struct_I.unpack(str[start:end]) 01558 pattern = '<%sd'%length 01559 start = end 01560 end += struct.calcsize(pattern) 01561 self.action_goal.goal.grasp.grasp_posture.position = struct.unpack(pattern, str[start:end]) 01562 start = end 01563 end += 4 01564 (length,) = _struct_I.unpack(str[start:end]) 01565 pattern = '<%sd'%length 01566 start = end 01567 end += struct.calcsize(pattern) 01568 self.action_goal.goal.grasp.grasp_posture.velocity = struct.unpack(pattern, str[start:end]) 01569 start = end 01570 end += 4 01571 (length,) = _struct_I.unpack(str[start:end]) 01572 pattern = '<%sd'%length 01573 start = end 01574 end += struct.calcsize(pattern) 01575 self.action_goal.goal.grasp.grasp_posture.effort = struct.unpack(pattern, str[start:end]) 01576 _x = self 01577 start = end 01578 end += 73 01579 (_x.action_goal.goal.grasp.grasp_pose.position.x, _x.action_goal.goal.grasp.grasp_pose.position.y, _x.action_goal.goal.grasp.grasp_pose.position.z, _x.action_goal.goal.grasp.grasp_pose.orientation.x, _x.action_goal.goal.grasp.grasp_pose.orientation.y, _x.action_goal.goal.grasp.grasp_pose.orientation.z, _x.action_goal.goal.grasp.grasp_pose.orientation.w, _x.action_goal.goal.grasp.success_probability, _x.action_goal.goal.grasp.cluster_rep, _x.action_goal.goal.grasp.desired_approach_distance, _x.action_goal.goal.grasp.min_approach_distance,) = _struct_8dB2f.unpack(str[start:end]) 01580 self.action_goal.goal.grasp.cluster_rep = bool(self.action_goal.goal.grasp.cluster_rep) 01581 start = end 01582 end += 4 01583 (length,) = _struct_I.unpack(str[start:end]) 01584 self.action_goal.goal.grasp.moved_obstacles = [] 01585 for i in range(0, length): 01586 val1 = object_manipulation_msgs.msg.GraspableObject() 01587 start = end 01588 end += 4 01589 (length,) = _struct_I.unpack(str[start:end]) 01590 start = end 01591 end += length 01592 val1.reference_frame_id = str[start:end] 01593 start = end 01594 end += 4 01595 (length,) = _struct_I.unpack(str[start:end]) 01596 val1.potential_models = [] 01597 for i in range(0, length): 01598 val2 = household_objects_database_msgs.msg.DatabaseModelPose() 01599 start = end 01600 end += 4 01601 (val2.model_id,) = _struct_i.unpack(str[start:end]) 01602 _v67 = val2.pose 01603 _v68 = _v67.header 01604 start = end 01605 end += 4 01606 (_v68.seq,) = _struct_I.unpack(str[start:end]) 01607 _v69 = _v68.stamp 01608 _x = _v69 01609 start = end 01610 end += 8 01611 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01612 start = end 01613 end += 4 01614 (length,) = _struct_I.unpack(str[start:end]) 01615 start = end 01616 end += length 01617 _v68.frame_id = str[start:end] 01618 _v70 = _v67.pose 01619 _v71 = _v70.position 01620 _x = _v71 01621 start = end 01622 end += 24 01623 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 01624 _v72 = _v70.orientation 01625 _x = _v72 01626 start = end 01627 end += 32 01628 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 01629 start = end 01630 end += 4 01631 (val2.confidence,) = _struct_f.unpack(str[start:end]) 01632 start = end 01633 end += 4 01634 (length,) = _struct_I.unpack(str[start:end]) 01635 start = end 01636 end += length 01637 val2.detector_name = str[start:end] 01638 val1.potential_models.append(val2) 01639 _v73 = val1.cluster 01640 _v74 = _v73.header 01641 start = end 01642 end += 4 01643 (_v74.seq,) = _struct_I.unpack(str[start:end]) 01644 _v75 = _v74.stamp 01645 _x = _v75 01646 start = end 01647 end += 8 01648 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01649 start = end 01650 end += 4 01651 (length,) = _struct_I.unpack(str[start:end]) 01652 start = end 01653 end += length 01654 _v74.frame_id = str[start:end] 01655 start = end 01656 end += 4 01657 (length,) = _struct_I.unpack(str[start:end]) 01658 _v73.points = [] 01659 for i in range(0, length): 01660 val3 = geometry_msgs.msg.Point32() 01661 _x = val3 01662 start = end 01663 end += 12 01664 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end]) 01665 _v73.points.append(val3) 01666 start = end 01667 end += 4 01668 (length,) = _struct_I.unpack(str[start:end]) 01669 _v73.channels = [] 01670 for i in range(0, length): 01671 val3 = sensor_msgs.msg.ChannelFloat32() 01672 start = end 01673 end += 4 01674 (length,) = _struct_I.unpack(str[start:end]) 01675 start = end 01676 end += length 01677 val3.name = str[start:end] 01678 start = end 01679 end += 4 01680 (length,) = _struct_I.unpack(str[start:end]) 01681 pattern = '<%sf'%length 01682 start = end 01683 end += struct.calcsize(pattern) 01684 val3.values = struct.unpack(pattern, str[start:end]) 01685 _v73.channels.append(val3) 01686 _v76 = val1.region 01687 _v77 = _v76.cloud 01688 _v78 = _v77.header 01689 start = end 01690 end += 4 01691 (_v78.seq,) = _struct_I.unpack(str[start:end]) 01692 _v79 = _v78.stamp 01693 _x = _v79 01694 start = end 01695 end += 8 01696 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01697 start = end 01698 end += 4 01699 (length,) = _struct_I.unpack(str[start:end]) 01700 start = end 01701 end += length 01702 _v78.frame_id = str[start:end] 01703 _x = _v77 01704 start = end 01705 end += 8 01706 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 01707 start = end 01708 end += 4 01709 (length,) = _struct_I.unpack(str[start:end]) 01710 _v77.fields = [] 01711 for i in range(0, length): 01712 val4 = sensor_msgs.msg.PointField() 01713 start = end 01714 end += 4 01715 (length,) = _struct_I.unpack(str[start:end]) 01716 start = end 01717 end += length 01718 val4.name = str[start:end] 01719 _x = val4 01720 start = end 01721 end += 9 01722 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end]) 01723 _v77.fields.append(val4) 01724 _x = _v77 01725 start = end 01726 end += 9 01727 (_x.is_bigendian, _x.point_step, _x.row_step,) = _struct_B2I.unpack(str[start:end]) 01728 _v77.is_bigendian = bool(_v77.is_bigendian) 01729 start = end 01730 end += 4 01731 (length,) = _struct_I.unpack(str[start:end]) 01732 start = end 01733 end += length 01734 _v77.data = str[start:end] 01735 start = end 01736 end += 1 01737 (_v77.is_dense,) = _struct_B.unpack(str[start:end]) 01738 _v77.is_dense = bool(_v77.is_dense) 01739 start = end 01740 end += 4 01741 (length,) = _struct_I.unpack(str[start:end]) 01742 pattern = '<%si'%length 01743 start = end 01744 end += struct.calcsize(pattern) 01745 _v76.mask = struct.unpack(pattern, str[start:end]) 01746 _v80 = _v76.image 01747 _v81 = _v80.header 01748 start = end 01749 end += 4 01750 (_v81.seq,) = _struct_I.unpack(str[start:end]) 01751 _v82 = _v81.stamp 01752 _x = _v82 01753 start = end 01754 end += 8 01755 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01756 start = end 01757 end += 4 01758 (length,) = _struct_I.unpack(str[start:end]) 01759 start = end 01760 end += length 01761 _v81.frame_id = str[start:end] 01762 _x = _v80 01763 start = end 01764 end += 8 01765 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 01766 start = end 01767 end += 4 01768 (length,) = _struct_I.unpack(str[start:end]) 01769 start = end 01770 end += length 01771 _v80.encoding = str[start:end] 01772 _x = _v80 01773 start = end 01774 end += 5 01775 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end]) 01776 start = end 01777 end += 4 01778 (length,) = _struct_I.unpack(str[start:end]) 01779 start = end 01780 end += length 01781 _v80.data = str[start:end] 01782 _v83 = _v76.disparity_image 01783 _v84 = _v83.header 01784 start = end 01785 end += 4 01786 (_v84.seq,) = _struct_I.unpack(str[start:end]) 01787 _v85 = _v84.stamp 01788 _x = _v85 01789 start = end 01790 end += 8 01791 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01792 start = end 01793 end += 4 01794 (length,) = _struct_I.unpack(str[start:end]) 01795 start = end 01796 end += length 01797 _v84.frame_id = str[start:end] 01798 _x = _v83 01799 start = end 01800 end += 8 01801 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 01802 start = end 01803 end += 4 01804 (length,) = _struct_I.unpack(str[start:end]) 01805 start = end 01806 end += length 01807 _v83.encoding = str[start:end] 01808 _x = _v83 01809 start = end 01810 end += 5 01811 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end]) 01812 start = end 01813 end += 4 01814 (length,) = _struct_I.unpack(str[start:end]) 01815 start = end 01816 end += length 01817 _v83.data = str[start:end] 01818 _v86 = _v76.cam_info 01819 _v87 = _v86.header 01820 start = end 01821 end += 4 01822 (_v87.seq,) = _struct_I.unpack(str[start:end]) 01823 _v88 = _v87.stamp 01824 _x = _v88 01825 start = end 01826 end += 8 01827 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01828 start = end 01829 end += 4 01830 (length,) = _struct_I.unpack(str[start:end]) 01831 start = end 01832 end += length 01833 _v87.frame_id = str[start:end] 01834 _x = _v86 01835 start = end 01836 end += 8 01837 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 01838 start = end 01839 end += 4 01840 (length,) = _struct_I.unpack(str[start:end]) 01841 start = end 01842 end += length 01843 _v86.distortion_model = str[start:end] 01844 start = end 01845 end += 4 01846 (length,) = _struct_I.unpack(str[start:end]) 01847 pattern = '<%sd'%length 01848 start = end 01849 end += struct.calcsize(pattern) 01850 _v86.D = struct.unpack(pattern, str[start:end]) 01851 start = end 01852 end += 72 01853 _v86.K = _struct_9d.unpack(str[start:end]) 01854 start = end 01855 end += 72 01856 _v86.R = _struct_9d.unpack(str[start:end]) 01857 start = end 01858 end += 96 01859 _v86.P = _struct_12d.unpack(str[start:end]) 01860 _x = _v86 01861 start = end 01862 end += 8 01863 (_x.binning_x, _x.binning_y,) = _struct_2I.unpack(str[start:end]) 01864 _v89 = _v86.roi 01865 _x = _v89 01866 start = end 01867 end += 17 01868 (_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify,) = _struct_4IB.unpack(str[start:end]) 01869 _v89.do_rectify = bool(_v89.do_rectify) 01870 _v90 = _v76.roi_box_pose 01871 _v91 = _v90.header 01872 start = end 01873 end += 4 01874 (_v91.seq,) = _struct_I.unpack(str[start:end]) 01875 _v92 = _v91.stamp 01876 _x = _v92 01877 start = end 01878 end += 8 01879 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01880 start = end 01881 end += 4 01882 (length,) = _struct_I.unpack(str[start:end]) 01883 start = end 01884 end += length 01885 _v91.frame_id = str[start:end] 01886 _v93 = _v90.pose 01887 _v94 = _v93.position 01888 _x = _v94 01889 start = end 01890 end += 24 01891 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 01892 _v95 = _v93.orientation 01893 _x = _v95 01894 start = end 01895 end += 32 01896 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 01897 _v96 = _v76.roi_box_dims 01898 _x = _v96 01899 start = end 01900 end += 24 01901 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 01902 start = end 01903 end += 4 01904 (length,) = _struct_I.unpack(str[start:end]) 01905 start = end 01906 end += length 01907 val1.collision_name = str[start:end] 01908 self.action_goal.goal.grasp.moved_obstacles.append(val1) 01909 _x = self 01910 start = end 01911 end += 20 01912 (_x.action_goal.goal.desired_retreat_distance, _x.action_goal.goal.min_retreat_distance, _x.action_goal.goal.approach.direction.header.seq, _x.action_goal.goal.approach.direction.header.stamp.secs, _x.action_goal.goal.approach.direction.header.stamp.nsecs,) = _struct_2f3I.unpack(str[start:end]) 01913 start = end 01914 end += 4 01915 (length,) = _struct_I.unpack(str[start:end]) 01916 start = end 01917 end += length 01918 self.action_goal.goal.approach.direction.header.frame_id = str[start:end] 01919 _x = self 01920 start = end 01921 end += 32 01922 (_x.action_goal.goal.approach.direction.vector.x, _x.action_goal.goal.approach.direction.vector.y, _x.action_goal.goal.approach.direction.vector.z, _x.action_goal.goal.approach.desired_distance, _x.action_goal.goal.approach.min_distance,) = _struct_3d2f.unpack(str[start:end]) 01923 start = end 01924 end += 4 01925 (length,) = _struct_I.unpack(str[start:end]) 01926 start = end 01927 end += length 01928 self.action_goal.goal.collision_object_name = str[start:end] 01929 start = end 01930 end += 4 01931 (length,) = _struct_I.unpack(str[start:end]) 01932 start = end 01933 end += length 01934 self.action_goal.goal.collision_support_surface_name = str[start:end] 01935 _x = self 01936 start = end 01937 end += 11 01938 (_x.action_goal.goal.allow_gripper_support_collision, _x.action_goal.goal.use_reactive_place, _x.action_goal.goal.place_padding, _x.action_goal.goal.only_perform_feasibility_test,) = _struct_2BdB.unpack(str[start:end]) 01939 self.action_goal.goal.allow_gripper_support_collision = bool(self.action_goal.goal.allow_gripper_support_collision) 01940 self.action_goal.goal.use_reactive_place = bool(self.action_goal.goal.use_reactive_place) 01941 self.action_goal.goal.only_perform_feasibility_test = bool(self.action_goal.goal.only_perform_feasibility_test) 01942 start = end 01943 end += 4 01944 (length,) = _struct_I.unpack(str[start:end]) 01945 self.action_goal.goal.path_constraints.joint_constraints = [] 01946 for i in range(0, length): 01947 val1 = arm_navigation_msgs.msg.JointConstraint() 01948 start = end 01949 end += 4 01950 (length,) = _struct_I.unpack(str[start:end]) 01951 start = end 01952 end += length 01953 val1.joint_name = str[start:end] 01954 _x = val1 01955 start = end 01956 end += 32 01957 (_x.position, _x.tolerance_above, _x.tolerance_below, _x.weight,) = _struct_4d.unpack(str[start:end]) 01958 self.action_goal.goal.path_constraints.joint_constraints.append(val1) 01959 start = end 01960 end += 4 01961 (length,) = _struct_I.unpack(str[start:end]) 01962 self.action_goal.goal.path_constraints.position_constraints = [] 01963 for i in range(0, length): 01964 val1 = arm_navigation_msgs.msg.PositionConstraint() 01965 _v97 = val1.header 01966 start = end 01967 end += 4 01968 (_v97.seq,) = _struct_I.unpack(str[start:end]) 01969 _v98 = _v97.stamp 01970 _x = _v98 01971 start = end 01972 end += 8 01973 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01974 start = end 01975 end += 4 01976 (length,) = _struct_I.unpack(str[start:end]) 01977 start = end 01978 end += length 01979 _v97.frame_id = str[start:end] 01980 start = end 01981 end += 4 01982 (length,) = _struct_I.unpack(str[start:end]) 01983 start = end 01984 end += length 01985 val1.link_name = str[start:end] 01986 _v99 = val1.target_point_offset 01987 _x = _v99 01988 start = end 01989 end += 24 01990 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 01991 _v100 = val1.position 01992 _x = _v100 01993 start = end 01994 end += 24 01995 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 01996 _v101 = val1.constraint_region_shape 01997 start = end 01998 end += 1 01999 (_v101.type,) = _struct_b.unpack(str[start:end]) 02000 start = end 02001 end += 4 02002 (length,) = _struct_I.unpack(str[start:end]) 02003 pattern = '<%sd'%length 02004 start = end 02005 end += struct.calcsize(pattern) 02006 _v101.dimensions = struct.unpack(pattern, str[start:end]) 02007 start = end 02008 end += 4 02009 (length,) = _struct_I.unpack(str[start:end]) 02010 pattern = '<%si'%length 02011 start = end 02012 end += struct.calcsize(pattern) 02013 _v101.triangles = struct.unpack(pattern, str[start:end]) 02014 start = end 02015 end += 4 02016 (length,) = _struct_I.unpack(str[start:end]) 02017 _v101.vertices = [] 02018 for i in range(0, length): 02019 val3 = geometry_msgs.msg.Point() 02020 _x = val3 02021 start = end 02022 end += 24 02023 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 02024 _v101.vertices.append(val3) 02025 _v102 = val1.constraint_region_orientation 02026 _x = _v102 02027 start = end 02028 end += 32 02029 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 02030 start = end 02031 end += 8 02032 (val1.weight,) = _struct_d.unpack(str[start:end]) 02033 self.action_goal.goal.path_constraints.position_constraints.append(val1) 02034 start = end 02035 end += 4 02036 (length,) = _struct_I.unpack(str[start:end]) 02037 self.action_goal.goal.path_constraints.orientation_constraints = [] 02038 for i in range(0, length): 02039 val1 = arm_navigation_msgs.msg.OrientationConstraint() 02040 _v103 = val1.header 02041 start = end 02042 end += 4 02043 (_v103.seq,) = _struct_I.unpack(str[start:end]) 02044 _v104 = _v103.stamp 02045 _x = _v104 02046 start = end 02047 end += 8 02048 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 02049 start = end 02050 end += 4 02051 (length,) = _struct_I.unpack(str[start:end]) 02052 start = end 02053 end += length 02054 _v103.frame_id = str[start:end] 02055 start = end 02056 end += 4 02057 (length,) = _struct_I.unpack(str[start:end]) 02058 start = end 02059 end += length 02060 val1.link_name = str[start:end] 02061 start = end 02062 end += 4 02063 (val1.type,) = _struct_i.unpack(str[start:end]) 02064 _v105 = val1.orientation 02065 _x = _v105 02066 start = end 02067 end += 32 02068 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 02069 _x = val1 02070 start = end 02071 end += 32 02072 (_x.absolute_roll_tolerance, _x.absolute_pitch_tolerance, _x.absolute_yaw_tolerance, _x.weight,) = _struct_4d.unpack(str[start:end]) 02073 self.action_goal.goal.path_constraints.orientation_constraints.append(val1) 02074 start = end 02075 end += 4 02076 (length,) = _struct_I.unpack(str[start:end]) 02077 self.action_goal.goal.path_constraints.visibility_constraints = [] 02078 for i in range(0, length): 02079 val1 = arm_navigation_msgs.msg.VisibilityConstraint() 02080 _v106 = val1.header 02081 start = end 02082 end += 4 02083 (_v106.seq,) = _struct_I.unpack(str[start:end]) 02084 _v107 = _v106.stamp 02085 _x = _v107 02086 start = end 02087 end += 8 02088 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 02089 start = end 02090 end += 4 02091 (length,) = _struct_I.unpack(str[start:end]) 02092 start = end 02093 end += length 02094 _v106.frame_id = str[start:end] 02095 _v108 = val1.target 02096 _v109 = _v108.header 02097 start = end 02098 end += 4 02099 (_v109.seq,) = _struct_I.unpack(str[start:end]) 02100 _v110 = _v109.stamp 02101 _x = _v110 02102 start = end 02103 end += 8 02104 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 02105 start = end 02106 end += 4 02107 (length,) = _struct_I.unpack(str[start:end]) 02108 start = end 02109 end += length 02110 _v109.frame_id = str[start:end] 02111 _v111 = _v108.point 02112 _x = _v111 02113 start = end 02114 end += 24 02115 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 02116 _v112 = val1.sensor_pose 02117 _v113 = _v112.header 02118 start = end 02119 end += 4 02120 (_v113.seq,) = _struct_I.unpack(str[start:end]) 02121 _v114 = _v113.stamp 02122 _x = _v114 02123 start = end 02124 end += 8 02125 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 02126 start = end 02127 end += 4 02128 (length,) = _struct_I.unpack(str[start:end]) 02129 start = end 02130 end += length 02131 _v113.frame_id = str[start:end] 02132 _v115 = _v112.pose 02133 _v116 = _v115.position 02134 _x = _v116 02135 start = end 02136 end += 24 02137 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 02138 _v117 = _v115.orientation 02139 _x = _v117 02140 start = end 02141 end += 32 02142 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 02143 start = end 02144 end += 8 02145 (val1.absolute_tolerance,) = _struct_d.unpack(str[start:end]) 02146 self.action_goal.goal.path_constraints.visibility_constraints.append(val1) 02147 start = end 02148 end += 4 02149 (length,) = _struct_I.unpack(str[start:end]) 02150 self.action_goal.goal.additional_collision_operations.collision_operations = [] 02151 for i in range(0, length): 02152 val1 = arm_navigation_msgs.msg.CollisionOperation() 02153 start = end 02154 end += 4 02155 (length,) = _struct_I.unpack(str[start:end]) 02156 start = end 02157 end += length 02158 val1.object1 = str[start:end] 02159 start = end 02160 end += 4 02161 (length,) = _struct_I.unpack(str[start:end]) 02162 start = end 02163 end += length 02164 val1.object2 = str[start:end] 02165 _x = val1 02166 start = end 02167 end += 12 02168 (_x.penetration_distance, _x.operation,) = _struct_di.unpack(str[start:end]) 02169 self.action_goal.goal.additional_collision_operations.collision_operations.append(val1) 02170 start = end 02171 end += 4 02172 (length,) = _struct_I.unpack(str[start:end]) 02173 self.action_goal.goal.additional_link_padding = [] 02174 for i in range(0, length): 02175 val1 = arm_navigation_msgs.msg.LinkPadding() 02176 start = end 02177 end += 4 02178 (length,) = _struct_I.unpack(str[start:end]) 02179 start = end 02180 end += length 02181 val1.link_name = str[start:end] 02182 start = end 02183 end += 8 02184 (val1.padding,) = _struct_d.unpack(str[start:end]) 02185 self.action_goal.goal.additional_link_padding.append(val1) 02186 _x = self 02187 start = end 02188 end += 12 02189 (_x.action_result.header.seq, _x.action_result.header.stamp.secs, _x.action_result.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 02190 start = end 02191 end += 4 02192 (length,) = _struct_I.unpack(str[start:end]) 02193 start = end 02194 end += length 02195 self.action_result.header.frame_id = str[start:end] 02196 _x = self 02197 start = end 02198 end += 8 02199 (_x.action_result.status.goal_id.stamp.secs, _x.action_result.status.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end]) 02200 start = end 02201 end += 4 02202 (length,) = _struct_I.unpack(str[start:end]) 02203 start = end 02204 end += length 02205 self.action_result.status.goal_id.id = str[start:end] 02206 start = end 02207 end += 1 02208 (self.action_result.status.status,) = _struct_B.unpack(str[start:end]) 02209 start = end 02210 end += 4 02211 (length,) = _struct_I.unpack(str[start:end]) 02212 start = end 02213 end += length 02214 self.action_result.status.text = str[start:end] 02215 _x = self 02216 start = end 02217 end += 16 02218 (_x.action_result.result.manipulation_result.value, _x.action_result.result.place_location.header.seq, _x.action_result.result.place_location.header.stamp.secs, _x.action_result.result.place_location.header.stamp.nsecs,) = _struct_i3I.unpack(str[start:end]) 02219 start = end 02220 end += 4 02221 (length,) = _struct_I.unpack(str[start:end]) 02222 start = end 02223 end += length 02224 self.action_result.result.place_location.header.frame_id = str[start:end] 02225 _x = self 02226 start = end 02227 end += 56 02228 (_x.action_result.result.place_location.pose.position.x, _x.action_result.result.place_location.pose.position.y, _x.action_result.result.place_location.pose.position.z, _x.action_result.result.place_location.pose.orientation.x, _x.action_result.result.place_location.pose.orientation.y, _x.action_result.result.place_location.pose.orientation.z, _x.action_result.result.place_location.pose.orientation.w,) = _struct_7d.unpack(str[start:end]) 02229 start = end 02230 end += 4 02231 (length,) = _struct_I.unpack(str[start:end]) 02232 self.action_result.result.attempted_locations = [] 02233 for i in range(0, length): 02234 val1 = geometry_msgs.msg.PoseStamped() 02235 _v118 = val1.header 02236 start = end 02237 end += 4 02238 (_v118.seq,) = _struct_I.unpack(str[start:end]) 02239 _v119 = _v118.stamp 02240 _x = _v119 02241 start = end 02242 end += 8 02243 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 02244 start = end 02245 end += 4 02246 (length,) = _struct_I.unpack(str[start:end]) 02247 start = end 02248 end += length 02249 _v118.frame_id = str[start:end] 02250 _v120 = val1.pose 02251 _v121 = _v120.position 02252 _x = _v121 02253 start = end 02254 end += 24 02255 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 02256 _v122 = _v120.orientation 02257 _x = _v122 02258 start = end 02259 end += 32 02260 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 02261 self.action_result.result.attempted_locations.append(val1) 02262 start = end 02263 end += 4 02264 (length,) = _struct_I.unpack(str[start:end]) 02265 self.action_result.result.attempted_location_results = [] 02266 for i in range(0, length): 02267 val1 = object_manipulation_msgs.msg.PlaceLocationResult() 02268 _x = val1 02269 start = end 02270 end += 5 02271 (_x.result_code, _x.continuation_possible,) = _struct_iB.unpack(str[start:end]) 02272 val1.continuation_possible = bool(val1.continuation_possible) 02273 self.action_result.result.attempted_location_results.append(val1) 02274 _x = self 02275 start = end 02276 end += 12 02277 (_x.action_feedback.header.seq, _x.action_feedback.header.stamp.secs, _x.action_feedback.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 02278 start = end 02279 end += 4 02280 (length,) = _struct_I.unpack(str[start:end]) 02281 start = end 02282 end += length 02283 self.action_feedback.header.frame_id = str[start:end] 02284 _x = self 02285 start = end 02286 end += 8 02287 (_x.action_feedback.status.goal_id.stamp.secs, _x.action_feedback.status.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end]) 02288 start = end 02289 end += 4 02290 (length,) = _struct_I.unpack(str[start:end]) 02291 start = end 02292 end += length 02293 self.action_feedback.status.goal_id.id = str[start:end] 02294 start = end 02295 end += 1 02296 (self.action_feedback.status.status,) = _struct_B.unpack(str[start:end]) 02297 start = end 02298 end += 4 02299 (length,) = _struct_I.unpack(str[start:end]) 02300 start = end 02301 end += length 02302 self.action_feedback.status.text = str[start:end] 02303 _x = self 02304 start = end 02305 end += 8 02306 (_x.action_feedback.feedback.current_location, _x.action_feedback.feedback.total_locations,) = _struct_2i.unpack(str[start:end]) 02307 return self 02308 except struct.error as e: 02309 raise roslib.message.DeserializationError(e) #most likely buffer underfill 02310 02311 02312 def serialize_numpy(self, buff, numpy): 02313 """ 02314 serialize message with numpy array types into buffer 02315 @param buff: buffer 02316 @type buff: StringIO 02317 @param numpy: numpy python module 02318 @type numpy module 02319 """ 02320 try: 02321 _x = self 02322 buff.write(_struct_3I.pack(_x.action_goal.header.seq, _x.action_goal.header.stamp.secs, _x.action_goal.header.stamp.nsecs)) 02323 _x = self.action_goal.header.frame_id 02324 length = len(_x) 02325 buff.write(struct.pack('<I%ss'%length, length, _x)) 02326 _x = self 02327 buff.write(_struct_2I.pack(_x.action_goal.goal_id.stamp.secs, _x.action_goal.goal_id.stamp.nsecs)) 02328 _x = self.action_goal.goal_id.id 02329 length = len(_x) 02330 buff.write(struct.pack('<I%ss'%length, length, _x)) 02331 _x = self.action_goal.goal.arm_name 02332 length = len(_x) 02333 buff.write(struct.pack('<I%ss'%length, length, _x)) 02334 length = len(self.action_goal.goal.place_locations) 02335 buff.write(_struct_I.pack(length)) 02336 for val1 in self.action_goal.goal.place_locations: 02337 _v123 = val1.header 02338 buff.write(_struct_I.pack(_v123.seq)) 02339 _v124 = _v123.stamp 02340 _x = _v124 02341 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02342 _x = _v123.frame_id 02343 length = len(_x) 02344 buff.write(struct.pack('<I%ss'%length, length, _x)) 02345 _v125 = val1.pose 02346 _v126 = _v125.position 02347 _x = _v126 02348 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 02349 _v127 = _v125.orientation 02350 _x = _v127 02351 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 02352 _x = self 02353 buff.write(_struct_3I.pack(_x.action_goal.goal.grasp.pre_grasp_posture.header.seq, _x.action_goal.goal.grasp.pre_grasp_posture.header.stamp.secs, _x.action_goal.goal.grasp.pre_grasp_posture.header.stamp.nsecs)) 02354 _x = self.action_goal.goal.grasp.pre_grasp_posture.header.frame_id 02355 length = len(_x) 02356 buff.write(struct.pack('<I%ss'%length, length, _x)) 02357 length = len(self.action_goal.goal.grasp.pre_grasp_posture.name) 02358 buff.write(_struct_I.pack(length)) 02359 for val1 in self.action_goal.goal.grasp.pre_grasp_posture.name: 02360 length = len(val1) 02361 buff.write(struct.pack('<I%ss'%length, length, val1)) 02362 length = len(self.action_goal.goal.grasp.pre_grasp_posture.position) 02363 buff.write(_struct_I.pack(length)) 02364 pattern = '<%sd'%length 02365 buff.write(self.action_goal.goal.grasp.pre_grasp_posture.position.tostring()) 02366 length = len(self.action_goal.goal.grasp.pre_grasp_posture.velocity) 02367 buff.write(_struct_I.pack(length)) 02368 pattern = '<%sd'%length 02369 buff.write(self.action_goal.goal.grasp.pre_grasp_posture.velocity.tostring()) 02370 length = len(self.action_goal.goal.grasp.pre_grasp_posture.effort) 02371 buff.write(_struct_I.pack(length)) 02372 pattern = '<%sd'%length 02373 buff.write(self.action_goal.goal.grasp.pre_grasp_posture.effort.tostring()) 02374 _x = self 02375 buff.write(_struct_3I.pack(_x.action_goal.goal.grasp.grasp_posture.header.seq, _x.action_goal.goal.grasp.grasp_posture.header.stamp.secs, _x.action_goal.goal.grasp.grasp_posture.header.stamp.nsecs)) 02376 _x = self.action_goal.goal.grasp.grasp_posture.header.frame_id 02377 length = len(_x) 02378 buff.write(struct.pack('<I%ss'%length, length, _x)) 02379 length = len(self.action_goal.goal.grasp.grasp_posture.name) 02380 buff.write(_struct_I.pack(length)) 02381 for val1 in self.action_goal.goal.grasp.grasp_posture.name: 02382 length = len(val1) 02383 buff.write(struct.pack('<I%ss'%length, length, val1)) 02384 length = len(self.action_goal.goal.grasp.grasp_posture.position) 02385 buff.write(_struct_I.pack(length)) 02386 pattern = '<%sd'%length 02387 buff.write(self.action_goal.goal.grasp.grasp_posture.position.tostring()) 02388 length = len(self.action_goal.goal.grasp.grasp_posture.velocity) 02389 buff.write(_struct_I.pack(length)) 02390 pattern = '<%sd'%length 02391 buff.write(self.action_goal.goal.grasp.grasp_posture.velocity.tostring()) 02392 length = len(self.action_goal.goal.grasp.grasp_posture.effort) 02393 buff.write(_struct_I.pack(length)) 02394 pattern = '<%sd'%length 02395 buff.write(self.action_goal.goal.grasp.grasp_posture.effort.tostring()) 02396 _x = self 02397 buff.write(_struct_8dB2f.pack(_x.action_goal.goal.grasp.grasp_pose.position.x, _x.action_goal.goal.grasp.grasp_pose.position.y, _x.action_goal.goal.grasp.grasp_pose.position.z, _x.action_goal.goal.grasp.grasp_pose.orientation.x, _x.action_goal.goal.grasp.grasp_pose.orientation.y, _x.action_goal.goal.grasp.grasp_pose.orientation.z, _x.action_goal.goal.grasp.grasp_pose.orientation.w, _x.action_goal.goal.grasp.success_probability, _x.action_goal.goal.grasp.cluster_rep, _x.action_goal.goal.grasp.desired_approach_distance, _x.action_goal.goal.grasp.min_approach_distance)) 02398 length = len(self.action_goal.goal.grasp.moved_obstacles) 02399 buff.write(_struct_I.pack(length)) 02400 for val1 in self.action_goal.goal.grasp.moved_obstacles: 02401 _x = val1.reference_frame_id 02402 length = len(_x) 02403 buff.write(struct.pack('<I%ss'%length, length, _x)) 02404 length = len(val1.potential_models) 02405 buff.write(_struct_I.pack(length)) 02406 for val2 in val1.potential_models: 02407 buff.write(_struct_i.pack(val2.model_id)) 02408 _v128 = val2.pose 02409 _v129 = _v128.header 02410 buff.write(_struct_I.pack(_v129.seq)) 02411 _v130 = _v129.stamp 02412 _x = _v130 02413 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02414 _x = _v129.frame_id 02415 length = len(_x) 02416 buff.write(struct.pack('<I%ss'%length, length, _x)) 02417 _v131 = _v128.pose 02418 _v132 = _v131.position 02419 _x = _v132 02420 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 02421 _v133 = _v131.orientation 02422 _x = _v133 02423 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 02424 buff.write(_struct_f.pack(val2.confidence)) 02425 _x = val2.detector_name 02426 length = len(_x) 02427 buff.write(struct.pack('<I%ss'%length, length, _x)) 02428 _v134 = val1.cluster 02429 _v135 = _v134.header 02430 buff.write(_struct_I.pack(_v135.seq)) 02431 _v136 = _v135.stamp 02432 _x = _v136 02433 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02434 _x = _v135.frame_id 02435 length = len(_x) 02436 buff.write(struct.pack('<I%ss'%length, length, _x)) 02437 length = len(_v134.points) 02438 buff.write(_struct_I.pack(length)) 02439 for val3 in _v134.points: 02440 _x = val3 02441 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z)) 02442 length = len(_v134.channels) 02443 buff.write(_struct_I.pack(length)) 02444 for val3 in _v134.channels: 02445 _x = val3.name 02446 length = len(_x) 02447 buff.write(struct.pack('<I%ss'%length, length, _x)) 02448 length = len(val3.values) 02449 buff.write(_struct_I.pack(length)) 02450 pattern = '<%sf'%length 02451 buff.write(val3.values.tostring()) 02452 _v137 = val1.region 02453 _v138 = _v137.cloud 02454 _v139 = _v138.header 02455 buff.write(_struct_I.pack(_v139.seq)) 02456 _v140 = _v139.stamp 02457 _x = _v140 02458 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02459 _x = _v139.frame_id 02460 length = len(_x) 02461 buff.write(struct.pack('<I%ss'%length, length, _x)) 02462 _x = _v138 02463 buff.write(_struct_2I.pack(_x.height, _x.width)) 02464 length = len(_v138.fields) 02465 buff.write(_struct_I.pack(length)) 02466 for val4 in _v138.fields: 02467 _x = val4.name 02468 length = len(_x) 02469 buff.write(struct.pack('<I%ss'%length, length, _x)) 02470 _x = val4 02471 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count)) 02472 _x = _v138 02473 buff.write(_struct_B2I.pack(_x.is_bigendian, _x.point_step, _x.row_step)) 02474 _x = _v138.data 02475 length = len(_x) 02476 # - if encoded as a list instead, serialize as bytes instead of string 02477 if type(_x) in [list, tuple]: 02478 buff.write(struct.pack('<I%sB'%length, length, *_x)) 02479 else: 02480 buff.write(struct.pack('<I%ss'%length, length, _x)) 02481 buff.write(_struct_B.pack(_v138.is_dense)) 02482 length = len(_v137.mask) 02483 buff.write(_struct_I.pack(length)) 02484 pattern = '<%si'%length 02485 buff.write(_v137.mask.tostring()) 02486 _v141 = _v137.image 02487 _v142 = _v141.header 02488 buff.write(_struct_I.pack(_v142.seq)) 02489 _v143 = _v142.stamp 02490 _x = _v143 02491 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02492 _x = _v142.frame_id 02493 length = len(_x) 02494 buff.write(struct.pack('<I%ss'%length, length, _x)) 02495 _x = _v141 02496 buff.write(_struct_2I.pack(_x.height, _x.width)) 02497 _x = _v141.encoding 02498 length = len(_x) 02499 buff.write(struct.pack('<I%ss'%length, length, _x)) 02500 _x = _v141 02501 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step)) 02502 _x = _v141.data 02503 length = len(_x) 02504 # - if encoded as a list instead, serialize as bytes instead of string 02505 if type(_x) in [list, tuple]: 02506 buff.write(struct.pack('<I%sB'%length, length, *_x)) 02507 else: 02508 buff.write(struct.pack('<I%ss'%length, length, _x)) 02509 _v144 = _v137.disparity_image 02510 _v145 = _v144.header 02511 buff.write(_struct_I.pack(_v145.seq)) 02512 _v146 = _v145.stamp 02513 _x = _v146 02514 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02515 _x = _v145.frame_id 02516 length = len(_x) 02517 buff.write(struct.pack('<I%ss'%length, length, _x)) 02518 _x = _v144 02519 buff.write(_struct_2I.pack(_x.height, _x.width)) 02520 _x = _v144.encoding 02521 length = len(_x) 02522 buff.write(struct.pack('<I%ss'%length, length, _x)) 02523 _x = _v144 02524 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step)) 02525 _x = _v144.data 02526 length = len(_x) 02527 # - if encoded as a list instead, serialize as bytes instead of string 02528 if type(_x) in [list, tuple]: 02529 buff.write(struct.pack('<I%sB'%length, length, *_x)) 02530 else: 02531 buff.write(struct.pack('<I%ss'%length, length, _x)) 02532 _v147 = _v137.cam_info 02533 _v148 = _v147.header 02534 buff.write(_struct_I.pack(_v148.seq)) 02535 _v149 = _v148.stamp 02536 _x = _v149 02537 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02538 _x = _v148.frame_id 02539 length = len(_x) 02540 buff.write(struct.pack('<I%ss'%length, length, _x)) 02541 _x = _v147 02542 buff.write(_struct_2I.pack(_x.height, _x.width)) 02543 _x = _v147.distortion_model 02544 length = len(_x) 02545 buff.write(struct.pack('<I%ss'%length, length, _x)) 02546 length = len(_v147.D) 02547 buff.write(_struct_I.pack(length)) 02548 pattern = '<%sd'%length 02549 buff.write(_v147.D.tostring()) 02550 buff.write(_v147.K.tostring()) 02551 buff.write(_v147.R.tostring()) 02552 buff.write(_v147.P.tostring()) 02553 _x = _v147 02554 buff.write(_struct_2I.pack(_x.binning_x, _x.binning_y)) 02555 _v150 = _v147.roi 02556 _x = _v150 02557 buff.write(_struct_4IB.pack(_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify)) 02558 _v151 = _v137.roi_box_pose 02559 _v152 = _v151.header 02560 buff.write(_struct_I.pack(_v152.seq)) 02561 _v153 = _v152.stamp 02562 _x = _v153 02563 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02564 _x = _v152.frame_id 02565 length = len(_x) 02566 buff.write(struct.pack('<I%ss'%length, length, _x)) 02567 _v154 = _v151.pose 02568 _v155 = _v154.position 02569 _x = _v155 02570 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 02571 _v156 = _v154.orientation 02572 _x = _v156 02573 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 02574 _v157 = _v137.roi_box_dims 02575 _x = _v157 02576 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 02577 _x = val1.collision_name 02578 length = len(_x) 02579 buff.write(struct.pack('<I%ss'%length, length, _x)) 02580 _x = self 02581 buff.write(_struct_2f3I.pack(_x.action_goal.goal.desired_retreat_distance, _x.action_goal.goal.min_retreat_distance, _x.action_goal.goal.approach.direction.header.seq, _x.action_goal.goal.approach.direction.header.stamp.secs, _x.action_goal.goal.approach.direction.header.stamp.nsecs)) 02582 _x = self.action_goal.goal.approach.direction.header.frame_id 02583 length = len(_x) 02584 buff.write(struct.pack('<I%ss'%length, length, _x)) 02585 _x = self 02586 buff.write(_struct_3d2f.pack(_x.action_goal.goal.approach.direction.vector.x, _x.action_goal.goal.approach.direction.vector.y, _x.action_goal.goal.approach.direction.vector.z, _x.action_goal.goal.approach.desired_distance, _x.action_goal.goal.approach.min_distance)) 02587 _x = self.action_goal.goal.collision_object_name 02588 length = len(_x) 02589 buff.write(struct.pack('<I%ss'%length, length, _x)) 02590 _x = self.action_goal.goal.collision_support_surface_name 02591 length = len(_x) 02592 buff.write(struct.pack('<I%ss'%length, length, _x)) 02593 _x = self 02594 buff.write(_struct_2BdB.pack(_x.action_goal.goal.allow_gripper_support_collision, _x.action_goal.goal.use_reactive_place, _x.action_goal.goal.place_padding, _x.action_goal.goal.only_perform_feasibility_test)) 02595 length = len(self.action_goal.goal.path_constraints.joint_constraints) 02596 buff.write(_struct_I.pack(length)) 02597 for val1 in self.action_goal.goal.path_constraints.joint_constraints: 02598 _x = val1.joint_name 02599 length = len(_x) 02600 buff.write(struct.pack('<I%ss'%length, length, _x)) 02601 _x = val1 02602 buff.write(_struct_4d.pack(_x.position, _x.tolerance_above, _x.tolerance_below, _x.weight)) 02603 length = len(self.action_goal.goal.path_constraints.position_constraints) 02604 buff.write(_struct_I.pack(length)) 02605 for val1 in self.action_goal.goal.path_constraints.position_constraints: 02606 _v158 = val1.header 02607 buff.write(_struct_I.pack(_v158.seq)) 02608 _v159 = _v158.stamp 02609 _x = _v159 02610 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02611 _x = _v158.frame_id 02612 length = len(_x) 02613 buff.write(struct.pack('<I%ss'%length, length, _x)) 02614 _x = val1.link_name 02615 length = len(_x) 02616 buff.write(struct.pack('<I%ss'%length, length, _x)) 02617 _v160 = val1.target_point_offset 02618 _x = _v160 02619 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 02620 _v161 = val1.position 02621 _x = _v161 02622 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 02623 _v162 = val1.constraint_region_shape 02624 buff.write(_struct_b.pack(_v162.type)) 02625 length = len(_v162.dimensions) 02626 buff.write(_struct_I.pack(length)) 02627 pattern = '<%sd'%length 02628 buff.write(_v162.dimensions.tostring()) 02629 length = len(_v162.triangles) 02630 buff.write(_struct_I.pack(length)) 02631 pattern = '<%si'%length 02632 buff.write(_v162.triangles.tostring()) 02633 length = len(_v162.vertices) 02634 buff.write(_struct_I.pack(length)) 02635 for val3 in _v162.vertices: 02636 _x = val3 02637 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 02638 _v163 = val1.constraint_region_orientation 02639 _x = _v163 02640 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 02641 buff.write(_struct_d.pack(val1.weight)) 02642 length = len(self.action_goal.goal.path_constraints.orientation_constraints) 02643 buff.write(_struct_I.pack(length)) 02644 for val1 in self.action_goal.goal.path_constraints.orientation_constraints: 02645 _v164 = val1.header 02646 buff.write(_struct_I.pack(_v164.seq)) 02647 _v165 = _v164.stamp 02648 _x = _v165 02649 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02650 _x = _v164.frame_id 02651 length = len(_x) 02652 buff.write(struct.pack('<I%ss'%length, length, _x)) 02653 _x = val1.link_name 02654 length = len(_x) 02655 buff.write(struct.pack('<I%ss'%length, length, _x)) 02656 buff.write(_struct_i.pack(val1.type)) 02657 _v166 = val1.orientation 02658 _x = _v166 02659 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 02660 _x = val1 02661 buff.write(_struct_4d.pack(_x.absolute_roll_tolerance, _x.absolute_pitch_tolerance, _x.absolute_yaw_tolerance, _x.weight)) 02662 length = len(self.action_goal.goal.path_constraints.visibility_constraints) 02663 buff.write(_struct_I.pack(length)) 02664 for val1 in self.action_goal.goal.path_constraints.visibility_constraints: 02665 _v167 = val1.header 02666 buff.write(_struct_I.pack(_v167.seq)) 02667 _v168 = _v167.stamp 02668 _x = _v168 02669 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02670 _x = _v167.frame_id 02671 length = len(_x) 02672 buff.write(struct.pack('<I%ss'%length, length, _x)) 02673 _v169 = val1.target 02674 _v170 = _v169.header 02675 buff.write(_struct_I.pack(_v170.seq)) 02676 _v171 = _v170.stamp 02677 _x = _v171 02678 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02679 _x = _v170.frame_id 02680 length = len(_x) 02681 buff.write(struct.pack('<I%ss'%length, length, _x)) 02682 _v172 = _v169.point 02683 _x = _v172 02684 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 02685 _v173 = val1.sensor_pose 02686 _v174 = _v173.header 02687 buff.write(_struct_I.pack(_v174.seq)) 02688 _v175 = _v174.stamp 02689 _x = _v175 02690 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02691 _x = _v174.frame_id 02692 length = len(_x) 02693 buff.write(struct.pack('<I%ss'%length, length, _x)) 02694 _v176 = _v173.pose 02695 _v177 = _v176.position 02696 _x = _v177 02697 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 02698 _v178 = _v176.orientation 02699 _x = _v178 02700 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 02701 buff.write(_struct_d.pack(val1.absolute_tolerance)) 02702 length = len(self.action_goal.goal.additional_collision_operations.collision_operations) 02703 buff.write(_struct_I.pack(length)) 02704 for val1 in self.action_goal.goal.additional_collision_operations.collision_operations: 02705 _x = val1.object1 02706 length = len(_x) 02707 buff.write(struct.pack('<I%ss'%length, length, _x)) 02708 _x = val1.object2 02709 length = len(_x) 02710 buff.write(struct.pack('<I%ss'%length, length, _x)) 02711 _x = val1 02712 buff.write(_struct_di.pack(_x.penetration_distance, _x.operation)) 02713 length = len(self.action_goal.goal.additional_link_padding) 02714 buff.write(_struct_I.pack(length)) 02715 for val1 in self.action_goal.goal.additional_link_padding: 02716 _x = val1.link_name 02717 length = len(_x) 02718 buff.write(struct.pack('<I%ss'%length, length, _x)) 02719 buff.write(_struct_d.pack(val1.padding)) 02720 _x = self 02721 buff.write(_struct_3I.pack(_x.action_result.header.seq, _x.action_result.header.stamp.secs, _x.action_result.header.stamp.nsecs)) 02722 _x = self.action_result.header.frame_id 02723 length = len(_x) 02724 buff.write(struct.pack('<I%ss'%length, length, _x)) 02725 _x = self 02726 buff.write(_struct_2I.pack(_x.action_result.status.goal_id.stamp.secs, _x.action_result.status.goal_id.stamp.nsecs)) 02727 _x = self.action_result.status.goal_id.id 02728 length = len(_x) 02729 buff.write(struct.pack('<I%ss'%length, length, _x)) 02730 buff.write(_struct_B.pack(self.action_result.status.status)) 02731 _x = self.action_result.status.text 02732 length = len(_x) 02733 buff.write(struct.pack('<I%ss'%length, length, _x)) 02734 _x = self 02735 buff.write(_struct_i3I.pack(_x.action_result.result.manipulation_result.value, _x.action_result.result.place_location.header.seq, _x.action_result.result.place_location.header.stamp.secs, _x.action_result.result.place_location.header.stamp.nsecs)) 02736 _x = self.action_result.result.place_location.header.frame_id 02737 length = len(_x) 02738 buff.write(struct.pack('<I%ss'%length, length, _x)) 02739 _x = self 02740 buff.write(_struct_7d.pack(_x.action_result.result.place_location.pose.position.x, _x.action_result.result.place_location.pose.position.y, _x.action_result.result.place_location.pose.position.z, _x.action_result.result.place_location.pose.orientation.x, _x.action_result.result.place_location.pose.orientation.y, _x.action_result.result.place_location.pose.orientation.z, _x.action_result.result.place_location.pose.orientation.w)) 02741 length = len(self.action_result.result.attempted_locations) 02742 buff.write(_struct_I.pack(length)) 02743 for val1 in self.action_result.result.attempted_locations: 02744 _v179 = val1.header 02745 buff.write(_struct_I.pack(_v179.seq)) 02746 _v180 = _v179.stamp 02747 _x = _v180 02748 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 02749 _x = _v179.frame_id 02750 length = len(_x) 02751 buff.write(struct.pack('<I%ss'%length, length, _x)) 02752 _v181 = val1.pose 02753 _v182 = _v181.position 02754 _x = _v182 02755 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 02756 _v183 = _v181.orientation 02757 _x = _v183 02758 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 02759 length = len(self.action_result.result.attempted_location_results) 02760 buff.write(_struct_I.pack(length)) 02761 for val1 in self.action_result.result.attempted_location_results: 02762 _x = val1 02763 buff.write(_struct_iB.pack(_x.result_code, _x.continuation_possible)) 02764 _x = self 02765 buff.write(_struct_3I.pack(_x.action_feedback.header.seq, _x.action_feedback.header.stamp.secs, _x.action_feedback.header.stamp.nsecs)) 02766 _x = self.action_feedback.header.frame_id 02767 length = len(_x) 02768 buff.write(struct.pack('<I%ss'%length, length, _x)) 02769 _x = self 02770 buff.write(_struct_2I.pack(_x.action_feedback.status.goal_id.stamp.secs, _x.action_feedback.status.goal_id.stamp.nsecs)) 02771 _x = self.action_feedback.status.goal_id.id 02772 length = len(_x) 02773 buff.write(struct.pack('<I%ss'%length, length, _x)) 02774 buff.write(_struct_B.pack(self.action_feedback.status.status)) 02775 _x = self.action_feedback.status.text 02776 length = len(_x) 02777 buff.write(struct.pack('<I%ss'%length, length, _x)) 02778 _x = self 02779 buff.write(_struct_2i.pack(_x.action_feedback.feedback.current_location, _x.action_feedback.feedback.total_locations)) 02780 except struct.error as se: self._check_types(se) 02781 except TypeError as te: self._check_types(te) 02782 02783 def deserialize_numpy(self, str, numpy): 02784 """ 02785 unpack serialized message in str into this message instance using numpy for array types 02786 @param str: byte array of serialized message 02787 @type str: str 02788 @param numpy: numpy python module 02789 @type numpy: module 02790 """ 02791 try: 02792 if self.action_goal is None: 02793 self.action_goal = object_manipulation_msgs.msg.PlaceActionGoal() 02794 if self.action_result is None: 02795 self.action_result = object_manipulation_msgs.msg.PlaceActionResult() 02796 if self.action_feedback is None: 02797 self.action_feedback = object_manipulation_msgs.msg.PlaceActionFeedback() 02798 end = 0 02799 _x = self 02800 start = end 02801 end += 12 02802 (_x.action_goal.header.seq, _x.action_goal.header.stamp.secs, _x.action_goal.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 02803 start = end 02804 end += 4 02805 (length,) = _struct_I.unpack(str[start:end]) 02806 start = end 02807 end += length 02808 self.action_goal.header.frame_id = str[start:end] 02809 _x = self 02810 start = end 02811 end += 8 02812 (_x.action_goal.goal_id.stamp.secs, _x.action_goal.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end]) 02813 start = end 02814 end += 4 02815 (length,) = _struct_I.unpack(str[start:end]) 02816 start = end 02817 end += length 02818 self.action_goal.goal_id.id = str[start:end] 02819 start = end 02820 end += 4 02821 (length,) = _struct_I.unpack(str[start:end]) 02822 start = end 02823 end += length 02824 self.action_goal.goal.arm_name = str[start:end] 02825 start = end 02826 end += 4 02827 (length,) = _struct_I.unpack(str[start:end]) 02828 self.action_goal.goal.place_locations = [] 02829 for i in range(0, length): 02830 val1 = geometry_msgs.msg.PoseStamped() 02831 _v184 = val1.header 02832 start = end 02833 end += 4 02834 (_v184.seq,) = _struct_I.unpack(str[start:end]) 02835 _v185 = _v184.stamp 02836 _x = _v185 02837 start = end 02838 end += 8 02839 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 02840 start = end 02841 end += 4 02842 (length,) = _struct_I.unpack(str[start:end]) 02843 start = end 02844 end += length 02845 _v184.frame_id = str[start:end] 02846 _v186 = val1.pose 02847 _v187 = _v186.position 02848 _x = _v187 02849 start = end 02850 end += 24 02851 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 02852 _v188 = _v186.orientation 02853 _x = _v188 02854 start = end 02855 end += 32 02856 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 02857 self.action_goal.goal.place_locations.append(val1) 02858 _x = self 02859 start = end 02860 end += 12 02861 (_x.action_goal.goal.grasp.pre_grasp_posture.header.seq, _x.action_goal.goal.grasp.pre_grasp_posture.header.stamp.secs, _x.action_goal.goal.grasp.pre_grasp_posture.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 02862 start = end 02863 end += 4 02864 (length,) = _struct_I.unpack(str[start:end]) 02865 start = end 02866 end += length 02867 self.action_goal.goal.grasp.pre_grasp_posture.header.frame_id = str[start:end] 02868 start = end 02869 end += 4 02870 (length,) = _struct_I.unpack(str[start:end]) 02871 self.action_goal.goal.grasp.pre_grasp_posture.name = [] 02872 for i in range(0, length): 02873 start = end 02874 end += 4 02875 (length,) = _struct_I.unpack(str[start:end]) 02876 start = end 02877 end += length 02878 val1 = str[start:end] 02879 self.action_goal.goal.grasp.pre_grasp_posture.name.append(val1) 02880 start = end 02881 end += 4 02882 (length,) = _struct_I.unpack(str[start:end]) 02883 pattern = '<%sd'%length 02884 start = end 02885 end += struct.calcsize(pattern) 02886 self.action_goal.goal.grasp.pre_grasp_posture.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 02887 start = end 02888 end += 4 02889 (length,) = _struct_I.unpack(str[start:end]) 02890 pattern = '<%sd'%length 02891 start = end 02892 end += struct.calcsize(pattern) 02893 self.action_goal.goal.grasp.pre_grasp_posture.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 02894 start = end 02895 end += 4 02896 (length,) = _struct_I.unpack(str[start:end]) 02897 pattern = '<%sd'%length 02898 start = end 02899 end += struct.calcsize(pattern) 02900 self.action_goal.goal.grasp.pre_grasp_posture.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 02901 _x = self 02902 start = end 02903 end += 12 02904 (_x.action_goal.goal.grasp.grasp_posture.header.seq, _x.action_goal.goal.grasp.grasp_posture.header.stamp.secs, _x.action_goal.goal.grasp.grasp_posture.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 02905 start = end 02906 end += 4 02907 (length,) = _struct_I.unpack(str[start:end]) 02908 start = end 02909 end += length 02910 self.action_goal.goal.grasp.grasp_posture.header.frame_id = str[start:end] 02911 start = end 02912 end += 4 02913 (length,) = _struct_I.unpack(str[start:end]) 02914 self.action_goal.goal.grasp.grasp_posture.name = [] 02915 for i in range(0, length): 02916 start = end 02917 end += 4 02918 (length,) = _struct_I.unpack(str[start:end]) 02919 start = end 02920 end += length 02921 val1 = str[start:end] 02922 self.action_goal.goal.grasp.grasp_posture.name.append(val1) 02923 start = end 02924 end += 4 02925 (length,) = _struct_I.unpack(str[start:end]) 02926 pattern = '<%sd'%length 02927 start = end 02928 end += struct.calcsize(pattern) 02929 self.action_goal.goal.grasp.grasp_posture.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 02930 start = end 02931 end += 4 02932 (length,) = _struct_I.unpack(str[start:end]) 02933 pattern = '<%sd'%length 02934 start = end 02935 end += struct.calcsize(pattern) 02936 self.action_goal.goal.grasp.grasp_posture.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 02937 start = end 02938 end += 4 02939 (length,) = _struct_I.unpack(str[start:end]) 02940 pattern = '<%sd'%length 02941 start = end 02942 end += struct.calcsize(pattern) 02943 self.action_goal.goal.grasp.grasp_posture.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 02944 _x = self 02945 start = end 02946 end += 73 02947 (_x.action_goal.goal.grasp.grasp_pose.position.x, _x.action_goal.goal.grasp.grasp_pose.position.y, _x.action_goal.goal.grasp.grasp_pose.position.z, _x.action_goal.goal.grasp.grasp_pose.orientation.x, _x.action_goal.goal.grasp.grasp_pose.orientation.y, _x.action_goal.goal.grasp.grasp_pose.orientation.z, _x.action_goal.goal.grasp.grasp_pose.orientation.w, _x.action_goal.goal.grasp.success_probability, _x.action_goal.goal.grasp.cluster_rep, _x.action_goal.goal.grasp.desired_approach_distance, _x.action_goal.goal.grasp.min_approach_distance,) = _struct_8dB2f.unpack(str[start:end]) 02948 self.action_goal.goal.grasp.cluster_rep = bool(self.action_goal.goal.grasp.cluster_rep) 02949 start = end 02950 end += 4 02951 (length,) = _struct_I.unpack(str[start:end]) 02952 self.action_goal.goal.grasp.moved_obstacles = [] 02953 for i in range(0, length): 02954 val1 = object_manipulation_msgs.msg.GraspableObject() 02955 start = end 02956 end += 4 02957 (length,) = _struct_I.unpack(str[start:end]) 02958 start = end 02959 end += length 02960 val1.reference_frame_id = str[start:end] 02961 start = end 02962 end += 4 02963 (length,) = _struct_I.unpack(str[start:end]) 02964 val1.potential_models = [] 02965 for i in range(0, length): 02966 val2 = household_objects_database_msgs.msg.DatabaseModelPose() 02967 start = end 02968 end += 4 02969 (val2.model_id,) = _struct_i.unpack(str[start:end]) 02970 _v189 = val2.pose 02971 _v190 = _v189.header 02972 start = end 02973 end += 4 02974 (_v190.seq,) = _struct_I.unpack(str[start:end]) 02975 _v191 = _v190.stamp 02976 _x = _v191 02977 start = end 02978 end += 8 02979 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 02980 start = end 02981 end += 4 02982 (length,) = _struct_I.unpack(str[start:end]) 02983 start = end 02984 end += length 02985 _v190.frame_id = str[start:end] 02986 _v192 = _v189.pose 02987 _v193 = _v192.position 02988 _x = _v193 02989 start = end 02990 end += 24 02991 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 02992 _v194 = _v192.orientation 02993 _x = _v194 02994 start = end 02995 end += 32 02996 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 02997 start = end 02998 end += 4 02999 (val2.confidence,) = _struct_f.unpack(str[start:end]) 03000 start = end 03001 end += 4 03002 (length,) = _struct_I.unpack(str[start:end]) 03003 start = end 03004 end += length 03005 val2.detector_name = str[start:end] 03006 val1.potential_models.append(val2) 03007 _v195 = val1.cluster 03008 _v196 = _v195.header 03009 start = end 03010 end += 4 03011 (_v196.seq,) = _struct_I.unpack(str[start:end]) 03012 _v197 = _v196.stamp 03013 _x = _v197 03014 start = end 03015 end += 8 03016 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 03017 start = end 03018 end += 4 03019 (length,) = _struct_I.unpack(str[start:end]) 03020 start = end 03021 end += length 03022 _v196.frame_id = str[start:end] 03023 start = end 03024 end += 4 03025 (length,) = _struct_I.unpack(str[start:end]) 03026 _v195.points = [] 03027 for i in range(0, length): 03028 val3 = geometry_msgs.msg.Point32() 03029 _x = val3 03030 start = end 03031 end += 12 03032 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end]) 03033 _v195.points.append(val3) 03034 start = end 03035 end += 4 03036 (length,) = _struct_I.unpack(str[start:end]) 03037 _v195.channels = [] 03038 for i in range(0, length): 03039 val3 = sensor_msgs.msg.ChannelFloat32() 03040 start = end 03041 end += 4 03042 (length,) = _struct_I.unpack(str[start:end]) 03043 start = end 03044 end += length 03045 val3.name = str[start:end] 03046 start = end 03047 end += 4 03048 (length,) = _struct_I.unpack(str[start:end]) 03049 pattern = '<%sf'%length 03050 start = end 03051 end += struct.calcsize(pattern) 03052 val3.values = numpy.frombuffer(str[start:end], dtype=numpy.float32, count=length) 03053 _v195.channels.append(val3) 03054 _v198 = val1.region 03055 _v199 = _v198.cloud 03056 _v200 = _v199.header 03057 start = end 03058 end += 4 03059 (_v200.seq,) = _struct_I.unpack(str[start:end]) 03060 _v201 = _v200.stamp 03061 _x = _v201 03062 start = end 03063 end += 8 03064 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 03065 start = end 03066 end += 4 03067 (length,) = _struct_I.unpack(str[start:end]) 03068 start = end 03069 end += length 03070 _v200.frame_id = str[start:end] 03071 _x = _v199 03072 start = end 03073 end += 8 03074 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 03075 start = end 03076 end += 4 03077 (length,) = _struct_I.unpack(str[start:end]) 03078 _v199.fields = [] 03079 for i in range(0, length): 03080 val4 = sensor_msgs.msg.PointField() 03081 start = end 03082 end += 4 03083 (length,) = _struct_I.unpack(str[start:end]) 03084 start = end 03085 end += length 03086 val4.name = str[start:end] 03087 _x = val4 03088 start = end 03089 end += 9 03090 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end]) 03091 _v199.fields.append(val4) 03092 _x = _v199 03093 start = end 03094 end += 9 03095 (_x.is_bigendian, _x.point_step, _x.row_step,) = _struct_B2I.unpack(str[start:end]) 03096 _v199.is_bigendian = bool(_v199.is_bigendian) 03097 start = end 03098 end += 4 03099 (length,) = _struct_I.unpack(str[start:end]) 03100 start = end 03101 end += length 03102 _v199.data = str[start:end] 03103 start = end 03104 end += 1 03105 (_v199.is_dense,) = _struct_B.unpack(str[start:end]) 03106 _v199.is_dense = bool(_v199.is_dense) 03107 start = end 03108 end += 4 03109 (length,) = _struct_I.unpack(str[start:end]) 03110 pattern = '<%si'%length 03111 start = end 03112 end += struct.calcsize(pattern) 03113 _v198.mask = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length) 03114 _v202 = _v198.image 03115 _v203 = _v202.header 03116 start = end 03117 end += 4 03118 (_v203.seq,) = _struct_I.unpack(str[start:end]) 03119 _v204 = _v203.stamp 03120 _x = _v204 03121 start = end 03122 end += 8 03123 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 03124 start = end 03125 end += 4 03126 (length,) = _struct_I.unpack(str[start:end]) 03127 start = end 03128 end += length 03129 _v203.frame_id = str[start:end] 03130 _x = _v202 03131 start = end 03132 end += 8 03133 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 03134 start = end 03135 end += 4 03136 (length,) = _struct_I.unpack(str[start:end]) 03137 start = end 03138 end += length 03139 _v202.encoding = str[start:end] 03140 _x = _v202 03141 start = end 03142 end += 5 03143 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end]) 03144 start = end 03145 end += 4 03146 (length,) = _struct_I.unpack(str[start:end]) 03147 start = end 03148 end += length 03149 _v202.data = str[start:end] 03150 _v205 = _v198.disparity_image 03151 _v206 = _v205.header 03152 start = end 03153 end += 4 03154 (_v206.seq,) = _struct_I.unpack(str[start:end]) 03155 _v207 = _v206.stamp 03156 _x = _v207 03157 start = end 03158 end += 8 03159 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 03160 start = end 03161 end += 4 03162 (length,) = _struct_I.unpack(str[start:end]) 03163 start = end 03164 end += length 03165 _v206.frame_id = str[start:end] 03166 _x = _v205 03167 start = end 03168 end += 8 03169 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 03170 start = end 03171 end += 4 03172 (length,) = _struct_I.unpack(str[start:end]) 03173 start = end 03174 end += length 03175 _v205.encoding = str[start:end] 03176 _x = _v205 03177 start = end 03178 end += 5 03179 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end]) 03180 start = end 03181 end += 4 03182 (length,) = _struct_I.unpack(str[start:end]) 03183 start = end 03184 end += length 03185 _v205.data = str[start:end] 03186 _v208 = _v198.cam_info 03187 _v209 = _v208.header 03188 start = end 03189 end += 4 03190 (_v209.seq,) = _struct_I.unpack(str[start:end]) 03191 _v210 = _v209.stamp 03192 _x = _v210 03193 start = end 03194 end += 8 03195 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 03196 start = end 03197 end += 4 03198 (length,) = _struct_I.unpack(str[start:end]) 03199 start = end 03200 end += length 03201 _v209.frame_id = str[start:end] 03202 _x = _v208 03203 start = end 03204 end += 8 03205 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 03206 start = end 03207 end += 4 03208 (length,) = _struct_I.unpack(str[start:end]) 03209 start = end 03210 end += length 03211 _v208.distortion_model = str[start:end] 03212 start = end 03213 end += 4 03214 (length,) = _struct_I.unpack(str[start:end]) 03215 pattern = '<%sd'%length 03216 start = end 03217 end += struct.calcsize(pattern) 03218 _v208.D = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 03219 start = end 03220 end += 72 03221 _v208.K = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9) 03222 start = end 03223 end += 72 03224 _v208.R = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9) 03225 start = end 03226 end += 96 03227 _v208.P = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=12) 03228 _x = _v208 03229 start = end 03230 end += 8 03231 (_x.binning_x, _x.binning_y,) = _struct_2I.unpack(str[start:end]) 03232 _v211 = _v208.roi 03233 _x = _v211 03234 start = end 03235 end += 17 03236 (_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify,) = _struct_4IB.unpack(str[start:end]) 03237 _v211.do_rectify = bool(_v211.do_rectify) 03238 _v212 = _v198.roi_box_pose 03239 _v213 = _v212.header 03240 start = end 03241 end += 4 03242 (_v213.seq,) = _struct_I.unpack(str[start:end]) 03243 _v214 = _v213.stamp 03244 _x = _v214 03245 start = end 03246 end += 8 03247 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 03248 start = end 03249 end += 4 03250 (length,) = _struct_I.unpack(str[start:end]) 03251 start = end 03252 end += length 03253 _v213.frame_id = str[start:end] 03254 _v215 = _v212.pose 03255 _v216 = _v215.position 03256 _x = _v216 03257 start = end 03258 end += 24 03259 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 03260 _v217 = _v215.orientation 03261 _x = _v217 03262 start = end 03263 end += 32 03264 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 03265 _v218 = _v198.roi_box_dims 03266 _x = _v218 03267 start = end 03268 end += 24 03269 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 03270 start = end 03271 end += 4 03272 (length,) = _struct_I.unpack(str[start:end]) 03273 start = end 03274 end += length 03275 val1.collision_name = str[start:end] 03276 self.action_goal.goal.grasp.moved_obstacles.append(val1) 03277 _x = self 03278 start = end 03279 end += 20 03280 (_x.action_goal.goal.desired_retreat_distance, _x.action_goal.goal.min_retreat_distance, _x.action_goal.goal.approach.direction.header.seq, _x.action_goal.goal.approach.direction.header.stamp.secs, _x.action_goal.goal.approach.direction.header.stamp.nsecs,) = _struct_2f3I.unpack(str[start:end]) 03281 start = end 03282 end += 4 03283 (length,) = _struct_I.unpack(str[start:end]) 03284 start = end 03285 end += length 03286 self.action_goal.goal.approach.direction.header.frame_id = str[start:end] 03287 _x = self 03288 start = end 03289 end += 32 03290 (_x.action_goal.goal.approach.direction.vector.x, _x.action_goal.goal.approach.direction.vector.y, _x.action_goal.goal.approach.direction.vector.z, _x.action_goal.goal.approach.desired_distance, _x.action_goal.goal.approach.min_distance,) = _struct_3d2f.unpack(str[start:end]) 03291 start = end 03292 end += 4 03293 (length,) = _struct_I.unpack(str[start:end]) 03294 start = end 03295 end += length 03296 self.action_goal.goal.collision_object_name = str[start:end] 03297 start = end 03298 end += 4 03299 (length,) = _struct_I.unpack(str[start:end]) 03300 start = end 03301 end += length 03302 self.action_goal.goal.collision_support_surface_name = str[start:end] 03303 _x = self 03304 start = end 03305 end += 11 03306 (_x.action_goal.goal.allow_gripper_support_collision, _x.action_goal.goal.use_reactive_place, _x.action_goal.goal.place_padding, _x.action_goal.goal.only_perform_feasibility_test,) = _struct_2BdB.unpack(str[start:end]) 03307 self.action_goal.goal.allow_gripper_support_collision = bool(self.action_goal.goal.allow_gripper_support_collision) 03308 self.action_goal.goal.use_reactive_place = bool(self.action_goal.goal.use_reactive_place) 03309 self.action_goal.goal.only_perform_feasibility_test = bool(self.action_goal.goal.only_perform_feasibility_test) 03310 start = end 03311 end += 4 03312 (length,) = _struct_I.unpack(str[start:end]) 03313 self.action_goal.goal.path_constraints.joint_constraints = [] 03314 for i in range(0, length): 03315 val1 = arm_navigation_msgs.msg.JointConstraint() 03316 start = end 03317 end += 4 03318 (length,) = _struct_I.unpack(str[start:end]) 03319 start = end 03320 end += length 03321 val1.joint_name = str[start:end] 03322 _x = val1 03323 start = end 03324 end += 32 03325 (_x.position, _x.tolerance_above, _x.tolerance_below, _x.weight,) = _struct_4d.unpack(str[start:end]) 03326 self.action_goal.goal.path_constraints.joint_constraints.append(val1) 03327 start = end 03328 end += 4 03329 (length,) = _struct_I.unpack(str[start:end]) 03330 self.action_goal.goal.path_constraints.position_constraints = [] 03331 for i in range(0, length): 03332 val1 = arm_navigation_msgs.msg.PositionConstraint() 03333 _v219 = val1.header 03334 start = end 03335 end += 4 03336 (_v219.seq,) = _struct_I.unpack(str[start:end]) 03337 _v220 = _v219.stamp 03338 _x = _v220 03339 start = end 03340 end += 8 03341 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 03342 start = end 03343 end += 4 03344 (length,) = _struct_I.unpack(str[start:end]) 03345 start = end 03346 end += length 03347 _v219.frame_id = str[start:end] 03348 start = end 03349 end += 4 03350 (length,) = _struct_I.unpack(str[start:end]) 03351 start = end 03352 end += length 03353 val1.link_name = str[start:end] 03354 _v221 = val1.target_point_offset 03355 _x = _v221 03356 start = end 03357 end += 24 03358 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 03359 _v222 = val1.position 03360 _x = _v222 03361 start = end 03362 end += 24 03363 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 03364 _v223 = val1.constraint_region_shape 03365 start = end 03366 end += 1 03367 (_v223.type,) = _struct_b.unpack(str[start:end]) 03368 start = end 03369 end += 4 03370 (length,) = _struct_I.unpack(str[start:end]) 03371 pattern = '<%sd'%length 03372 start = end 03373 end += struct.calcsize(pattern) 03374 _v223.dimensions = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 03375 start = end 03376 end += 4 03377 (length,) = _struct_I.unpack(str[start:end]) 03378 pattern = '<%si'%length 03379 start = end 03380 end += struct.calcsize(pattern) 03381 _v223.triangles = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length) 03382 start = end 03383 end += 4 03384 (length,) = _struct_I.unpack(str[start:end]) 03385 _v223.vertices = [] 03386 for i in range(0, length): 03387 val3 = geometry_msgs.msg.Point() 03388 _x = val3 03389 start = end 03390 end += 24 03391 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 03392 _v223.vertices.append(val3) 03393 _v224 = val1.constraint_region_orientation 03394 _x = _v224 03395 start = end 03396 end += 32 03397 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 03398 start = end 03399 end += 8 03400 (val1.weight,) = _struct_d.unpack(str[start:end]) 03401 self.action_goal.goal.path_constraints.position_constraints.append(val1) 03402 start = end 03403 end += 4 03404 (length,) = _struct_I.unpack(str[start:end]) 03405 self.action_goal.goal.path_constraints.orientation_constraints = [] 03406 for i in range(0, length): 03407 val1 = arm_navigation_msgs.msg.OrientationConstraint() 03408 _v225 = val1.header 03409 start = end 03410 end += 4 03411 (_v225.seq,) = _struct_I.unpack(str[start:end]) 03412 _v226 = _v225.stamp 03413 _x = _v226 03414 start = end 03415 end += 8 03416 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 03417 start = end 03418 end += 4 03419 (length,) = _struct_I.unpack(str[start:end]) 03420 start = end 03421 end += length 03422 _v225.frame_id = str[start:end] 03423 start = end 03424 end += 4 03425 (length,) = _struct_I.unpack(str[start:end]) 03426 start = end 03427 end += length 03428 val1.link_name = str[start:end] 03429 start = end 03430 end += 4 03431 (val1.type,) = _struct_i.unpack(str[start:end]) 03432 _v227 = val1.orientation 03433 _x = _v227 03434 start = end 03435 end += 32 03436 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 03437 _x = val1 03438 start = end 03439 end += 32 03440 (_x.absolute_roll_tolerance, _x.absolute_pitch_tolerance, _x.absolute_yaw_tolerance, _x.weight,) = _struct_4d.unpack(str[start:end]) 03441 self.action_goal.goal.path_constraints.orientation_constraints.append(val1) 03442 start = end 03443 end += 4 03444 (length,) = _struct_I.unpack(str[start:end]) 03445 self.action_goal.goal.path_constraints.visibility_constraints = [] 03446 for i in range(0, length): 03447 val1 = arm_navigation_msgs.msg.VisibilityConstraint() 03448 _v228 = val1.header 03449 start = end 03450 end += 4 03451 (_v228.seq,) = _struct_I.unpack(str[start:end]) 03452 _v229 = _v228.stamp 03453 _x = _v229 03454 start = end 03455 end += 8 03456 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 03457 start = end 03458 end += 4 03459 (length,) = _struct_I.unpack(str[start:end]) 03460 start = end 03461 end += length 03462 _v228.frame_id = str[start:end] 03463 _v230 = val1.target 03464 _v231 = _v230.header 03465 start = end 03466 end += 4 03467 (_v231.seq,) = _struct_I.unpack(str[start:end]) 03468 _v232 = _v231.stamp 03469 _x = _v232 03470 start = end 03471 end += 8 03472 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 03473 start = end 03474 end += 4 03475 (length,) = _struct_I.unpack(str[start:end]) 03476 start = end 03477 end += length 03478 _v231.frame_id = str[start:end] 03479 _v233 = _v230.point 03480 _x = _v233 03481 start = end 03482 end += 24 03483 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 03484 _v234 = val1.sensor_pose 03485 _v235 = _v234.header 03486 start = end 03487 end += 4 03488 (_v235.seq,) = _struct_I.unpack(str[start:end]) 03489 _v236 = _v235.stamp 03490 _x = _v236 03491 start = end 03492 end += 8 03493 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 03494 start = end 03495 end += 4 03496 (length,) = _struct_I.unpack(str[start:end]) 03497 start = end 03498 end += length 03499 _v235.frame_id = str[start:end] 03500 _v237 = _v234.pose 03501 _v238 = _v237.position 03502 _x = _v238 03503 start = end 03504 end += 24 03505 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 03506 _v239 = _v237.orientation 03507 _x = _v239 03508 start = end 03509 end += 32 03510 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 03511 start = end 03512 end += 8 03513 (val1.absolute_tolerance,) = _struct_d.unpack(str[start:end]) 03514 self.action_goal.goal.path_constraints.visibility_constraints.append(val1) 03515 start = end 03516 end += 4 03517 (length,) = _struct_I.unpack(str[start:end]) 03518 self.action_goal.goal.additional_collision_operations.collision_operations = [] 03519 for i in range(0, length): 03520 val1 = arm_navigation_msgs.msg.CollisionOperation() 03521 start = end 03522 end += 4 03523 (length,) = _struct_I.unpack(str[start:end]) 03524 start = end 03525 end += length 03526 val1.object1 = str[start:end] 03527 start = end 03528 end += 4 03529 (length,) = _struct_I.unpack(str[start:end]) 03530 start = end 03531 end += length 03532 val1.object2 = str[start:end] 03533 _x = val1 03534 start = end 03535 end += 12 03536 (_x.penetration_distance, _x.operation,) = _struct_di.unpack(str[start:end]) 03537 self.action_goal.goal.additional_collision_operations.collision_operations.append(val1) 03538 start = end 03539 end += 4 03540 (length,) = _struct_I.unpack(str[start:end]) 03541 self.action_goal.goal.additional_link_padding = [] 03542 for i in range(0, length): 03543 val1 = arm_navigation_msgs.msg.LinkPadding() 03544 start = end 03545 end += 4 03546 (length,) = _struct_I.unpack(str[start:end]) 03547 start = end 03548 end += length 03549 val1.link_name = str[start:end] 03550 start = end 03551 end += 8 03552 (val1.padding,) = _struct_d.unpack(str[start:end]) 03553 self.action_goal.goal.additional_link_padding.append(val1) 03554 _x = self 03555 start = end 03556 end += 12 03557 (_x.action_result.header.seq, _x.action_result.header.stamp.secs, _x.action_result.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 03558 start = end 03559 end += 4 03560 (length,) = _struct_I.unpack(str[start:end]) 03561 start = end 03562 end += length 03563 self.action_result.header.frame_id = str[start:end] 03564 _x = self 03565 start = end 03566 end += 8 03567 (_x.action_result.status.goal_id.stamp.secs, _x.action_result.status.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end]) 03568 start = end 03569 end += 4 03570 (length,) = _struct_I.unpack(str[start:end]) 03571 start = end 03572 end += length 03573 self.action_result.status.goal_id.id = str[start:end] 03574 start = end 03575 end += 1 03576 (self.action_result.status.status,) = _struct_B.unpack(str[start:end]) 03577 start = end 03578 end += 4 03579 (length,) = _struct_I.unpack(str[start:end]) 03580 start = end 03581 end += length 03582 self.action_result.status.text = str[start:end] 03583 _x = self 03584 start = end 03585 end += 16 03586 (_x.action_result.result.manipulation_result.value, _x.action_result.result.place_location.header.seq, _x.action_result.result.place_location.header.stamp.secs, _x.action_result.result.place_location.header.stamp.nsecs,) = _struct_i3I.unpack(str[start:end]) 03587 start = end 03588 end += 4 03589 (length,) = _struct_I.unpack(str[start:end]) 03590 start = end 03591 end += length 03592 self.action_result.result.place_location.header.frame_id = str[start:end] 03593 _x = self 03594 start = end 03595 end += 56 03596 (_x.action_result.result.place_location.pose.position.x, _x.action_result.result.place_location.pose.position.y, _x.action_result.result.place_location.pose.position.z, _x.action_result.result.place_location.pose.orientation.x, _x.action_result.result.place_location.pose.orientation.y, _x.action_result.result.place_location.pose.orientation.z, _x.action_result.result.place_location.pose.orientation.w,) = _struct_7d.unpack(str[start:end]) 03597 start = end 03598 end += 4 03599 (length,) = _struct_I.unpack(str[start:end]) 03600 self.action_result.result.attempted_locations = [] 03601 for i in range(0, length): 03602 val1 = geometry_msgs.msg.PoseStamped() 03603 _v240 = val1.header 03604 start = end 03605 end += 4 03606 (_v240.seq,) = _struct_I.unpack(str[start:end]) 03607 _v241 = _v240.stamp 03608 _x = _v241 03609 start = end 03610 end += 8 03611 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 03612 start = end 03613 end += 4 03614 (length,) = _struct_I.unpack(str[start:end]) 03615 start = end 03616 end += length 03617 _v240.frame_id = str[start:end] 03618 _v242 = val1.pose 03619 _v243 = _v242.position 03620 _x = _v243 03621 start = end 03622 end += 24 03623 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 03624 _v244 = _v242.orientation 03625 _x = _v244 03626 start = end 03627 end += 32 03628 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 03629 self.action_result.result.attempted_locations.append(val1) 03630 start = end 03631 end += 4 03632 (length,) = _struct_I.unpack(str[start:end]) 03633 self.action_result.result.attempted_location_results = [] 03634 for i in range(0, length): 03635 val1 = object_manipulation_msgs.msg.PlaceLocationResult() 03636 _x = val1 03637 start = end 03638 end += 5 03639 (_x.result_code, _x.continuation_possible,) = _struct_iB.unpack(str[start:end]) 03640 val1.continuation_possible = bool(val1.continuation_possible) 03641 self.action_result.result.attempted_location_results.append(val1) 03642 _x = self 03643 start = end 03644 end += 12 03645 (_x.action_feedback.header.seq, _x.action_feedback.header.stamp.secs, _x.action_feedback.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 03646 start = end 03647 end += 4 03648 (length,) = _struct_I.unpack(str[start:end]) 03649 start = end 03650 end += length 03651 self.action_feedback.header.frame_id = str[start:end] 03652 _x = self 03653 start = end 03654 end += 8 03655 (_x.action_feedback.status.goal_id.stamp.secs, _x.action_feedback.status.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end]) 03656 start = end 03657 end += 4 03658 (length,) = _struct_I.unpack(str[start:end]) 03659 start = end 03660 end += length 03661 self.action_feedback.status.goal_id.id = str[start:end] 03662 start = end 03663 end += 1 03664 (self.action_feedback.status.status,) = _struct_B.unpack(str[start:end]) 03665 start = end 03666 end += 4 03667 (length,) = _struct_I.unpack(str[start:end]) 03668 start = end 03669 end += length 03670 self.action_feedback.status.text = str[start:end] 03671 _x = self 03672 start = end 03673 end += 8 03674 (_x.action_feedback.feedback.current_location, _x.action_feedback.feedback.total_locations,) = _struct_2i.unpack(str[start:end]) 03675 return self 03676 except struct.error as e: 03677 raise roslib.message.DeserializationError(e) #most likely buffer underfill 03678 03679 _struct_I = roslib.message.struct_I 03680 _struct_IBI = struct.Struct("<IBI") 03681 _struct_7d = struct.Struct("<7d") 03682 _struct_12d = struct.Struct("<12d") 03683 _struct_BI = struct.Struct("<BI") 03684 _struct_2BdB = struct.Struct("<2BdB") 03685 _struct_3I = struct.Struct("<3I") 03686 _struct_B2I = struct.Struct("<B2I") 03687 _struct_8dB2f = struct.Struct("<8dB2f") 03688 _struct_2f3I = struct.Struct("<2f3I") 03689 _struct_i3I = struct.Struct("<i3I") 03690 _struct_iB = struct.Struct("<iB") 03691 _struct_3f = struct.Struct("<3f") 03692 _struct_3d = struct.Struct("<3d") 03693 _struct_B = struct.Struct("<B") 03694 _struct_di = struct.Struct("<di") 03695 _struct_9d = struct.Struct("<9d") 03696 _struct_2I = struct.Struct("<2I") 03697 _struct_b = struct.Struct("<b") 03698 _struct_d = struct.Struct("<d") 03699 _struct_f = struct.Struct("<f") 03700 _struct_i = struct.Struct("<i") 03701 _struct_3d2f = struct.Struct("<3d2f") 03702 _struct_4d = struct.Struct("<4d") 03703 _struct_2i = struct.Struct("<2i") 03704 _struct_4IB = struct.Struct("<4IB")