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