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