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