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