$search
00001 """autogenerated by genmsg_py from ReactiveLiftGoal.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 ReactiveLiftGoal(roslib.message.Message): 00014 _md5sum = "ccf1b7602acb55923fcdaeb7ef4ae5e8" 00015 _type = "object_manipulation_msgs/ReactiveLiftGoal" 00016 _has_header = False #flag to mark the presence of a Header object 00017 _full_text = """# ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ====== 00018 00019 # the name of the arm being used 00020 string arm_name 00021 00022 # the object to be grasped 00023 GraspableObject target 00024 00025 # How the object should be lifted 00026 GripperTranslation lift 00027 00028 # the joint trajectory to use for the approach (if available) 00029 # this trajectory is expected to start at the current pose of the gripper 00030 # and end at the desired grasp pose 00031 trajectory_msgs/JointTrajectory trajectory 00032 00033 # the name of the support surface in the collision environment, if any 00034 string collision_support_surface_name 00035 00036 00037 ================================================================================ 00038 MSG: object_manipulation_msgs/GraspableObject 00039 # an object that the object_manipulator can work on 00040 00041 # a graspable object can be represented in multiple ways. This message 00042 # can contain all of them. Which one is actually used is up to the receiver 00043 # of this message. When adding new representations, one must be careful that 00044 # they have reasonable lightweight defaults indicating that that particular 00045 # representation is not available. 00046 00047 # the tf frame to be used as a reference frame when combining information from 00048 # the different representations below 00049 string reference_frame_id 00050 00051 # potential recognition results from a database of models 00052 # all poses are relative to the object reference pose 00053 household_objects_database_msgs/DatabaseModelPose[] potential_models 00054 00055 # the point cloud itself 00056 sensor_msgs/PointCloud cluster 00057 00058 # a region of a PointCloud2 of interest 00059 object_manipulation_msgs/SceneRegion region 00060 00061 # the name that this object has in the collision environment 00062 string collision_name 00063 ================================================================================ 00064 MSG: household_objects_database_msgs/DatabaseModelPose 00065 # Informs that a specific model from the Model Database has been 00066 # identified at a certain location 00067 00068 # the database id of the model 00069 int32 model_id 00070 00071 # the pose that it can be found in 00072 geometry_msgs/PoseStamped pose 00073 00074 # a measure of the confidence level in this detection result 00075 float32 confidence 00076 00077 # the name of the object detector that generated this detection result 00078 string detector_name 00079 00080 ================================================================================ 00081 MSG: geometry_msgs/PoseStamped 00082 # A Pose with reference coordinate frame and timestamp 00083 Header header 00084 Pose pose 00085 00086 ================================================================================ 00087 MSG: std_msgs/Header 00088 # Standard metadata for higher-level stamped data types. 00089 # This is generally used to communicate timestamped data 00090 # in a particular coordinate frame. 00091 # 00092 # sequence ID: consecutively increasing ID 00093 uint32 seq 00094 #Two-integer timestamp that is expressed as: 00095 # * stamp.secs: seconds (stamp_secs) since epoch 00096 # * stamp.nsecs: nanoseconds since stamp_secs 00097 # time-handling sugar is provided by the client library 00098 time stamp 00099 #Frame this data is associated with 00100 # 0: no frame 00101 # 1: global frame 00102 string frame_id 00103 00104 ================================================================================ 00105 MSG: geometry_msgs/Pose 00106 # A representation of pose in free space, composed of postion and orientation. 00107 Point position 00108 Quaternion orientation 00109 00110 ================================================================================ 00111 MSG: geometry_msgs/Point 00112 # This contains the position of a point in free space 00113 float64 x 00114 float64 y 00115 float64 z 00116 00117 ================================================================================ 00118 MSG: geometry_msgs/Quaternion 00119 # This represents an orientation in free space in quaternion form. 00120 00121 float64 x 00122 float64 y 00123 float64 z 00124 float64 w 00125 00126 ================================================================================ 00127 MSG: sensor_msgs/PointCloud 00128 # This message holds a collection of 3d points, plus optional additional 00129 # information about each point. 00130 00131 # Time of sensor data acquisition, coordinate frame ID. 00132 Header header 00133 00134 # Array of 3d points. Each Point32 should be interpreted as a 3d point 00135 # in the frame given in the header. 00136 geometry_msgs/Point32[] points 00137 00138 # Each channel should have the same number of elements as points array, 00139 # and the data in each channel should correspond 1:1 with each point. 00140 # Channel names in common practice are listed in ChannelFloat32.msg. 00141 ChannelFloat32[] channels 00142 00143 ================================================================================ 00144 MSG: geometry_msgs/Point32 00145 # This contains the position of a point in free space(with 32 bits of precision). 00146 # It is recommeded to use Point wherever possible instead of Point32. 00147 # 00148 # This recommendation is to promote interoperability. 00149 # 00150 # This message is designed to take up less space when sending 00151 # lots of points at once, as in the case of a PointCloud. 00152 00153 float32 x 00154 float32 y 00155 float32 z 00156 ================================================================================ 00157 MSG: sensor_msgs/ChannelFloat32 00158 # This message is used by the PointCloud message to hold optional data 00159 # associated with each point in the cloud. The length of the values 00160 # array should be the same as the length of the points array in the 00161 # PointCloud, and each value should be associated with the corresponding 00162 # point. 00163 00164 # Channel names in existing practice include: 00165 # "u", "v" - row and column (respectively) in the left stereo image. 00166 # This is opposite to usual conventions but remains for 00167 # historical reasons. The newer PointCloud2 message has no 00168 # such problem. 00169 # "rgb" - For point clouds produced by color stereo cameras. uint8 00170 # (R,G,B) values packed into the least significant 24 bits, 00171 # in order. 00172 # "intensity" - laser or pixel intensity. 00173 # "distance" 00174 00175 # The channel name should give semantics of the channel (e.g. 00176 # "intensity" instead of "value"). 00177 string name 00178 00179 # The values array should be 1-1 with the elements of the associated 00180 # PointCloud. 00181 float32[] values 00182 00183 ================================================================================ 00184 MSG: object_manipulation_msgs/SceneRegion 00185 # Point cloud 00186 sensor_msgs/PointCloud2 cloud 00187 00188 # Indices for the region of interest 00189 int32[] mask 00190 00191 # One of the corresponding 2D images, if applicable 00192 sensor_msgs/Image image 00193 00194 # The disparity image, if applicable 00195 sensor_msgs/Image disparity_image 00196 00197 # Camera info for the camera that took the image 00198 sensor_msgs/CameraInfo cam_info 00199 00200 # a 3D region of interest for grasp planning 00201 geometry_msgs/PoseStamped roi_box_pose 00202 geometry_msgs/Vector3 roi_box_dims 00203 00204 ================================================================================ 00205 MSG: sensor_msgs/PointCloud2 00206 # This message holds a collection of N-dimensional points, which may 00207 # contain additional information such as normals, intensity, etc. The 00208 # point data is stored as a binary blob, its layout described by the 00209 # contents of the "fields" array. 00210 00211 # The point cloud data may be organized 2d (image-like) or 1d 00212 # (unordered). Point clouds organized as 2d images may be produced by 00213 # camera depth sensors such as stereo or time-of-flight. 00214 00215 # Time of sensor data acquisition, and the coordinate frame ID (for 3d 00216 # points). 00217 Header header 00218 00219 # 2D structure of the point cloud. If the cloud is unordered, height is 00220 # 1 and width is the length of the point cloud. 00221 uint32 height 00222 uint32 width 00223 00224 # Describes the channels and their layout in the binary data blob. 00225 PointField[] fields 00226 00227 bool is_bigendian # Is this data bigendian? 00228 uint32 point_step # Length of a point in bytes 00229 uint32 row_step # Length of a row in bytes 00230 uint8[] data # Actual point data, size is (row_step*height) 00231 00232 bool is_dense # True if there are no invalid points 00233 00234 ================================================================================ 00235 MSG: sensor_msgs/PointField 00236 # This message holds the description of one point entry in the 00237 # PointCloud2 message format. 00238 uint8 INT8 = 1 00239 uint8 UINT8 = 2 00240 uint8 INT16 = 3 00241 uint8 UINT16 = 4 00242 uint8 INT32 = 5 00243 uint8 UINT32 = 6 00244 uint8 FLOAT32 = 7 00245 uint8 FLOAT64 = 8 00246 00247 string name # Name of field 00248 uint32 offset # Offset from start of point struct 00249 uint8 datatype # Datatype enumeration, see above 00250 uint32 count # How many elements in the field 00251 00252 ================================================================================ 00253 MSG: sensor_msgs/Image 00254 # This message contains an uncompressed image 00255 # (0, 0) is at top-left corner of image 00256 # 00257 00258 Header header # Header timestamp should be acquisition time of image 00259 # Header frame_id should be optical frame of camera 00260 # origin of frame should be optical center of cameara 00261 # +x should point to the right in the image 00262 # +y should point down in the image 00263 # +z should point into to plane of the image 00264 # If the frame_id here and the frame_id of the CameraInfo 00265 # message associated with the image conflict 00266 # the behavior is undefined 00267 00268 uint32 height # image height, that is, number of rows 00269 uint32 width # image width, that is, number of columns 00270 00271 # The legal values for encoding are in file src/image_encodings.cpp 00272 # If you want to standardize a new string format, join 00273 # ros-users@lists.sourceforge.net and send an email proposing a new encoding. 00274 00275 string encoding # Encoding of pixels -- channel meaning, ordering, size 00276 # taken from the list of strings in src/image_encodings.cpp 00277 00278 uint8 is_bigendian # is this data bigendian? 00279 uint32 step # Full row length in bytes 00280 uint8[] data # actual matrix data, size is (step * rows) 00281 00282 ================================================================================ 00283 MSG: sensor_msgs/CameraInfo 00284 # This message defines meta information for a camera. It should be in a 00285 # camera namespace on topic "camera_info" and accompanied by up to five 00286 # image topics named: 00287 # 00288 # image_raw - raw data from the camera driver, possibly Bayer encoded 00289 # image - monochrome, distorted 00290 # image_color - color, distorted 00291 # image_rect - monochrome, rectified 00292 # image_rect_color - color, rectified 00293 # 00294 # The image_pipeline contains packages (image_proc, stereo_image_proc) 00295 # for producing the four processed image topics from image_raw and 00296 # camera_info. The meaning of the camera parameters are described in 00297 # detail at http://www.ros.org/wiki/image_pipeline/CameraInfo. 00298 # 00299 # The image_geometry package provides a user-friendly interface to 00300 # common operations using this meta information. If you want to, e.g., 00301 # project a 3d point into image coordinates, we strongly recommend 00302 # using image_geometry. 00303 # 00304 # If the camera is uncalibrated, the matrices D, K, R, P should be left 00305 # zeroed out. In particular, clients may assume that K[0] == 0.0 00306 # indicates an uncalibrated camera. 00307 00308 ####################################################################### 00309 # Image acquisition info # 00310 ####################################################################### 00311 00312 # Time of image acquisition, camera coordinate frame ID 00313 Header header # Header timestamp should be acquisition time of image 00314 # Header frame_id should be optical frame of camera 00315 # origin of frame should be optical center of camera 00316 # +x should point to the right in the image 00317 # +y should point down in the image 00318 # +z should point into the plane of the image 00319 00320 00321 ####################################################################### 00322 # Calibration Parameters # 00323 ####################################################################### 00324 # These are fixed during camera calibration. Their values will be the # 00325 # same in all messages until the camera is recalibrated. Note that # 00326 # self-calibrating systems may "recalibrate" frequently. # 00327 # # 00328 # The internal parameters can be used to warp a raw (distorted) image # 00329 # to: # 00330 # 1. An undistorted image (requires D and K) # 00331 # 2. A rectified image (requires D, K, R) # 00332 # The projection matrix P projects 3D points into the rectified image.# 00333 ####################################################################### 00334 00335 # The image dimensions with which the camera was calibrated. Normally 00336 # this will be the full camera resolution in pixels. 00337 uint32 height 00338 uint32 width 00339 00340 # The distortion model used. Supported models are listed in 00341 # sensor_msgs/distortion_models.h. For most cameras, "plumb_bob" - a 00342 # simple model of radial and tangential distortion - is sufficent. 00343 string distortion_model 00344 00345 # The distortion parameters, size depending on the distortion model. 00346 # For "plumb_bob", the 5 parameters are: (k1, k2, t1, t2, k3). 00347 float64[] D 00348 00349 # Intrinsic camera matrix for the raw (distorted) images. 00350 # [fx 0 cx] 00351 # K = [ 0 fy cy] 00352 # [ 0 0 1] 00353 # Projects 3D points in the camera coordinate frame to 2D pixel 00354 # coordinates using the focal lengths (fx, fy) and principal point 00355 # (cx, cy). 00356 float64[9] K # 3x3 row-major matrix 00357 00358 # Rectification matrix (stereo cameras only) 00359 # A rotation matrix aligning the camera coordinate system to the ideal 00360 # stereo image plane so that epipolar lines in both stereo images are 00361 # parallel. 00362 float64[9] R # 3x3 row-major matrix 00363 00364 # Projection/camera matrix 00365 # [fx' 0 cx' Tx] 00366 # P = [ 0 fy' cy' Ty] 00367 # [ 0 0 1 0] 00368 # By convention, this matrix specifies the intrinsic (camera) matrix 00369 # of the processed (rectified) image. That is, the left 3x3 portion 00370 # is the normal camera intrinsic matrix for the rectified image. 00371 # It projects 3D points in the camera coordinate frame to 2D pixel 00372 # coordinates using the focal lengths (fx', fy') and principal point 00373 # (cx', cy') - these may differ from the values in K. 00374 # For monocular cameras, Tx = Ty = 0. Normally, monocular cameras will 00375 # also have R = the identity and P[1:3,1:3] = K. 00376 # For a stereo pair, the fourth column [Tx Ty 0]' is related to the 00377 # position of the optical center of the second camera in the first 00378 # camera's frame. We assume Tz = 0 so both cameras are in the same 00379 # stereo image plane. The first camera always has Tx = Ty = 0. For 00380 # the right (second) camera of a horizontal stereo pair, Ty = 0 and 00381 # Tx = -fx' * B, where B is the baseline between the cameras. 00382 # Given a 3D point [X Y Z]', the projection (x, y) of the point onto 00383 # the rectified image is given by: 00384 # [u v w]' = P * [X Y Z 1]' 00385 # x = u / w 00386 # y = v / w 00387 # This holds for both images of a stereo pair. 00388 float64[12] P # 3x4 row-major matrix 00389 00390 00391 ####################################################################### 00392 # Operational Parameters # 00393 ####################################################################### 00394 # These define the image region actually captured by the camera # 00395 # driver. Although they affect the geometry of the output image, they # 00396 # may be changed freely without recalibrating the camera. # 00397 ####################################################################### 00398 00399 # Binning refers here to any camera setting which combines rectangular 00400 # neighborhoods of pixels into larger "super-pixels." It reduces the 00401 # resolution of the output image to 00402 # (width / binning_x) x (height / binning_y). 00403 # The default values binning_x = binning_y = 0 is considered the same 00404 # as binning_x = binning_y = 1 (no subsampling). 00405 uint32 binning_x 00406 uint32 binning_y 00407 00408 # Region of interest (subwindow of full camera resolution), given in 00409 # full resolution (unbinned) image coordinates. A particular ROI 00410 # always denotes the same window of pixels on the camera sensor, 00411 # regardless of binning settings. 00412 # The default setting of roi (all values 0) is considered the same as 00413 # full resolution (roi.width = width, roi.height = height). 00414 RegionOfInterest roi 00415 00416 ================================================================================ 00417 MSG: sensor_msgs/RegionOfInterest 00418 # This message is used to specify a region of interest within an image. 00419 # 00420 # When used to specify the ROI setting of the camera when the image was 00421 # taken, the height and width fields should either match the height and 00422 # width fields for the associated image; or height = width = 0 00423 # indicates that the full resolution image was captured. 00424 00425 uint32 x_offset # Leftmost pixel of the ROI 00426 # (0 if the ROI includes the left edge of the image) 00427 uint32 y_offset # Topmost pixel of the ROI 00428 # (0 if the ROI includes the top edge of the image) 00429 uint32 height # Height of ROI 00430 uint32 width # Width of ROI 00431 00432 # True if a distinct rectified ROI should be calculated from the "raw" 00433 # ROI in this message. Typically this should be False if the full image 00434 # is captured (ROI not used), and True if a subwindow is captured (ROI 00435 # used). 00436 bool do_rectify 00437 00438 ================================================================================ 00439 MSG: geometry_msgs/Vector3 00440 # This represents a vector in free space. 00441 00442 float64 x 00443 float64 y 00444 float64 z 00445 ================================================================================ 00446 MSG: object_manipulation_msgs/GripperTranslation 00447 # defines a translation for the gripper, used in pickup or place tasks 00448 # for example for lifting an object off a table or approaching the table for placing 00449 00450 # the direction of the translation 00451 geometry_msgs/Vector3Stamped direction 00452 00453 # the desired translation distance 00454 float32 desired_distance 00455 00456 # the min distance that must be considered feasible before the 00457 # grasp is even attempted 00458 float32 min_distance 00459 ================================================================================ 00460 MSG: geometry_msgs/Vector3Stamped 00461 # This represents a Vector3 with reference coordinate frame and timestamp 00462 Header header 00463 Vector3 vector 00464 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 __slots__ = ['arm_name','target','lift','trajectory','collision_support_surface_name'] 00478 _slot_types = ['string','object_manipulation_msgs/GraspableObject','object_manipulation_msgs/GripperTranslation','trajectory_msgs/JointTrajectory','string'] 00479 00480 def __init__(self, *args, **kwds): 00481 """ 00482 Constructor. Any message fields that are implicitly/explicitly 00483 set to None will be assigned a default value. The recommend 00484 use is keyword arguments as this is more robust to future message 00485 changes. You cannot mix in-order arguments and keyword arguments. 00486 00487 The available fields are: 00488 arm_name,target,lift,trajectory,collision_support_surface_name 00489 00490 @param args: complete set of field values, in .msg order 00491 @param kwds: use keyword arguments corresponding to message field names 00492 to set specific fields. 00493 """ 00494 if args or kwds: 00495 super(ReactiveLiftGoal, self).__init__(*args, **kwds) 00496 #message fields cannot be None, assign default values for those that are 00497 if self.arm_name is None: 00498 self.arm_name = '' 00499 if self.target is None: 00500 self.target = object_manipulation_msgs.msg.GraspableObject() 00501 if self.lift is None: 00502 self.lift = object_manipulation_msgs.msg.GripperTranslation() 00503 if self.trajectory is None: 00504 self.trajectory = trajectory_msgs.msg.JointTrajectory() 00505 if self.collision_support_surface_name is None: 00506 self.collision_support_surface_name = '' 00507 else: 00508 self.arm_name = '' 00509 self.target = object_manipulation_msgs.msg.GraspableObject() 00510 self.lift = object_manipulation_msgs.msg.GripperTranslation() 00511 self.trajectory = trajectory_msgs.msg.JointTrajectory() 00512 self.collision_support_surface_name = '' 00513 00514 def _get_types(self): 00515 """ 00516 internal API method 00517 """ 00518 return self._slot_types 00519 00520 def serialize(self, buff): 00521 """ 00522 serialize message into buffer 00523 @param buff: buffer 00524 @type buff: StringIO 00525 """ 00526 try: 00527 _x = self.arm_name 00528 length = len(_x) 00529 buff.write(struct.pack('<I%ss'%length, length, _x)) 00530 _x = self.target.reference_frame_id 00531 length = len(_x) 00532 buff.write(struct.pack('<I%ss'%length, length, _x)) 00533 length = len(self.target.potential_models) 00534 buff.write(_struct_I.pack(length)) 00535 for val1 in self.target.potential_models: 00536 buff.write(_struct_i.pack(val1.model_id)) 00537 _v1 = val1.pose 00538 _v2 = _v1.header 00539 buff.write(_struct_I.pack(_v2.seq)) 00540 _v3 = _v2.stamp 00541 _x = _v3 00542 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00543 _x = _v2.frame_id 00544 length = len(_x) 00545 buff.write(struct.pack('<I%ss'%length, length, _x)) 00546 _v4 = _v1.pose 00547 _v5 = _v4.position 00548 _x = _v5 00549 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00550 _v6 = _v4.orientation 00551 _x = _v6 00552 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 00553 buff.write(_struct_f.pack(val1.confidence)) 00554 _x = val1.detector_name 00555 length = len(_x) 00556 buff.write(struct.pack('<I%ss'%length, length, _x)) 00557 _x = self 00558 buff.write(_struct_3I.pack(_x.target.cluster.header.seq, _x.target.cluster.header.stamp.secs, _x.target.cluster.header.stamp.nsecs)) 00559 _x = self.target.cluster.header.frame_id 00560 length = len(_x) 00561 buff.write(struct.pack('<I%ss'%length, length, _x)) 00562 length = len(self.target.cluster.points) 00563 buff.write(_struct_I.pack(length)) 00564 for val1 in self.target.cluster.points: 00565 _x = val1 00566 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z)) 00567 length = len(self.target.cluster.channels) 00568 buff.write(_struct_I.pack(length)) 00569 for val1 in self.target.cluster.channels: 00570 _x = val1.name 00571 length = len(_x) 00572 buff.write(struct.pack('<I%ss'%length, length, _x)) 00573 length = len(val1.values) 00574 buff.write(_struct_I.pack(length)) 00575 pattern = '<%sf'%length 00576 buff.write(struct.pack(pattern, *val1.values)) 00577 _x = self 00578 buff.write(_struct_3I.pack(_x.target.region.cloud.header.seq, _x.target.region.cloud.header.stamp.secs, _x.target.region.cloud.header.stamp.nsecs)) 00579 _x = self.target.region.cloud.header.frame_id 00580 length = len(_x) 00581 buff.write(struct.pack('<I%ss'%length, length, _x)) 00582 _x = self 00583 buff.write(_struct_2I.pack(_x.target.region.cloud.height, _x.target.region.cloud.width)) 00584 length = len(self.target.region.cloud.fields) 00585 buff.write(_struct_I.pack(length)) 00586 for val1 in self.target.region.cloud.fields: 00587 _x = val1.name 00588 length = len(_x) 00589 buff.write(struct.pack('<I%ss'%length, length, _x)) 00590 _x = val1 00591 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count)) 00592 _x = self 00593 buff.write(_struct_B2I.pack(_x.target.region.cloud.is_bigendian, _x.target.region.cloud.point_step, _x.target.region.cloud.row_step)) 00594 _x = self.target.region.cloud.data 00595 length = len(_x) 00596 # - if encoded as a list instead, serialize as bytes instead of string 00597 if type(_x) in [list, tuple]: 00598 buff.write(struct.pack('<I%sB'%length, length, *_x)) 00599 else: 00600 buff.write(struct.pack('<I%ss'%length, length, _x)) 00601 buff.write(_struct_B.pack(self.target.region.cloud.is_dense)) 00602 length = len(self.target.region.mask) 00603 buff.write(_struct_I.pack(length)) 00604 pattern = '<%si'%length 00605 buff.write(struct.pack(pattern, *self.target.region.mask)) 00606 _x = self 00607 buff.write(_struct_3I.pack(_x.target.region.image.header.seq, _x.target.region.image.header.stamp.secs, _x.target.region.image.header.stamp.nsecs)) 00608 _x = self.target.region.image.header.frame_id 00609 length = len(_x) 00610 buff.write(struct.pack('<I%ss'%length, length, _x)) 00611 _x = self 00612 buff.write(_struct_2I.pack(_x.target.region.image.height, _x.target.region.image.width)) 00613 _x = self.target.region.image.encoding 00614 length = len(_x) 00615 buff.write(struct.pack('<I%ss'%length, length, _x)) 00616 _x = self 00617 buff.write(_struct_BI.pack(_x.target.region.image.is_bigendian, _x.target.region.image.step)) 00618 _x = self.target.region.image.data 00619 length = len(_x) 00620 # - if encoded as a list instead, serialize as bytes instead of string 00621 if type(_x) in [list, tuple]: 00622 buff.write(struct.pack('<I%sB'%length, length, *_x)) 00623 else: 00624 buff.write(struct.pack('<I%ss'%length, length, _x)) 00625 _x = self 00626 buff.write(_struct_3I.pack(_x.target.region.disparity_image.header.seq, _x.target.region.disparity_image.header.stamp.secs, _x.target.region.disparity_image.header.stamp.nsecs)) 00627 _x = self.target.region.disparity_image.header.frame_id 00628 length = len(_x) 00629 buff.write(struct.pack('<I%ss'%length, length, _x)) 00630 _x = self 00631 buff.write(_struct_2I.pack(_x.target.region.disparity_image.height, _x.target.region.disparity_image.width)) 00632 _x = self.target.region.disparity_image.encoding 00633 length = len(_x) 00634 buff.write(struct.pack('<I%ss'%length, length, _x)) 00635 _x = self 00636 buff.write(_struct_BI.pack(_x.target.region.disparity_image.is_bigendian, _x.target.region.disparity_image.step)) 00637 _x = self.target.region.disparity_image.data 00638 length = len(_x) 00639 # - if encoded as a list instead, serialize as bytes instead of string 00640 if type(_x) in [list, tuple]: 00641 buff.write(struct.pack('<I%sB'%length, length, *_x)) 00642 else: 00643 buff.write(struct.pack('<I%ss'%length, length, _x)) 00644 _x = self 00645 buff.write(_struct_3I.pack(_x.target.region.cam_info.header.seq, _x.target.region.cam_info.header.stamp.secs, _x.target.region.cam_info.header.stamp.nsecs)) 00646 _x = self.target.region.cam_info.header.frame_id 00647 length = len(_x) 00648 buff.write(struct.pack('<I%ss'%length, length, _x)) 00649 _x = self 00650 buff.write(_struct_2I.pack(_x.target.region.cam_info.height, _x.target.region.cam_info.width)) 00651 _x = self.target.region.cam_info.distortion_model 00652 length = len(_x) 00653 buff.write(struct.pack('<I%ss'%length, length, _x)) 00654 length = len(self.target.region.cam_info.D) 00655 buff.write(_struct_I.pack(length)) 00656 pattern = '<%sd'%length 00657 buff.write(struct.pack(pattern, *self.target.region.cam_info.D)) 00658 buff.write(_struct_9d.pack(*self.target.region.cam_info.K)) 00659 buff.write(_struct_9d.pack(*self.target.region.cam_info.R)) 00660 buff.write(_struct_12d.pack(*self.target.region.cam_info.P)) 00661 _x = self 00662 buff.write(_struct_6IB3I.pack(_x.target.region.cam_info.binning_x, _x.target.region.cam_info.binning_y, _x.target.region.cam_info.roi.x_offset, _x.target.region.cam_info.roi.y_offset, _x.target.region.cam_info.roi.height, _x.target.region.cam_info.roi.width, _x.target.region.cam_info.roi.do_rectify, _x.target.region.roi_box_pose.header.seq, _x.target.region.roi_box_pose.header.stamp.secs, _x.target.region.roi_box_pose.header.stamp.nsecs)) 00663 _x = self.target.region.roi_box_pose.header.frame_id 00664 length = len(_x) 00665 buff.write(struct.pack('<I%ss'%length, length, _x)) 00666 _x = self 00667 buff.write(_struct_10d.pack(_x.target.region.roi_box_pose.pose.position.x, _x.target.region.roi_box_pose.pose.position.y, _x.target.region.roi_box_pose.pose.position.z, _x.target.region.roi_box_pose.pose.orientation.x, _x.target.region.roi_box_pose.pose.orientation.y, _x.target.region.roi_box_pose.pose.orientation.z, _x.target.region.roi_box_pose.pose.orientation.w, _x.target.region.roi_box_dims.x, _x.target.region.roi_box_dims.y, _x.target.region.roi_box_dims.z)) 00668 _x = self.target.collision_name 00669 length = len(_x) 00670 buff.write(struct.pack('<I%ss'%length, length, _x)) 00671 _x = self 00672 buff.write(_struct_3I.pack(_x.lift.direction.header.seq, _x.lift.direction.header.stamp.secs, _x.lift.direction.header.stamp.nsecs)) 00673 _x = self.lift.direction.header.frame_id 00674 length = len(_x) 00675 buff.write(struct.pack('<I%ss'%length, length, _x)) 00676 _x = self 00677 buff.write(_struct_3d2f3I.pack(_x.lift.direction.vector.x, _x.lift.direction.vector.y, _x.lift.direction.vector.z, _x.lift.desired_distance, _x.lift.min_distance, _x.trajectory.header.seq, _x.trajectory.header.stamp.secs, _x.trajectory.header.stamp.nsecs)) 00678 _x = self.trajectory.header.frame_id 00679 length = len(_x) 00680 buff.write(struct.pack('<I%ss'%length, length, _x)) 00681 length = len(self.trajectory.joint_names) 00682 buff.write(_struct_I.pack(length)) 00683 for val1 in self.trajectory.joint_names: 00684 length = len(val1) 00685 buff.write(struct.pack('<I%ss'%length, length, val1)) 00686 length = len(self.trajectory.points) 00687 buff.write(_struct_I.pack(length)) 00688 for val1 in self.trajectory.points: 00689 length = len(val1.positions) 00690 buff.write(_struct_I.pack(length)) 00691 pattern = '<%sd'%length 00692 buff.write(struct.pack(pattern, *val1.positions)) 00693 length = len(val1.velocities) 00694 buff.write(_struct_I.pack(length)) 00695 pattern = '<%sd'%length 00696 buff.write(struct.pack(pattern, *val1.velocities)) 00697 length = len(val1.accelerations) 00698 buff.write(_struct_I.pack(length)) 00699 pattern = '<%sd'%length 00700 buff.write(struct.pack(pattern, *val1.accelerations)) 00701 _v7 = val1.time_from_start 00702 _x = _v7 00703 buff.write(_struct_2i.pack(_x.secs, _x.nsecs)) 00704 _x = self.collision_support_surface_name 00705 length = len(_x) 00706 buff.write(struct.pack('<I%ss'%length, length, _x)) 00707 except struct.error as se: self._check_types(se) 00708 except TypeError as te: self._check_types(te) 00709 00710 def deserialize(self, str): 00711 """ 00712 unpack serialized message in str into this message instance 00713 @param str: byte array of serialized message 00714 @type str: str 00715 """ 00716 try: 00717 if self.target is None: 00718 self.target = object_manipulation_msgs.msg.GraspableObject() 00719 if self.lift is None: 00720 self.lift = object_manipulation_msgs.msg.GripperTranslation() 00721 if self.trajectory is None: 00722 self.trajectory = trajectory_msgs.msg.JointTrajectory() 00723 end = 0 00724 start = end 00725 end += 4 00726 (length,) = _struct_I.unpack(str[start:end]) 00727 start = end 00728 end += length 00729 self.arm_name = str[start:end] 00730 start = end 00731 end += 4 00732 (length,) = _struct_I.unpack(str[start:end]) 00733 start = end 00734 end += length 00735 self.target.reference_frame_id = str[start:end] 00736 start = end 00737 end += 4 00738 (length,) = _struct_I.unpack(str[start:end]) 00739 self.target.potential_models = [] 00740 for i in range(0, length): 00741 val1 = household_objects_database_msgs.msg.DatabaseModelPose() 00742 start = end 00743 end += 4 00744 (val1.model_id,) = _struct_i.unpack(str[start:end]) 00745 _v8 = val1.pose 00746 _v9 = _v8.header 00747 start = end 00748 end += 4 00749 (_v9.seq,) = _struct_I.unpack(str[start:end]) 00750 _v10 = _v9.stamp 00751 _x = _v10 00752 start = end 00753 end += 8 00754 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 00755 start = end 00756 end += 4 00757 (length,) = _struct_I.unpack(str[start:end]) 00758 start = end 00759 end += length 00760 _v9.frame_id = str[start:end] 00761 _v11 = _v8.pose 00762 _v12 = _v11.position 00763 _x = _v12 00764 start = end 00765 end += 24 00766 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 00767 _v13 = _v11.orientation 00768 _x = _v13 00769 start = end 00770 end += 32 00771 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 00772 start = end 00773 end += 4 00774 (val1.confidence,) = _struct_f.unpack(str[start:end]) 00775 start = end 00776 end += 4 00777 (length,) = _struct_I.unpack(str[start:end]) 00778 start = end 00779 end += length 00780 val1.detector_name = str[start:end] 00781 self.target.potential_models.append(val1) 00782 _x = self 00783 start = end 00784 end += 12 00785 (_x.target.cluster.header.seq, _x.target.cluster.header.stamp.secs, _x.target.cluster.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 00786 start = end 00787 end += 4 00788 (length,) = _struct_I.unpack(str[start:end]) 00789 start = end 00790 end += length 00791 self.target.cluster.header.frame_id = str[start:end] 00792 start = end 00793 end += 4 00794 (length,) = _struct_I.unpack(str[start:end]) 00795 self.target.cluster.points = [] 00796 for i in range(0, length): 00797 val1 = geometry_msgs.msg.Point32() 00798 _x = val1 00799 start = end 00800 end += 12 00801 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end]) 00802 self.target.cluster.points.append(val1) 00803 start = end 00804 end += 4 00805 (length,) = _struct_I.unpack(str[start:end]) 00806 self.target.cluster.channels = [] 00807 for i in range(0, length): 00808 val1 = sensor_msgs.msg.ChannelFloat32() 00809 start = end 00810 end += 4 00811 (length,) = _struct_I.unpack(str[start:end]) 00812 start = end 00813 end += length 00814 val1.name = str[start:end] 00815 start = end 00816 end += 4 00817 (length,) = _struct_I.unpack(str[start:end]) 00818 pattern = '<%sf'%length 00819 start = end 00820 end += struct.calcsize(pattern) 00821 val1.values = struct.unpack(pattern, str[start:end]) 00822 self.target.cluster.channels.append(val1) 00823 _x = self 00824 start = end 00825 end += 12 00826 (_x.target.region.cloud.header.seq, _x.target.region.cloud.header.stamp.secs, _x.target.region.cloud.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 00827 start = end 00828 end += 4 00829 (length,) = _struct_I.unpack(str[start:end]) 00830 start = end 00831 end += length 00832 self.target.region.cloud.header.frame_id = str[start:end] 00833 _x = self 00834 start = end 00835 end += 8 00836 (_x.target.region.cloud.height, _x.target.region.cloud.width,) = _struct_2I.unpack(str[start:end]) 00837 start = end 00838 end += 4 00839 (length,) = _struct_I.unpack(str[start:end]) 00840 self.target.region.cloud.fields = [] 00841 for i in range(0, length): 00842 val1 = sensor_msgs.msg.PointField() 00843 start = end 00844 end += 4 00845 (length,) = _struct_I.unpack(str[start:end]) 00846 start = end 00847 end += length 00848 val1.name = str[start:end] 00849 _x = val1 00850 start = end 00851 end += 9 00852 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end]) 00853 self.target.region.cloud.fields.append(val1) 00854 _x = self 00855 start = end 00856 end += 9 00857 (_x.target.region.cloud.is_bigendian, _x.target.region.cloud.point_step, _x.target.region.cloud.row_step,) = _struct_B2I.unpack(str[start:end]) 00858 self.target.region.cloud.is_bigendian = bool(self.target.region.cloud.is_bigendian) 00859 start = end 00860 end += 4 00861 (length,) = _struct_I.unpack(str[start:end]) 00862 start = end 00863 end += length 00864 self.target.region.cloud.data = str[start:end] 00865 start = end 00866 end += 1 00867 (self.target.region.cloud.is_dense,) = _struct_B.unpack(str[start:end]) 00868 self.target.region.cloud.is_dense = bool(self.target.region.cloud.is_dense) 00869 start = end 00870 end += 4 00871 (length,) = _struct_I.unpack(str[start:end]) 00872 pattern = '<%si'%length 00873 start = end 00874 end += struct.calcsize(pattern) 00875 self.target.region.mask = struct.unpack(pattern, str[start:end]) 00876 _x = self 00877 start = end 00878 end += 12 00879 (_x.target.region.image.header.seq, _x.target.region.image.header.stamp.secs, _x.target.region.image.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 00880 start = end 00881 end += 4 00882 (length,) = _struct_I.unpack(str[start:end]) 00883 start = end 00884 end += length 00885 self.target.region.image.header.frame_id = str[start:end] 00886 _x = self 00887 start = end 00888 end += 8 00889 (_x.target.region.image.height, _x.target.region.image.width,) = _struct_2I.unpack(str[start:end]) 00890 start = end 00891 end += 4 00892 (length,) = _struct_I.unpack(str[start:end]) 00893 start = end 00894 end += length 00895 self.target.region.image.encoding = str[start:end] 00896 _x = self 00897 start = end 00898 end += 5 00899 (_x.target.region.image.is_bigendian, _x.target.region.image.step,) = _struct_BI.unpack(str[start:end]) 00900 start = end 00901 end += 4 00902 (length,) = _struct_I.unpack(str[start:end]) 00903 start = end 00904 end += length 00905 self.target.region.image.data = str[start:end] 00906 _x = self 00907 start = end 00908 end += 12 00909 (_x.target.region.disparity_image.header.seq, _x.target.region.disparity_image.header.stamp.secs, _x.target.region.disparity_image.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 00910 start = end 00911 end += 4 00912 (length,) = _struct_I.unpack(str[start:end]) 00913 start = end 00914 end += length 00915 self.target.region.disparity_image.header.frame_id = str[start:end] 00916 _x = self 00917 start = end 00918 end += 8 00919 (_x.target.region.disparity_image.height, _x.target.region.disparity_image.width,) = _struct_2I.unpack(str[start:end]) 00920 start = end 00921 end += 4 00922 (length,) = _struct_I.unpack(str[start:end]) 00923 start = end 00924 end += length 00925 self.target.region.disparity_image.encoding = str[start:end] 00926 _x = self 00927 start = end 00928 end += 5 00929 (_x.target.region.disparity_image.is_bigendian, _x.target.region.disparity_image.step,) = _struct_BI.unpack(str[start:end]) 00930 start = end 00931 end += 4 00932 (length,) = _struct_I.unpack(str[start:end]) 00933 start = end 00934 end += length 00935 self.target.region.disparity_image.data = str[start:end] 00936 _x = self 00937 start = end 00938 end += 12 00939 (_x.target.region.cam_info.header.seq, _x.target.region.cam_info.header.stamp.secs, _x.target.region.cam_info.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 00940 start = end 00941 end += 4 00942 (length,) = _struct_I.unpack(str[start:end]) 00943 start = end 00944 end += length 00945 self.target.region.cam_info.header.frame_id = str[start:end] 00946 _x = self 00947 start = end 00948 end += 8 00949 (_x.target.region.cam_info.height, _x.target.region.cam_info.width,) = _struct_2I.unpack(str[start:end]) 00950 start = end 00951 end += 4 00952 (length,) = _struct_I.unpack(str[start:end]) 00953 start = end 00954 end += length 00955 self.target.region.cam_info.distortion_model = str[start:end] 00956 start = end 00957 end += 4 00958 (length,) = _struct_I.unpack(str[start:end]) 00959 pattern = '<%sd'%length 00960 start = end 00961 end += struct.calcsize(pattern) 00962 self.target.region.cam_info.D = struct.unpack(pattern, str[start:end]) 00963 start = end 00964 end += 72 00965 self.target.region.cam_info.K = _struct_9d.unpack(str[start:end]) 00966 start = end 00967 end += 72 00968 self.target.region.cam_info.R = _struct_9d.unpack(str[start:end]) 00969 start = end 00970 end += 96 00971 self.target.region.cam_info.P = _struct_12d.unpack(str[start:end]) 00972 _x = self 00973 start = end 00974 end += 37 00975 (_x.target.region.cam_info.binning_x, _x.target.region.cam_info.binning_y, _x.target.region.cam_info.roi.x_offset, _x.target.region.cam_info.roi.y_offset, _x.target.region.cam_info.roi.height, _x.target.region.cam_info.roi.width, _x.target.region.cam_info.roi.do_rectify, _x.target.region.roi_box_pose.header.seq, _x.target.region.roi_box_pose.header.stamp.secs, _x.target.region.roi_box_pose.header.stamp.nsecs,) = _struct_6IB3I.unpack(str[start:end]) 00976 self.target.region.cam_info.roi.do_rectify = bool(self.target.region.cam_info.roi.do_rectify) 00977 start = end 00978 end += 4 00979 (length,) = _struct_I.unpack(str[start:end]) 00980 start = end 00981 end += length 00982 self.target.region.roi_box_pose.header.frame_id = str[start:end] 00983 _x = self 00984 start = end 00985 end += 80 00986 (_x.target.region.roi_box_pose.pose.position.x, _x.target.region.roi_box_pose.pose.position.y, _x.target.region.roi_box_pose.pose.position.z, _x.target.region.roi_box_pose.pose.orientation.x, _x.target.region.roi_box_pose.pose.orientation.y, _x.target.region.roi_box_pose.pose.orientation.z, _x.target.region.roi_box_pose.pose.orientation.w, _x.target.region.roi_box_dims.x, _x.target.region.roi_box_dims.y, _x.target.region.roi_box_dims.z,) = _struct_10d.unpack(str[start:end]) 00987 start = end 00988 end += 4 00989 (length,) = _struct_I.unpack(str[start:end]) 00990 start = end 00991 end += length 00992 self.target.collision_name = str[start:end] 00993 _x = self 00994 start = end 00995 end += 12 00996 (_x.lift.direction.header.seq, _x.lift.direction.header.stamp.secs, _x.lift.direction.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 00997 start = end 00998 end += 4 00999 (length,) = _struct_I.unpack(str[start:end]) 01000 start = end 01001 end += length 01002 self.lift.direction.header.frame_id = str[start:end] 01003 _x = self 01004 start = end 01005 end += 44 01006 (_x.lift.direction.vector.x, _x.lift.direction.vector.y, _x.lift.direction.vector.z, _x.lift.desired_distance, _x.lift.min_distance, _x.trajectory.header.seq, _x.trajectory.header.stamp.secs, _x.trajectory.header.stamp.nsecs,) = _struct_3d2f3I.unpack(str[start:end]) 01007 start = end 01008 end += 4 01009 (length,) = _struct_I.unpack(str[start:end]) 01010 start = end 01011 end += length 01012 self.trajectory.header.frame_id = str[start:end] 01013 start = end 01014 end += 4 01015 (length,) = _struct_I.unpack(str[start:end]) 01016 self.trajectory.joint_names = [] 01017 for i in range(0, length): 01018 start = end 01019 end += 4 01020 (length,) = _struct_I.unpack(str[start:end]) 01021 start = end 01022 end += length 01023 val1 = str[start:end] 01024 self.trajectory.joint_names.append(val1) 01025 start = end 01026 end += 4 01027 (length,) = _struct_I.unpack(str[start:end]) 01028 self.trajectory.points = [] 01029 for i in range(0, length): 01030 val1 = trajectory_msgs.msg.JointTrajectoryPoint() 01031 start = end 01032 end += 4 01033 (length,) = _struct_I.unpack(str[start:end]) 01034 pattern = '<%sd'%length 01035 start = end 01036 end += struct.calcsize(pattern) 01037 val1.positions = struct.unpack(pattern, str[start:end]) 01038 start = end 01039 end += 4 01040 (length,) = _struct_I.unpack(str[start:end]) 01041 pattern = '<%sd'%length 01042 start = end 01043 end += struct.calcsize(pattern) 01044 val1.velocities = struct.unpack(pattern, str[start:end]) 01045 start = end 01046 end += 4 01047 (length,) = _struct_I.unpack(str[start:end]) 01048 pattern = '<%sd'%length 01049 start = end 01050 end += struct.calcsize(pattern) 01051 val1.accelerations = struct.unpack(pattern, str[start:end]) 01052 _v14 = val1.time_from_start 01053 _x = _v14 01054 start = end 01055 end += 8 01056 (_x.secs, _x.nsecs,) = _struct_2i.unpack(str[start:end]) 01057 self.trajectory.points.append(val1) 01058 start = end 01059 end += 4 01060 (length,) = _struct_I.unpack(str[start:end]) 01061 start = end 01062 end += length 01063 self.collision_support_surface_name = str[start:end] 01064 return self 01065 except struct.error as e: 01066 raise roslib.message.DeserializationError(e) #most likely buffer underfill 01067 01068 01069 def serialize_numpy(self, buff, numpy): 01070 """ 01071 serialize message with numpy array types into buffer 01072 @param buff: buffer 01073 @type buff: StringIO 01074 @param numpy: numpy python module 01075 @type numpy module 01076 """ 01077 try: 01078 _x = self.arm_name 01079 length = len(_x) 01080 buff.write(struct.pack('<I%ss'%length, length, _x)) 01081 _x = self.target.reference_frame_id 01082 length = len(_x) 01083 buff.write(struct.pack('<I%ss'%length, length, _x)) 01084 length = len(self.target.potential_models) 01085 buff.write(_struct_I.pack(length)) 01086 for val1 in self.target.potential_models: 01087 buff.write(_struct_i.pack(val1.model_id)) 01088 _v15 = val1.pose 01089 _v16 = _v15.header 01090 buff.write(_struct_I.pack(_v16.seq)) 01091 _v17 = _v16.stamp 01092 _x = _v17 01093 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 01094 _x = _v16.frame_id 01095 length = len(_x) 01096 buff.write(struct.pack('<I%ss'%length, length, _x)) 01097 _v18 = _v15.pose 01098 _v19 = _v18.position 01099 _x = _v19 01100 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 01101 _v20 = _v18.orientation 01102 _x = _v20 01103 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 01104 buff.write(_struct_f.pack(val1.confidence)) 01105 _x = val1.detector_name 01106 length = len(_x) 01107 buff.write(struct.pack('<I%ss'%length, length, _x)) 01108 _x = self 01109 buff.write(_struct_3I.pack(_x.target.cluster.header.seq, _x.target.cluster.header.stamp.secs, _x.target.cluster.header.stamp.nsecs)) 01110 _x = self.target.cluster.header.frame_id 01111 length = len(_x) 01112 buff.write(struct.pack('<I%ss'%length, length, _x)) 01113 length = len(self.target.cluster.points) 01114 buff.write(_struct_I.pack(length)) 01115 for val1 in self.target.cluster.points: 01116 _x = val1 01117 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z)) 01118 length = len(self.target.cluster.channels) 01119 buff.write(_struct_I.pack(length)) 01120 for val1 in self.target.cluster.channels: 01121 _x = val1.name 01122 length = len(_x) 01123 buff.write(struct.pack('<I%ss'%length, length, _x)) 01124 length = len(val1.values) 01125 buff.write(_struct_I.pack(length)) 01126 pattern = '<%sf'%length 01127 buff.write(val1.values.tostring()) 01128 _x = self 01129 buff.write(_struct_3I.pack(_x.target.region.cloud.header.seq, _x.target.region.cloud.header.stamp.secs, _x.target.region.cloud.header.stamp.nsecs)) 01130 _x = self.target.region.cloud.header.frame_id 01131 length = len(_x) 01132 buff.write(struct.pack('<I%ss'%length, length, _x)) 01133 _x = self 01134 buff.write(_struct_2I.pack(_x.target.region.cloud.height, _x.target.region.cloud.width)) 01135 length = len(self.target.region.cloud.fields) 01136 buff.write(_struct_I.pack(length)) 01137 for val1 in self.target.region.cloud.fields: 01138 _x = val1.name 01139 length = len(_x) 01140 buff.write(struct.pack('<I%ss'%length, length, _x)) 01141 _x = val1 01142 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count)) 01143 _x = self 01144 buff.write(_struct_B2I.pack(_x.target.region.cloud.is_bigendian, _x.target.region.cloud.point_step, _x.target.region.cloud.row_step)) 01145 _x = self.target.region.cloud.data 01146 length = len(_x) 01147 # - if encoded as a list instead, serialize as bytes instead of string 01148 if type(_x) in [list, tuple]: 01149 buff.write(struct.pack('<I%sB'%length, length, *_x)) 01150 else: 01151 buff.write(struct.pack('<I%ss'%length, length, _x)) 01152 buff.write(_struct_B.pack(self.target.region.cloud.is_dense)) 01153 length = len(self.target.region.mask) 01154 buff.write(_struct_I.pack(length)) 01155 pattern = '<%si'%length 01156 buff.write(self.target.region.mask.tostring()) 01157 _x = self 01158 buff.write(_struct_3I.pack(_x.target.region.image.header.seq, _x.target.region.image.header.stamp.secs, _x.target.region.image.header.stamp.nsecs)) 01159 _x = self.target.region.image.header.frame_id 01160 length = len(_x) 01161 buff.write(struct.pack('<I%ss'%length, length, _x)) 01162 _x = self 01163 buff.write(_struct_2I.pack(_x.target.region.image.height, _x.target.region.image.width)) 01164 _x = self.target.region.image.encoding 01165 length = len(_x) 01166 buff.write(struct.pack('<I%ss'%length, length, _x)) 01167 _x = self 01168 buff.write(_struct_BI.pack(_x.target.region.image.is_bigendian, _x.target.region.image.step)) 01169 _x = self.target.region.image.data 01170 length = len(_x) 01171 # - if encoded as a list instead, serialize as bytes instead of string 01172 if type(_x) in [list, tuple]: 01173 buff.write(struct.pack('<I%sB'%length, length, *_x)) 01174 else: 01175 buff.write(struct.pack('<I%ss'%length, length, _x)) 01176 _x = self 01177 buff.write(_struct_3I.pack(_x.target.region.disparity_image.header.seq, _x.target.region.disparity_image.header.stamp.secs, _x.target.region.disparity_image.header.stamp.nsecs)) 01178 _x = self.target.region.disparity_image.header.frame_id 01179 length = len(_x) 01180 buff.write(struct.pack('<I%ss'%length, length, _x)) 01181 _x = self 01182 buff.write(_struct_2I.pack(_x.target.region.disparity_image.height, _x.target.region.disparity_image.width)) 01183 _x = self.target.region.disparity_image.encoding 01184 length = len(_x) 01185 buff.write(struct.pack('<I%ss'%length, length, _x)) 01186 _x = self 01187 buff.write(_struct_BI.pack(_x.target.region.disparity_image.is_bigendian, _x.target.region.disparity_image.step)) 01188 _x = self.target.region.disparity_image.data 01189 length = len(_x) 01190 # - if encoded as a list instead, serialize as bytes instead of string 01191 if type(_x) in [list, tuple]: 01192 buff.write(struct.pack('<I%sB'%length, length, *_x)) 01193 else: 01194 buff.write(struct.pack('<I%ss'%length, length, _x)) 01195 _x = self 01196 buff.write(_struct_3I.pack(_x.target.region.cam_info.header.seq, _x.target.region.cam_info.header.stamp.secs, _x.target.region.cam_info.header.stamp.nsecs)) 01197 _x = self.target.region.cam_info.header.frame_id 01198 length = len(_x) 01199 buff.write(struct.pack('<I%ss'%length, length, _x)) 01200 _x = self 01201 buff.write(_struct_2I.pack(_x.target.region.cam_info.height, _x.target.region.cam_info.width)) 01202 _x = self.target.region.cam_info.distortion_model 01203 length = len(_x) 01204 buff.write(struct.pack('<I%ss'%length, length, _x)) 01205 length = len(self.target.region.cam_info.D) 01206 buff.write(_struct_I.pack(length)) 01207 pattern = '<%sd'%length 01208 buff.write(self.target.region.cam_info.D.tostring()) 01209 buff.write(self.target.region.cam_info.K.tostring()) 01210 buff.write(self.target.region.cam_info.R.tostring()) 01211 buff.write(self.target.region.cam_info.P.tostring()) 01212 _x = self 01213 buff.write(_struct_6IB3I.pack(_x.target.region.cam_info.binning_x, _x.target.region.cam_info.binning_y, _x.target.region.cam_info.roi.x_offset, _x.target.region.cam_info.roi.y_offset, _x.target.region.cam_info.roi.height, _x.target.region.cam_info.roi.width, _x.target.region.cam_info.roi.do_rectify, _x.target.region.roi_box_pose.header.seq, _x.target.region.roi_box_pose.header.stamp.secs, _x.target.region.roi_box_pose.header.stamp.nsecs)) 01214 _x = self.target.region.roi_box_pose.header.frame_id 01215 length = len(_x) 01216 buff.write(struct.pack('<I%ss'%length, length, _x)) 01217 _x = self 01218 buff.write(_struct_10d.pack(_x.target.region.roi_box_pose.pose.position.x, _x.target.region.roi_box_pose.pose.position.y, _x.target.region.roi_box_pose.pose.position.z, _x.target.region.roi_box_pose.pose.orientation.x, _x.target.region.roi_box_pose.pose.orientation.y, _x.target.region.roi_box_pose.pose.orientation.z, _x.target.region.roi_box_pose.pose.orientation.w, _x.target.region.roi_box_dims.x, _x.target.region.roi_box_dims.y, _x.target.region.roi_box_dims.z)) 01219 _x = self.target.collision_name 01220 length = len(_x) 01221 buff.write(struct.pack('<I%ss'%length, length, _x)) 01222 _x = self 01223 buff.write(_struct_3I.pack(_x.lift.direction.header.seq, _x.lift.direction.header.stamp.secs, _x.lift.direction.header.stamp.nsecs)) 01224 _x = self.lift.direction.header.frame_id 01225 length = len(_x) 01226 buff.write(struct.pack('<I%ss'%length, length, _x)) 01227 _x = self 01228 buff.write(_struct_3d2f3I.pack(_x.lift.direction.vector.x, _x.lift.direction.vector.y, _x.lift.direction.vector.z, _x.lift.desired_distance, _x.lift.min_distance, _x.trajectory.header.seq, _x.trajectory.header.stamp.secs, _x.trajectory.header.stamp.nsecs)) 01229 _x = self.trajectory.header.frame_id 01230 length = len(_x) 01231 buff.write(struct.pack('<I%ss'%length, length, _x)) 01232 length = len(self.trajectory.joint_names) 01233 buff.write(_struct_I.pack(length)) 01234 for val1 in self.trajectory.joint_names: 01235 length = len(val1) 01236 buff.write(struct.pack('<I%ss'%length, length, val1)) 01237 length = len(self.trajectory.points) 01238 buff.write(_struct_I.pack(length)) 01239 for val1 in self.trajectory.points: 01240 length = len(val1.positions) 01241 buff.write(_struct_I.pack(length)) 01242 pattern = '<%sd'%length 01243 buff.write(val1.positions.tostring()) 01244 length = len(val1.velocities) 01245 buff.write(_struct_I.pack(length)) 01246 pattern = '<%sd'%length 01247 buff.write(val1.velocities.tostring()) 01248 length = len(val1.accelerations) 01249 buff.write(_struct_I.pack(length)) 01250 pattern = '<%sd'%length 01251 buff.write(val1.accelerations.tostring()) 01252 _v21 = val1.time_from_start 01253 _x = _v21 01254 buff.write(_struct_2i.pack(_x.secs, _x.nsecs)) 01255 _x = self.collision_support_surface_name 01256 length = len(_x) 01257 buff.write(struct.pack('<I%ss'%length, length, _x)) 01258 except struct.error as se: self._check_types(se) 01259 except TypeError as te: self._check_types(te) 01260 01261 def deserialize_numpy(self, str, numpy): 01262 """ 01263 unpack serialized message in str into this message instance using numpy for array types 01264 @param str: byte array of serialized message 01265 @type str: str 01266 @param numpy: numpy python module 01267 @type numpy: module 01268 """ 01269 try: 01270 if self.target is None: 01271 self.target = object_manipulation_msgs.msg.GraspableObject() 01272 if self.lift is None: 01273 self.lift = object_manipulation_msgs.msg.GripperTranslation() 01274 if self.trajectory is None: 01275 self.trajectory = trajectory_msgs.msg.JointTrajectory() 01276 end = 0 01277 start = end 01278 end += 4 01279 (length,) = _struct_I.unpack(str[start:end]) 01280 start = end 01281 end += length 01282 self.arm_name = str[start:end] 01283 start = end 01284 end += 4 01285 (length,) = _struct_I.unpack(str[start:end]) 01286 start = end 01287 end += length 01288 self.target.reference_frame_id = str[start:end] 01289 start = end 01290 end += 4 01291 (length,) = _struct_I.unpack(str[start:end]) 01292 self.target.potential_models = [] 01293 for i in range(0, length): 01294 val1 = household_objects_database_msgs.msg.DatabaseModelPose() 01295 start = end 01296 end += 4 01297 (val1.model_id,) = _struct_i.unpack(str[start:end]) 01298 _v22 = val1.pose 01299 _v23 = _v22.header 01300 start = end 01301 end += 4 01302 (_v23.seq,) = _struct_I.unpack(str[start:end]) 01303 _v24 = _v23.stamp 01304 _x = _v24 01305 start = end 01306 end += 8 01307 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01308 start = end 01309 end += 4 01310 (length,) = _struct_I.unpack(str[start:end]) 01311 start = end 01312 end += length 01313 _v23.frame_id = str[start:end] 01314 _v25 = _v22.pose 01315 _v26 = _v25.position 01316 _x = _v26 01317 start = end 01318 end += 24 01319 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 01320 _v27 = _v25.orientation 01321 _x = _v27 01322 start = end 01323 end += 32 01324 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 01325 start = end 01326 end += 4 01327 (val1.confidence,) = _struct_f.unpack(str[start:end]) 01328 start = end 01329 end += 4 01330 (length,) = _struct_I.unpack(str[start:end]) 01331 start = end 01332 end += length 01333 val1.detector_name = str[start:end] 01334 self.target.potential_models.append(val1) 01335 _x = self 01336 start = end 01337 end += 12 01338 (_x.target.cluster.header.seq, _x.target.cluster.header.stamp.secs, _x.target.cluster.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 01339 start = end 01340 end += 4 01341 (length,) = _struct_I.unpack(str[start:end]) 01342 start = end 01343 end += length 01344 self.target.cluster.header.frame_id = str[start:end] 01345 start = end 01346 end += 4 01347 (length,) = _struct_I.unpack(str[start:end]) 01348 self.target.cluster.points = [] 01349 for i in range(0, length): 01350 val1 = geometry_msgs.msg.Point32() 01351 _x = val1 01352 start = end 01353 end += 12 01354 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end]) 01355 self.target.cluster.points.append(val1) 01356 start = end 01357 end += 4 01358 (length,) = _struct_I.unpack(str[start:end]) 01359 self.target.cluster.channels = [] 01360 for i in range(0, length): 01361 val1 = sensor_msgs.msg.ChannelFloat32() 01362 start = end 01363 end += 4 01364 (length,) = _struct_I.unpack(str[start:end]) 01365 start = end 01366 end += length 01367 val1.name = str[start:end] 01368 start = end 01369 end += 4 01370 (length,) = _struct_I.unpack(str[start:end]) 01371 pattern = '<%sf'%length 01372 start = end 01373 end += struct.calcsize(pattern) 01374 val1.values = numpy.frombuffer(str[start:end], dtype=numpy.float32, count=length) 01375 self.target.cluster.channels.append(val1) 01376 _x = self 01377 start = end 01378 end += 12 01379 (_x.target.region.cloud.header.seq, _x.target.region.cloud.header.stamp.secs, _x.target.region.cloud.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 01380 start = end 01381 end += 4 01382 (length,) = _struct_I.unpack(str[start:end]) 01383 start = end 01384 end += length 01385 self.target.region.cloud.header.frame_id = str[start:end] 01386 _x = self 01387 start = end 01388 end += 8 01389 (_x.target.region.cloud.height, _x.target.region.cloud.width,) = _struct_2I.unpack(str[start:end]) 01390 start = end 01391 end += 4 01392 (length,) = _struct_I.unpack(str[start:end]) 01393 self.target.region.cloud.fields = [] 01394 for i in range(0, length): 01395 val1 = sensor_msgs.msg.PointField() 01396 start = end 01397 end += 4 01398 (length,) = _struct_I.unpack(str[start:end]) 01399 start = end 01400 end += length 01401 val1.name = str[start:end] 01402 _x = val1 01403 start = end 01404 end += 9 01405 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end]) 01406 self.target.region.cloud.fields.append(val1) 01407 _x = self 01408 start = end 01409 end += 9 01410 (_x.target.region.cloud.is_bigendian, _x.target.region.cloud.point_step, _x.target.region.cloud.row_step,) = _struct_B2I.unpack(str[start:end]) 01411 self.target.region.cloud.is_bigendian = bool(self.target.region.cloud.is_bigendian) 01412 start = end 01413 end += 4 01414 (length,) = _struct_I.unpack(str[start:end]) 01415 start = end 01416 end += length 01417 self.target.region.cloud.data = str[start:end] 01418 start = end 01419 end += 1 01420 (self.target.region.cloud.is_dense,) = _struct_B.unpack(str[start:end]) 01421 self.target.region.cloud.is_dense = bool(self.target.region.cloud.is_dense) 01422 start = end 01423 end += 4 01424 (length,) = _struct_I.unpack(str[start:end]) 01425 pattern = '<%si'%length 01426 start = end 01427 end += struct.calcsize(pattern) 01428 self.target.region.mask = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length) 01429 _x = self 01430 start = end 01431 end += 12 01432 (_x.target.region.image.header.seq, _x.target.region.image.header.stamp.secs, _x.target.region.image.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 01433 start = end 01434 end += 4 01435 (length,) = _struct_I.unpack(str[start:end]) 01436 start = end 01437 end += length 01438 self.target.region.image.header.frame_id = str[start:end] 01439 _x = self 01440 start = end 01441 end += 8 01442 (_x.target.region.image.height, _x.target.region.image.width,) = _struct_2I.unpack(str[start:end]) 01443 start = end 01444 end += 4 01445 (length,) = _struct_I.unpack(str[start:end]) 01446 start = end 01447 end += length 01448 self.target.region.image.encoding = str[start:end] 01449 _x = self 01450 start = end 01451 end += 5 01452 (_x.target.region.image.is_bigendian, _x.target.region.image.step,) = _struct_BI.unpack(str[start:end]) 01453 start = end 01454 end += 4 01455 (length,) = _struct_I.unpack(str[start:end]) 01456 start = end 01457 end += length 01458 self.target.region.image.data = str[start:end] 01459 _x = self 01460 start = end 01461 end += 12 01462 (_x.target.region.disparity_image.header.seq, _x.target.region.disparity_image.header.stamp.secs, _x.target.region.disparity_image.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 01463 start = end 01464 end += 4 01465 (length,) = _struct_I.unpack(str[start:end]) 01466 start = end 01467 end += length 01468 self.target.region.disparity_image.header.frame_id = str[start:end] 01469 _x = self 01470 start = end 01471 end += 8 01472 (_x.target.region.disparity_image.height, _x.target.region.disparity_image.width,) = _struct_2I.unpack(str[start:end]) 01473 start = end 01474 end += 4 01475 (length,) = _struct_I.unpack(str[start:end]) 01476 start = end 01477 end += length 01478 self.target.region.disparity_image.encoding = str[start:end] 01479 _x = self 01480 start = end 01481 end += 5 01482 (_x.target.region.disparity_image.is_bigendian, _x.target.region.disparity_image.step,) = _struct_BI.unpack(str[start:end]) 01483 start = end 01484 end += 4 01485 (length,) = _struct_I.unpack(str[start:end]) 01486 start = end 01487 end += length 01488 self.target.region.disparity_image.data = str[start:end] 01489 _x = self 01490 start = end 01491 end += 12 01492 (_x.target.region.cam_info.header.seq, _x.target.region.cam_info.header.stamp.secs, _x.target.region.cam_info.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 01493 start = end 01494 end += 4 01495 (length,) = _struct_I.unpack(str[start:end]) 01496 start = end 01497 end += length 01498 self.target.region.cam_info.header.frame_id = str[start:end] 01499 _x = self 01500 start = end 01501 end += 8 01502 (_x.target.region.cam_info.height, _x.target.region.cam_info.width,) = _struct_2I.unpack(str[start:end]) 01503 start = end 01504 end += 4 01505 (length,) = _struct_I.unpack(str[start:end]) 01506 start = end 01507 end += length 01508 self.target.region.cam_info.distortion_model = str[start:end] 01509 start = end 01510 end += 4 01511 (length,) = _struct_I.unpack(str[start:end]) 01512 pattern = '<%sd'%length 01513 start = end 01514 end += struct.calcsize(pattern) 01515 self.target.region.cam_info.D = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 01516 start = end 01517 end += 72 01518 self.target.region.cam_info.K = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9) 01519 start = end 01520 end += 72 01521 self.target.region.cam_info.R = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9) 01522 start = end 01523 end += 96 01524 self.target.region.cam_info.P = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=12) 01525 _x = self 01526 start = end 01527 end += 37 01528 (_x.target.region.cam_info.binning_x, _x.target.region.cam_info.binning_y, _x.target.region.cam_info.roi.x_offset, _x.target.region.cam_info.roi.y_offset, _x.target.region.cam_info.roi.height, _x.target.region.cam_info.roi.width, _x.target.region.cam_info.roi.do_rectify, _x.target.region.roi_box_pose.header.seq, _x.target.region.roi_box_pose.header.stamp.secs, _x.target.region.roi_box_pose.header.stamp.nsecs,) = _struct_6IB3I.unpack(str[start:end]) 01529 self.target.region.cam_info.roi.do_rectify = bool(self.target.region.cam_info.roi.do_rectify) 01530 start = end 01531 end += 4 01532 (length,) = _struct_I.unpack(str[start:end]) 01533 start = end 01534 end += length 01535 self.target.region.roi_box_pose.header.frame_id = str[start:end] 01536 _x = self 01537 start = end 01538 end += 80 01539 (_x.target.region.roi_box_pose.pose.position.x, _x.target.region.roi_box_pose.pose.position.y, _x.target.region.roi_box_pose.pose.position.z, _x.target.region.roi_box_pose.pose.orientation.x, _x.target.region.roi_box_pose.pose.orientation.y, _x.target.region.roi_box_pose.pose.orientation.z, _x.target.region.roi_box_pose.pose.orientation.w, _x.target.region.roi_box_dims.x, _x.target.region.roi_box_dims.y, _x.target.region.roi_box_dims.z,) = _struct_10d.unpack(str[start:end]) 01540 start = end 01541 end += 4 01542 (length,) = _struct_I.unpack(str[start:end]) 01543 start = end 01544 end += length 01545 self.target.collision_name = str[start:end] 01546 _x = self 01547 start = end 01548 end += 12 01549 (_x.lift.direction.header.seq, _x.lift.direction.header.stamp.secs, _x.lift.direction.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 01550 start = end 01551 end += 4 01552 (length,) = _struct_I.unpack(str[start:end]) 01553 start = end 01554 end += length 01555 self.lift.direction.header.frame_id = str[start:end] 01556 _x = self 01557 start = end 01558 end += 44 01559 (_x.lift.direction.vector.x, _x.lift.direction.vector.y, _x.lift.direction.vector.z, _x.lift.desired_distance, _x.lift.min_distance, _x.trajectory.header.seq, _x.trajectory.header.stamp.secs, _x.trajectory.header.stamp.nsecs,) = _struct_3d2f3I.unpack(str[start:end]) 01560 start = end 01561 end += 4 01562 (length,) = _struct_I.unpack(str[start:end]) 01563 start = end 01564 end += length 01565 self.trajectory.header.frame_id = str[start:end] 01566 start = end 01567 end += 4 01568 (length,) = _struct_I.unpack(str[start:end]) 01569 self.trajectory.joint_names = [] 01570 for i in range(0, length): 01571 start = end 01572 end += 4 01573 (length,) = _struct_I.unpack(str[start:end]) 01574 start = end 01575 end += length 01576 val1 = str[start:end] 01577 self.trajectory.joint_names.append(val1) 01578 start = end 01579 end += 4 01580 (length,) = _struct_I.unpack(str[start:end]) 01581 self.trajectory.points = [] 01582 for i in range(0, length): 01583 val1 = trajectory_msgs.msg.JointTrajectoryPoint() 01584 start = end 01585 end += 4 01586 (length,) = _struct_I.unpack(str[start:end]) 01587 pattern = '<%sd'%length 01588 start = end 01589 end += struct.calcsize(pattern) 01590 val1.positions = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 01591 start = end 01592 end += 4 01593 (length,) = _struct_I.unpack(str[start:end]) 01594 pattern = '<%sd'%length 01595 start = end 01596 end += struct.calcsize(pattern) 01597 val1.velocities = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 01598 start = end 01599 end += 4 01600 (length,) = _struct_I.unpack(str[start:end]) 01601 pattern = '<%sd'%length 01602 start = end 01603 end += struct.calcsize(pattern) 01604 val1.accelerations = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 01605 _v28 = val1.time_from_start 01606 _x = _v28 01607 start = end 01608 end += 8 01609 (_x.secs, _x.nsecs,) = _struct_2i.unpack(str[start:end]) 01610 self.trajectory.points.append(val1) 01611 start = end 01612 end += 4 01613 (length,) = _struct_I.unpack(str[start:end]) 01614 start = end 01615 end += length 01616 self.collision_support_surface_name = str[start:end] 01617 return self 01618 except struct.error as e: 01619 raise roslib.message.DeserializationError(e) #most likely buffer underfill 01620 01621 _struct_I = roslib.message.struct_I 01622 _struct_IBI = struct.Struct("<IBI") 01623 _struct_6IB3I = struct.Struct("<6IB3I") 01624 _struct_B = struct.Struct("<B") 01625 _struct_12d = struct.Struct("<12d") 01626 _struct_f = struct.Struct("<f") 01627 _struct_i = struct.Struct("<i") 01628 _struct_BI = struct.Struct("<BI") 01629 _struct_10d = struct.Struct("<10d") 01630 _struct_3f = struct.Struct("<3f") 01631 _struct_2i = struct.Struct("<2i") 01632 _struct_3I = struct.Struct("<3I") 01633 _struct_9d = struct.Struct("<9d") 01634 _struct_B2I = struct.Struct("<B2I") 01635 _struct_3d2f3I = struct.Struct("<3d2f3I") 01636 _struct_4d = struct.Struct("<4d") 01637 _struct_2I = struct.Struct("<2I") 01638 _struct_3d = struct.Struct("<3d")