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