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