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