00001 """autogenerated by genpy from pr2_create_object_model/ModelObjectInHandAction.msg. Do not edit."""
00002 import sys
00003 python3 = True if sys.hexversion > 0x03000000 else False
00004 import genpy
00005 import struct
00006
00007 import actionlib_msgs.msg
00008 import pr2_create_object_model.msg
00009 import geometry_msgs.msg
00010 import sensor_msgs.msg
00011 import genpy
00012 import std_msgs.msg
00013
00014 class ModelObjectInHandAction(genpy.Message):
00015 _md5sum = "48f36e754cdbe5df61db81cd410cecf9"
00016 _type = "pr2_create_object_model/ModelObjectInHandAction"
00017 _has_header = False
00018 _full_text = """# ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00019
00020 ModelObjectInHandActionGoal action_goal
00021 ModelObjectInHandActionResult action_result
00022 ModelObjectInHandActionFeedback action_feedback
00023
00024 ================================================================================
00025 MSG: pr2_create_object_model/ModelObjectInHandActionGoal
00026 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00027
00028 Header header
00029 actionlib_msgs/GoalID goal_id
00030 ModelObjectInHandGoal goal
00031
00032 ================================================================================
00033 MSG: std_msgs/Header
00034 # Standard metadata for higher-level stamped data types.
00035 # This is generally used to communicate timestamped data
00036 # in a particular coordinate frame.
00037 #
00038 # sequence ID: consecutively increasing ID
00039 uint32 seq
00040 #Two-integer timestamp that is expressed as:
00041 # * stamp.secs: seconds (stamp_secs) since epoch
00042 # * stamp.nsecs: nanoseconds since stamp_secs
00043 # time-handling sugar is provided by the client library
00044 time stamp
00045 #Frame this data is associated with
00046 # 0: no frame
00047 # 1: global frame
00048 string frame_id
00049
00050 ================================================================================
00051 MSG: actionlib_msgs/GoalID
00052 # The stamp should store the time at which this goal was requested.
00053 # It is used by an action server when it tries to preempt all
00054 # goals that were requested before a certain time
00055 time stamp
00056
00057 # The id provides a way to associate feedback and
00058 # result message with specific goal requests. The id
00059 # specified must be unique.
00060 string id
00061
00062
00063 ================================================================================
00064 MSG: pr2_create_object_model/ModelObjectInHandGoal
00065 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00066 # which arm to use
00067 string arm_name
00068
00069 # the relative motion in which to move the gripper to get it clear of objects
00070 #(if left unfilled, will not move)
00071 geometry_msgs/Vector3Stamped clear_move
00072
00073 # the pose to go to for rotating (if left unfilled, will not go)
00074 geometry_msgs/PoseStamped rotate_pose
00075
00076 # whether to rotate the object at rotate_pose to build up the model
00077 uint8 rotate_object
00078
00079 # whether to add the object to the collision map
00080 uint8 add_to_collision_map
00081
00082 # whether to keep the object level while rotating
00083 uint8 keep_level
00084
00085
00086 ================================================================================
00087 MSG: geometry_msgs/Vector3Stamped
00088 # This represents a Vector3 with reference coordinate frame and timestamp
00089 Header header
00090 Vector3 vector
00091
00092 ================================================================================
00093 MSG: geometry_msgs/Vector3
00094 # This represents a vector in free space.
00095
00096 float64 x
00097 float64 y
00098 float64 z
00099 ================================================================================
00100 MSG: geometry_msgs/PoseStamped
00101 # A Pose with reference coordinate frame and timestamp
00102 Header header
00103 Pose pose
00104
00105 ================================================================================
00106 MSG: geometry_msgs/Pose
00107 # A representation of pose in free space, composed of postion and orientation.
00108 Point position
00109 Quaternion orientation
00110
00111 ================================================================================
00112 MSG: geometry_msgs/Point
00113 # This contains the position of a point in free space
00114 float64 x
00115 float64 y
00116 float64 z
00117
00118 ================================================================================
00119 MSG: geometry_msgs/Quaternion
00120 # This represents an orientation in free space in quaternion form.
00121
00122 float64 x
00123 float64 y
00124 float64 z
00125 float64 w
00126
00127 ================================================================================
00128 MSG: pr2_create_object_model/ModelObjectInHandActionResult
00129 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00130
00131 Header header
00132 actionlib_msgs/GoalStatus status
00133 ModelObjectInHandResult result
00134
00135 ================================================================================
00136 MSG: actionlib_msgs/GoalStatus
00137 GoalID goal_id
00138 uint8 status
00139 uint8 PENDING = 0 # The goal has yet to be processed by the action server
00140 uint8 ACTIVE = 1 # The goal is currently being processed by the action server
00141 uint8 PREEMPTED = 2 # The goal received a cancel request after it started executing
00142 # and has since completed its execution (Terminal State)
00143 uint8 SUCCEEDED = 3 # The goal was achieved successfully by the action server (Terminal State)
00144 uint8 ABORTED = 4 # The goal was aborted during execution by the action server due
00145 # to some failure (Terminal State)
00146 uint8 REJECTED = 5 # The goal was rejected by the action server without being processed,
00147 # because the goal was unattainable or invalid (Terminal State)
00148 uint8 PREEMPTING = 6 # The goal received a cancel request after it started executing
00149 # and has not yet completed execution
00150 uint8 RECALLING = 7 # The goal received a cancel request before it started executing,
00151 # but the action server has not yet confirmed that the goal is canceled
00152 uint8 RECALLED = 8 # The goal received a cancel request before it started executing
00153 # and was successfully cancelled (Terminal State)
00154 uint8 LOST = 9 # An action client can determine that a goal is LOST. This should not be
00155 # sent over the wire by an action server
00156
00157 #Allow for the user to associate a string with GoalStatus for debugging
00158 string text
00159
00160
00161 ================================================================================
00162 MSG: pr2_create_object_model/ModelObjectInHandResult
00163 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00164
00165 # the resulting object point cloud
00166 sensor_msgs/PointCloud2 cluster
00167
00168 # the resulting collision name, if added to the collision map
00169 string collision_name
00170
00171
00172 ================================================================================
00173 MSG: sensor_msgs/PointCloud2
00174 # This message holds a collection of N-dimensional points, which may
00175 # contain additional information such as normals, intensity, etc. The
00176 # point data is stored as a binary blob, its layout described by the
00177 # contents of the "fields" array.
00178
00179 # The point cloud data may be organized 2d (image-like) or 1d
00180 # (unordered). Point clouds organized as 2d images may be produced by
00181 # camera depth sensors such as stereo or time-of-flight.
00182
00183 # Time of sensor data acquisition, and the coordinate frame ID (for 3d
00184 # points).
00185 Header header
00186
00187 # 2D structure of the point cloud. If the cloud is unordered, height is
00188 # 1 and width is the length of the point cloud.
00189 uint32 height
00190 uint32 width
00191
00192 # Describes the channels and their layout in the binary data blob.
00193 PointField[] fields
00194
00195 bool is_bigendian # Is this data bigendian?
00196 uint32 point_step # Length of a point in bytes
00197 uint32 row_step # Length of a row in bytes
00198 uint8[] data # Actual point data, size is (row_step*height)
00199
00200 bool is_dense # True if there are no invalid points
00201
00202 ================================================================================
00203 MSG: sensor_msgs/PointField
00204 # This message holds the description of one point entry in the
00205 # PointCloud2 message format.
00206 uint8 INT8 = 1
00207 uint8 UINT8 = 2
00208 uint8 INT16 = 3
00209 uint8 UINT16 = 4
00210 uint8 INT32 = 5
00211 uint8 UINT32 = 6
00212 uint8 FLOAT32 = 7
00213 uint8 FLOAT64 = 8
00214
00215 string name # Name of field
00216 uint32 offset # Offset from start of point struct
00217 uint8 datatype # Datatype enumeration, see above
00218 uint32 count # How many elements in the field
00219
00220 ================================================================================
00221 MSG: pr2_create_object_model/ModelObjectInHandActionFeedback
00222 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00223
00224 Header header
00225 actionlib_msgs/GoalStatus status
00226 ModelObjectInHandFeedback feedback
00227
00228 ================================================================================
00229 MSG: pr2_create_object_model/ModelObjectInHandFeedback
00230 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00231
00232 # which phase the process is in
00233 int32 phase
00234 int32 BEFORE_MOVE=0
00235 int32 CLEAR_MOVE=1
00236 int32 MOVE_TO_ROTATE_POSE=2
00237 int32 ROTATING=3
00238 int32 DONE=4
00239
00240 # how many rotate-poses have we gone to/are we in now
00241 int32 rotate_ind
00242
00243
00244 """
00245 __slots__ = ['action_goal','action_result','action_feedback']
00246 _slot_types = ['pr2_create_object_model/ModelObjectInHandActionGoal','pr2_create_object_model/ModelObjectInHandActionResult','pr2_create_object_model/ModelObjectInHandActionFeedback']
00247
00248 def __init__(self, *args, **kwds):
00249 """
00250 Constructor. Any message fields that are implicitly/explicitly
00251 set to None will be assigned a default value. The recommend
00252 use is keyword arguments as this is more robust to future message
00253 changes. You cannot mix in-order arguments and keyword arguments.
00254
00255 The available fields are:
00256 action_goal,action_result,action_feedback
00257
00258 :param args: complete set of field values, in .msg order
00259 :param kwds: use keyword arguments corresponding to message field names
00260 to set specific fields.
00261 """
00262 if args or kwds:
00263 super(ModelObjectInHandAction, self).__init__(*args, **kwds)
00264
00265 if self.action_goal is None:
00266 self.action_goal = pr2_create_object_model.msg.ModelObjectInHandActionGoal()
00267 if self.action_result is None:
00268 self.action_result = pr2_create_object_model.msg.ModelObjectInHandActionResult()
00269 if self.action_feedback is None:
00270 self.action_feedback = pr2_create_object_model.msg.ModelObjectInHandActionFeedback()
00271 else:
00272 self.action_goal = pr2_create_object_model.msg.ModelObjectInHandActionGoal()
00273 self.action_result = pr2_create_object_model.msg.ModelObjectInHandActionResult()
00274 self.action_feedback = pr2_create_object_model.msg.ModelObjectInHandActionFeedback()
00275
00276 def _get_types(self):
00277 """
00278 internal API method
00279 """
00280 return self._slot_types
00281
00282 def serialize(self, buff):
00283 """
00284 serialize message into buffer
00285 :param buff: buffer, ``StringIO``
00286 """
00287 try:
00288 _x = self
00289 buff.write(_struct_3I.pack(_x.action_goal.header.seq, _x.action_goal.header.stamp.secs, _x.action_goal.header.stamp.nsecs))
00290 _x = self.action_goal.header.frame_id
00291 length = len(_x)
00292 if python3 or type(_x) == unicode:
00293 _x = _x.encode('utf-8')
00294 length = len(_x)
00295 buff.write(struct.pack('<I%ss'%length, length, _x))
00296 _x = self
00297 buff.write(_struct_2I.pack(_x.action_goal.goal_id.stamp.secs, _x.action_goal.goal_id.stamp.nsecs))
00298 _x = self.action_goal.goal_id.id
00299 length = len(_x)
00300 if python3 or type(_x) == unicode:
00301 _x = _x.encode('utf-8')
00302 length = len(_x)
00303 buff.write(struct.pack('<I%ss'%length, length, _x))
00304 _x = self.action_goal.goal.arm_name
00305 length = len(_x)
00306 if python3 or type(_x) == unicode:
00307 _x = _x.encode('utf-8')
00308 length = len(_x)
00309 buff.write(struct.pack('<I%ss'%length, length, _x))
00310 _x = self
00311 buff.write(_struct_3I.pack(_x.action_goal.goal.clear_move.header.seq, _x.action_goal.goal.clear_move.header.stamp.secs, _x.action_goal.goal.clear_move.header.stamp.nsecs))
00312 _x = self.action_goal.goal.clear_move.header.frame_id
00313 length = len(_x)
00314 if python3 or type(_x) == unicode:
00315 _x = _x.encode('utf-8')
00316 length = len(_x)
00317 buff.write(struct.pack('<I%ss'%length, length, _x))
00318 _x = self
00319 buff.write(_struct_3d3I.pack(_x.action_goal.goal.clear_move.vector.x, _x.action_goal.goal.clear_move.vector.y, _x.action_goal.goal.clear_move.vector.z, _x.action_goal.goal.rotate_pose.header.seq, _x.action_goal.goal.rotate_pose.header.stamp.secs, _x.action_goal.goal.rotate_pose.header.stamp.nsecs))
00320 _x = self.action_goal.goal.rotate_pose.header.frame_id
00321 length = len(_x)
00322 if python3 or type(_x) == unicode:
00323 _x = _x.encode('utf-8')
00324 length = len(_x)
00325 buff.write(struct.pack('<I%ss'%length, length, _x))
00326 _x = self
00327 buff.write(_struct_7d3B3I.pack(_x.action_goal.goal.rotate_pose.pose.position.x, _x.action_goal.goal.rotate_pose.pose.position.y, _x.action_goal.goal.rotate_pose.pose.position.z, _x.action_goal.goal.rotate_pose.pose.orientation.x, _x.action_goal.goal.rotate_pose.pose.orientation.y, _x.action_goal.goal.rotate_pose.pose.orientation.z, _x.action_goal.goal.rotate_pose.pose.orientation.w, _x.action_goal.goal.rotate_object, _x.action_goal.goal.add_to_collision_map, _x.action_goal.goal.keep_level, _x.action_result.header.seq, _x.action_result.header.stamp.secs, _x.action_result.header.stamp.nsecs))
00328 _x = self.action_result.header.frame_id
00329 length = len(_x)
00330 if python3 or type(_x) == unicode:
00331 _x = _x.encode('utf-8')
00332 length = len(_x)
00333 buff.write(struct.pack('<I%ss'%length, length, _x))
00334 _x = self
00335 buff.write(_struct_2I.pack(_x.action_result.status.goal_id.stamp.secs, _x.action_result.status.goal_id.stamp.nsecs))
00336 _x = self.action_result.status.goal_id.id
00337 length = len(_x)
00338 if python3 or type(_x) == unicode:
00339 _x = _x.encode('utf-8')
00340 length = len(_x)
00341 buff.write(struct.pack('<I%ss'%length, length, _x))
00342 buff.write(_struct_B.pack(self.action_result.status.status))
00343 _x = self.action_result.status.text
00344 length = len(_x)
00345 if python3 or type(_x) == unicode:
00346 _x = _x.encode('utf-8')
00347 length = len(_x)
00348 buff.write(struct.pack('<I%ss'%length, length, _x))
00349 _x = self
00350 buff.write(_struct_3I.pack(_x.action_result.result.cluster.header.seq, _x.action_result.result.cluster.header.stamp.secs, _x.action_result.result.cluster.header.stamp.nsecs))
00351 _x = self.action_result.result.cluster.header.frame_id
00352 length = len(_x)
00353 if python3 or type(_x) == unicode:
00354 _x = _x.encode('utf-8')
00355 length = len(_x)
00356 buff.write(struct.pack('<I%ss'%length, length, _x))
00357 _x = self
00358 buff.write(_struct_2I.pack(_x.action_result.result.cluster.height, _x.action_result.result.cluster.width))
00359 length = len(self.action_result.result.cluster.fields)
00360 buff.write(_struct_I.pack(length))
00361 for val1 in self.action_result.result.cluster.fields:
00362 _x = val1.name
00363 length = len(_x)
00364 if python3 or type(_x) == unicode:
00365 _x = _x.encode('utf-8')
00366 length = len(_x)
00367 buff.write(struct.pack('<I%ss'%length, length, _x))
00368 _x = val1
00369 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count))
00370 _x = self
00371 buff.write(_struct_B2I.pack(_x.action_result.result.cluster.is_bigendian, _x.action_result.result.cluster.point_step, _x.action_result.result.cluster.row_step))
00372 _x = self.action_result.result.cluster.data
00373 length = len(_x)
00374
00375 if type(_x) in [list, tuple]:
00376 buff.write(struct.pack('<I%sB'%length, length, *_x))
00377 else:
00378 buff.write(struct.pack('<I%ss'%length, length, _x))
00379 buff.write(_struct_B.pack(self.action_result.result.cluster.is_dense))
00380 _x = self.action_result.result.collision_name
00381 length = len(_x)
00382 if python3 or type(_x) == unicode:
00383 _x = _x.encode('utf-8')
00384 length = len(_x)
00385 buff.write(struct.pack('<I%ss'%length, length, _x))
00386 _x = self
00387 buff.write(_struct_3I.pack(_x.action_feedback.header.seq, _x.action_feedback.header.stamp.secs, _x.action_feedback.header.stamp.nsecs))
00388 _x = self.action_feedback.header.frame_id
00389 length = len(_x)
00390 if python3 or type(_x) == unicode:
00391 _x = _x.encode('utf-8')
00392 length = len(_x)
00393 buff.write(struct.pack('<I%ss'%length, length, _x))
00394 _x = self
00395 buff.write(_struct_2I.pack(_x.action_feedback.status.goal_id.stamp.secs, _x.action_feedback.status.goal_id.stamp.nsecs))
00396 _x = self.action_feedback.status.goal_id.id
00397 length = len(_x)
00398 if python3 or type(_x) == unicode:
00399 _x = _x.encode('utf-8')
00400 length = len(_x)
00401 buff.write(struct.pack('<I%ss'%length, length, _x))
00402 buff.write(_struct_B.pack(self.action_feedback.status.status))
00403 _x = self.action_feedback.status.text
00404 length = len(_x)
00405 if python3 or type(_x) == unicode:
00406 _x = _x.encode('utf-8')
00407 length = len(_x)
00408 buff.write(struct.pack('<I%ss'%length, length, _x))
00409 _x = self
00410 buff.write(_struct_2i.pack(_x.action_feedback.feedback.phase, _x.action_feedback.feedback.rotate_ind))
00411 except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
00412 except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
00413
00414 def deserialize(self, str):
00415 """
00416 unpack serialized message in str into this message instance
00417 :param str: byte array of serialized message, ``str``
00418 """
00419 try:
00420 if self.action_goal is None:
00421 self.action_goal = pr2_create_object_model.msg.ModelObjectInHandActionGoal()
00422 if self.action_result is None:
00423 self.action_result = pr2_create_object_model.msg.ModelObjectInHandActionResult()
00424 if self.action_feedback is None:
00425 self.action_feedback = pr2_create_object_model.msg.ModelObjectInHandActionFeedback()
00426 end = 0
00427 _x = self
00428 start = end
00429 end += 12
00430 (_x.action_goal.header.seq, _x.action_goal.header.stamp.secs, _x.action_goal.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00431 start = end
00432 end += 4
00433 (length,) = _struct_I.unpack(str[start:end])
00434 start = end
00435 end += length
00436 if python3:
00437 self.action_goal.header.frame_id = str[start:end].decode('utf-8')
00438 else:
00439 self.action_goal.header.frame_id = str[start:end]
00440 _x = self
00441 start = end
00442 end += 8
00443 (_x.action_goal.goal_id.stamp.secs, _x.action_goal.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
00444 start = end
00445 end += 4
00446 (length,) = _struct_I.unpack(str[start:end])
00447 start = end
00448 end += length
00449 if python3:
00450 self.action_goal.goal_id.id = str[start:end].decode('utf-8')
00451 else:
00452 self.action_goal.goal_id.id = str[start:end]
00453 start = end
00454 end += 4
00455 (length,) = _struct_I.unpack(str[start:end])
00456 start = end
00457 end += length
00458 if python3:
00459 self.action_goal.goal.arm_name = str[start:end].decode('utf-8')
00460 else:
00461 self.action_goal.goal.arm_name = str[start:end]
00462 _x = self
00463 start = end
00464 end += 12
00465 (_x.action_goal.goal.clear_move.header.seq, _x.action_goal.goal.clear_move.header.stamp.secs, _x.action_goal.goal.clear_move.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00466 start = end
00467 end += 4
00468 (length,) = _struct_I.unpack(str[start:end])
00469 start = end
00470 end += length
00471 if python3:
00472 self.action_goal.goal.clear_move.header.frame_id = str[start:end].decode('utf-8')
00473 else:
00474 self.action_goal.goal.clear_move.header.frame_id = str[start:end]
00475 _x = self
00476 start = end
00477 end += 36
00478 (_x.action_goal.goal.clear_move.vector.x, _x.action_goal.goal.clear_move.vector.y, _x.action_goal.goal.clear_move.vector.z, _x.action_goal.goal.rotate_pose.header.seq, _x.action_goal.goal.rotate_pose.header.stamp.secs, _x.action_goal.goal.rotate_pose.header.stamp.nsecs,) = _struct_3d3I.unpack(str[start:end])
00479 start = end
00480 end += 4
00481 (length,) = _struct_I.unpack(str[start:end])
00482 start = end
00483 end += length
00484 if python3:
00485 self.action_goal.goal.rotate_pose.header.frame_id = str[start:end].decode('utf-8')
00486 else:
00487 self.action_goal.goal.rotate_pose.header.frame_id = str[start:end]
00488 _x = self
00489 start = end
00490 end += 71
00491 (_x.action_goal.goal.rotate_pose.pose.position.x, _x.action_goal.goal.rotate_pose.pose.position.y, _x.action_goal.goal.rotate_pose.pose.position.z, _x.action_goal.goal.rotate_pose.pose.orientation.x, _x.action_goal.goal.rotate_pose.pose.orientation.y, _x.action_goal.goal.rotate_pose.pose.orientation.z, _x.action_goal.goal.rotate_pose.pose.orientation.w, _x.action_goal.goal.rotate_object, _x.action_goal.goal.add_to_collision_map, _x.action_goal.goal.keep_level, _x.action_result.header.seq, _x.action_result.header.stamp.secs, _x.action_result.header.stamp.nsecs,) = _struct_7d3B3I.unpack(str[start:end])
00492 start = end
00493 end += 4
00494 (length,) = _struct_I.unpack(str[start:end])
00495 start = end
00496 end += length
00497 if python3:
00498 self.action_result.header.frame_id = str[start:end].decode('utf-8')
00499 else:
00500 self.action_result.header.frame_id = str[start:end]
00501 _x = self
00502 start = end
00503 end += 8
00504 (_x.action_result.status.goal_id.stamp.secs, _x.action_result.status.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
00505 start = end
00506 end += 4
00507 (length,) = _struct_I.unpack(str[start:end])
00508 start = end
00509 end += length
00510 if python3:
00511 self.action_result.status.goal_id.id = str[start:end].decode('utf-8')
00512 else:
00513 self.action_result.status.goal_id.id = str[start:end]
00514 start = end
00515 end += 1
00516 (self.action_result.status.status,) = _struct_B.unpack(str[start:end])
00517 start = end
00518 end += 4
00519 (length,) = _struct_I.unpack(str[start:end])
00520 start = end
00521 end += length
00522 if python3:
00523 self.action_result.status.text = str[start:end].decode('utf-8')
00524 else:
00525 self.action_result.status.text = str[start:end]
00526 _x = self
00527 start = end
00528 end += 12
00529 (_x.action_result.result.cluster.header.seq, _x.action_result.result.cluster.header.stamp.secs, _x.action_result.result.cluster.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00530 start = end
00531 end += 4
00532 (length,) = _struct_I.unpack(str[start:end])
00533 start = end
00534 end += length
00535 if python3:
00536 self.action_result.result.cluster.header.frame_id = str[start:end].decode('utf-8')
00537 else:
00538 self.action_result.result.cluster.header.frame_id = str[start:end]
00539 _x = self
00540 start = end
00541 end += 8
00542 (_x.action_result.result.cluster.height, _x.action_result.result.cluster.width,) = _struct_2I.unpack(str[start:end])
00543 start = end
00544 end += 4
00545 (length,) = _struct_I.unpack(str[start:end])
00546 self.action_result.result.cluster.fields = []
00547 for i in range(0, length):
00548 val1 = sensor_msgs.msg.PointField()
00549 start = end
00550 end += 4
00551 (length,) = _struct_I.unpack(str[start:end])
00552 start = end
00553 end += length
00554 if python3:
00555 val1.name = str[start:end].decode('utf-8')
00556 else:
00557 val1.name = str[start:end]
00558 _x = val1
00559 start = end
00560 end += 9
00561 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end])
00562 self.action_result.result.cluster.fields.append(val1)
00563 _x = self
00564 start = end
00565 end += 9
00566 (_x.action_result.result.cluster.is_bigendian, _x.action_result.result.cluster.point_step, _x.action_result.result.cluster.row_step,) = _struct_B2I.unpack(str[start:end])
00567 self.action_result.result.cluster.is_bigendian = bool(self.action_result.result.cluster.is_bigendian)
00568 start = end
00569 end += 4
00570 (length,) = _struct_I.unpack(str[start:end])
00571 start = end
00572 end += length
00573 self.action_result.result.cluster.data = str[start:end]
00574 start = end
00575 end += 1
00576 (self.action_result.result.cluster.is_dense,) = _struct_B.unpack(str[start:end])
00577 self.action_result.result.cluster.is_dense = bool(self.action_result.result.cluster.is_dense)
00578 start = end
00579 end += 4
00580 (length,) = _struct_I.unpack(str[start:end])
00581 start = end
00582 end += length
00583 if python3:
00584 self.action_result.result.collision_name = str[start:end].decode('utf-8')
00585 else:
00586 self.action_result.result.collision_name = str[start:end]
00587 _x = self
00588 start = end
00589 end += 12
00590 (_x.action_feedback.header.seq, _x.action_feedback.header.stamp.secs, _x.action_feedback.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00591 start = end
00592 end += 4
00593 (length,) = _struct_I.unpack(str[start:end])
00594 start = end
00595 end += length
00596 if python3:
00597 self.action_feedback.header.frame_id = str[start:end].decode('utf-8')
00598 else:
00599 self.action_feedback.header.frame_id = str[start:end]
00600 _x = self
00601 start = end
00602 end += 8
00603 (_x.action_feedback.status.goal_id.stamp.secs, _x.action_feedback.status.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
00604 start = end
00605 end += 4
00606 (length,) = _struct_I.unpack(str[start:end])
00607 start = end
00608 end += length
00609 if python3:
00610 self.action_feedback.status.goal_id.id = str[start:end].decode('utf-8')
00611 else:
00612 self.action_feedback.status.goal_id.id = str[start:end]
00613 start = end
00614 end += 1
00615 (self.action_feedback.status.status,) = _struct_B.unpack(str[start:end])
00616 start = end
00617 end += 4
00618 (length,) = _struct_I.unpack(str[start:end])
00619 start = end
00620 end += length
00621 if python3:
00622 self.action_feedback.status.text = str[start:end].decode('utf-8')
00623 else:
00624 self.action_feedback.status.text = str[start:end]
00625 _x = self
00626 start = end
00627 end += 8
00628 (_x.action_feedback.feedback.phase, _x.action_feedback.feedback.rotate_ind,) = _struct_2i.unpack(str[start:end])
00629 return self
00630 except struct.error as e:
00631 raise genpy.DeserializationError(e)
00632
00633
00634 def serialize_numpy(self, buff, numpy):
00635 """
00636 serialize message with numpy array types into buffer
00637 :param buff: buffer, ``StringIO``
00638 :param numpy: numpy python module
00639 """
00640 try:
00641 _x = self
00642 buff.write(_struct_3I.pack(_x.action_goal.header.seq, _x.action_goal.header.stamp.secs, _x.action_goal.header.stamp.nsecs))
00643 _x = self.action_goal.header.frame_id
00644 length = len(_x)
00645 if python3 or type(_x) == unicode:
00646 _x = _x.encode('utf-8')
00647 length = len(_x)
00648 buff.write(struct.pack('<I%ss'%length, length, _x))
00649 _x = self
00650 buff.write(_struct_2I.pack(_x.action_goal.goal_id.stamp.secs, _x.action_goal.goal_id.stamp.nsecs))
00651 _x = self.action_goal.goal_id.id
00652 length = len(_x)
00653 if python3 or type(_x) == unicode:
00654 _x = _x.encode('utf-8')
00655 length = len(_x)
00656 buff.write(struct.pack('<I%ss'%length, length, _x))
00657 _x = self.action_goal.goal.arm_name
00658 length = len(_x)
00659 if python3 or type(_x) == unicode:
00660 _x = _x.encode('utf-8')
00661 length = len(_x)
00662 buff.write(struct.pack('<I%ss'%length, length, _x))
00663 _x = self
00664 buff.write(_struct_3I.pack(_x.action_goal.goal.clear_move.header.seq, _x.action_goal.goal.clear_move.header.stamp.secs, _x.action_goal.goal.clear_move.header.stamp.nsecs))
00665 _x = self.action_goal.goal.clear_move.header.frame_id
00666 length = len(_x)
00667 if python3 or type(_x) == unicode:
00668 _x = _x.encode('utf-8')
00669 length = len(_x)
00670 buff.write(struct.pack('<I%ss'%length, length, _x))
00671 _x = self
00672 buff.write(_struct_3d3I.pack(_x.action_goal.goal.clear_move.vector.x, _x.action_goal.goal.clear_move.vector.y, _x.action_goal.goal.clear_move.vector.z, _x.action_goal.goal.rotate_pose.header.seq, _x.action_goal.goal.rotate_pose.header.stamp.secs, _x.action_goal.goal.rotate_pose.header.stamp.nsecs))
00673 _x = self.action_goal.goal.rotate_pose.header.frame_id
00674 length = len(_x)
00675 if python3 or type(_x) == unicode:
00676 _x = _x.encode('utf-8')
00677 length = len(_x)
00678 buff.write(struct.pack('<I%ss'%length, length, _x))
00679 _x = self
00680 buff.write(_struct_7d3B3I.pack(_x.action_goal.goal.rotate_pose.pose.position.x, _x.action_goal.goal.rotate_pose.pose.position.y, _x.action_goal.goal.rotate_pose.pose.position.z, _x.action_goal.goal.rotate_pose.pose.orientation.x, _x.action_goal.goal.rotate_pose.pose.orientation.y, _x.action_goal.goal.rotate_pose.pose.orientation.z, _x.action_goal.goal.rotate_pose.pose.orientation.w, _x.action_goal.goal.rotate_object, _x.action_goal.goal.add_to_collision_map, _x.action_goal.goal.keep_level, _x.action_result.header.seq, _x.action_result.header.stamp.secs, _x.action_result.header.stamp.nsecs))
00681 _x = self.action_result.header.frame_id
00682 length = len(_x)
00683 if python3 or type(_x) == unicode:
00684 _x = _x.encode('utf-8')
00685 length = len(_x)
00686 buff.write(struct.pack('<I%ss'%length, length, _x))
00687 _x = self
00688 buff.write(_struct_2I.pack(_x.action_result.status.goal_id.stamp.secs, _x.action_result.status.goal_id.stamp.nsecs))
00689 _x = self.action_result.status.goal_id.id
00690 length = len(_x)
00691 if python3 or type(_x) == unicode:
00692 _x = _x.encode('utf-8')
00693 length = len(_x)
00694 buff.write(struct.pack('<I%ss'%length, length, _x))
00695 buff.write(_struct_B.pack(self.action_result.status.status))
00696 _x = self.action_result.status.text
00697 length = len(_x)
00698 if python3 or type(_x) == unicode:
00699 _x = _x.encode('utf-8')
00700 length = len(_x)
00701 buff.write(struct.pack('<I%ss'%length, length, _x))
00702 _x = self
00703 buff.write(_struct_3I.pack(_x.action_result.result.cluster.header.seq, _x.action_result.result.cluster.header.stamp.secs, _x.action_result.result.cluster.header.stamp.nsecs))
00704 _x = self.action_result.result.cluster.header.frame_id
00705 length = len(_x)
00706 if python3 or type(_x) == unicode:
00707 _x = _x.encode('utf-8')
00708 length = len(_x)
00709 buff.write(struct.pack('<I%ss'%length, length, _x))
00710 _x = self
00711 buff.write(_struct_2I.pack(_x.action_result.result.cluster.height, _x.action_result.result.cluster.width))
00712 length = len(self.action_result.result.cluster.fields)
00713 buff.write(_struct_I.pack(length))
00714 for val1 in self.action_result.result.cluster.fields:
00715 _x = val1.name
00716 length = len(_x)
00717 if python3 or type(_x) == unicode:
00718 _x = _x.encode('utf-8')
00719 length = len(_x)
00720 buff.write(struct.pack('<I%ss'%length, length, _x))
00721 _x = val1
00722 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count))
00723 _x = self
00724 buff.write(_struct_B2I.pack(_x.action_result.result.cluster.is_bigendian, _x.action_result.result.cluster.point_step, _x.action_result.result.cluster.row_step))
00725 _x = self.action_result.result.cluster.data
00726 length = len(_x)
00727
00728 if type(_x) in [list, tuple]:
00729 buff.write(struct.pack('<I%sB'%length, length, *_x))
00730 else:
00731 buff.write(struct.pack('<I%ss'%length, length, _x))
00732 buff.write(_struct_B.pack(self.action_result.result.cluster.is_dense))
00733 _x = self.action_result.result.collision_name
00734 length = len(_x)
00735 if python3 or type(_x) == unicode:
00736 _x = _x.encode('utf-8')
00737 length = len(_x)
00738 buff.write(struct.pack('<I%ss'%length, length, _x))
00739 _x = self
00740 buff.write(_struct_3I.pack(_x.action_feedback.header.seq, _x.action_feedback.header.stamp.secs, _x.action_feedback.header.stamp.nsecs))
00741 _x = self.action_feedback.header.frame_id
00742 length = len(_x)
00743 if python3 or type(_x) == unicode:
00744 _x = _x.encode('utf-8')
00745 length = len(_x)
00746 buff.write(struct.pack('<I%ss'%length, length, _x))
00747 _x = self
00748 buff.write(_struct_2I.pack(_x.action_feedback.status.goal_id.stamp.secs, _x.action_feedback.status.goal_id.stamp.nsecs))
00749 _x = self.action_feedback.status.goal_id.id
00750 length = len(_x)
00751 if python3 or type(_x) == unicode:
00752 _x = _x.encode('utf-8')
00753 length = len(_x)
00754 buff.write(struct.pack('<I%ss'%length, length, _x))
00755 buff.write(_struct_B.pack(self.action_feedback.status.status))
00756 _x = self.action_feedback.status.text
00757 length = len(_x)
00758 if python3 or type(_x) == unicode:
00759 _x = _x.encode('utf-8')
00760 length = len(_x)
00761 buff.write(struct.pack('<I%ss'%length, length, _x))
00762 _x = self
00763 buff.write(_struct_2i.pack(_x.action_feedback.feedback.phase, _x.action_feedback.feedback.rotate_ind))
00764 except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
00765 except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
00766
00767 def deserialize_numpy(self, str, numpy):
00768 """
00769 unpack serialized message in str into this message instance using numpy for array types
00770 :param str: byte array of serialized message, ``str``
00771 :param numpy: numpy python module
00772 """
00773 try:
00774 if self.action_goal is None:
00775 self.action_goal = pr2_create_object_model.msg.ModelObjectInHandActionGoal()
00776 if self.action_result is None:
00777 self.action_result = pr2_create_object_model.msg.ModelObjectInHandActionResult()
00778 if self.action_feedback is None:
00779 self.action_feedback = pr2_create_object_model.msg.ModelObjectInHandActionFeedback()
00780 end = 0
00781 _x = self
00782 start = end
00783 end += 12
00784 (_x.action_goal.header.seq, _x.action_goal.header.stamp.secs, _x.action_goal.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00785 start = end
00786 end += 4
00787 (length,) = _struct_I.unpack(str[start:end])
00788 start = end
00789 end += length
00790 if python3:
00791 self.action_goal.header.frame_id = str[start:end].decode('utf-8')
00792 else:
00793 self.action_goal.header.frame_id = str[start:end]
00794 _x = self
00795 start = end
00796 end += 8
00797 (_x.action_goal.goal_id.stamp.secs, _x.action_goal.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
00798 start = end
00799 end += 4
00800 (length,) = _struct_I.unpack(str[start:end])
00801 start = end
00802 end += length
00803 if python3:
00804 self.action_goal.goal_id.id = str[start:end].decode('utf-8')
00805 else:
00806 self.action_goal.goal_id.id = str[start:end]
00807 start = end
00808 end += 4
00809 (length,) = _struct_I.unpack(str[start:end])
00810 start = end
00811 end += length
00812 if python3:
00813 self.action_goal.goal.arm_name = str[start:end].decode('utf-8')
00814 else:
00815 self.action_goal.goal.arm_name = str[start:end]
00816 _x = self
00817 start = end
00818 end += 12
00819 (_x.action_goal.goal.clear_move.header.seq, _x.action_goal.goal.clear_move.header.stamp.secs, _x.action_goal.goal.clear_move.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00820 start = end
00821 end += 4
00822 (length,) = _struct_I.unpack(str[start:end])
00823 start = end
00824 end += length
00825 if python3:
00826 self.action_goal.goal.clear_move.header.frame_id = str[start:end].decode('utf-8')
00827 else:
00828 self.action_goal.goal.clear_move.header.frame_id = str[start:end]
00829 _x = self
00830 start = end
00831 end += 36
00832 (_x.action_goal.goal.clear_move.vector.x, _x.action_goal.goal.clear_move.vector.y, _x.action_goal.goal.clear_move.vector.z, _x.action_goal.goal.rotate_pose.header.seq, _x.action_goal.goal.rotate_pose.header.stamp.secs, _x.action_goal.goal.rotate_pose.header.stamp.nsecs,) = _struct_3d3I.unpack(str[start:end])
00833 start = end
00834 end += 4
00835 (length,) = _struct_I.unpack(str[start:end])
00836 start = end
00837 end += length
00838 if python3:
00839 self.action_goal.goal.rotate_pose.header.frame_id = str[start:end].decode('utf-8')
00840 else:
00841 self.action_goal.goal.rotate_pose.header.frame_id = str[start:end]
00842 _x = self
00843 start = end
00844 end += 71
00845 (_x.action_goal.goal.rotate_pose.pose.position.x, _x.action_goal.goal.rotate_pose.pose.position.y, _x.action_goal.goal.rotate_pose.pose.position.z, _x.action_goal.goal.rotate_pose.pose.orientation.x, _x.action_goal.goal.rotate_pose.pose.orientation.y, _x.action_goal.goal.rotate_pose.pose.orientation.z, _x.action_goal.goal.rotate_pose.pose.orientation.w, _x.action_goal.goal.rotate_object, _x.action_goal.goal.add_to_collision_map, _x.action_goal.goal.keep_level, _x.action_result.header.seq, _x.action_result.header.stamp.secs, _x.action_result.header.stamp.nsecs,) = _struct_7d3B3I.unpack(str[start:end])
00846 start = end
00847 end += 4
00848 (length,) = _struct_I.unpack(str[start:end])
00849 start = end
00850 end += length
00851 if python3:
00852 self.action_result.header.frame_id = str[start:end].decode('utf-8')
00853 else:
00854 self.action_result.header.frame_id = str[start:end]
00855 _x = self
00856 start = end
00857 end += 8
00858 (_x.action_result.status.goal_id.stamp.secs, _x.action_result.status.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
00859 start = end
00860 end += 4
00861 (length,) = _struct_I.unpack(str[start:end])
00862 start = end
00863 end += length
00864 if python3:
00865 self.action_result.status.goal_id.id = str[start:end].decode('utf-8')
00866 else:
00867 self.action_result.status.goal_id.id = str[start:end]
00868 start = end
00869 end += 1
00870 (self.action_result.status.status,) = _struct_B.unpack(str[start:end])
00871 start = end
00872 end += 4
00873 (length,) = _struct_I.unpack(str[start:end])
00874 start = end
00875 end += length
00876 if python3:
00877 self.action_result.status.text = str[start:end].decode('utf-8')
00878 else:
00879 self.action_result.status.text = str[start:end]
00880 _x = self
00881 start = end
00882 end += 12
00883 (_x.action_result.result.cluster.header.seq, _x.action_result.result.cluster.header.stamp.secs, _x.action_result.result.cluster.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00884 start = end
00885 end += 4
00886 (length,) = _struct_I.unpack(str[start:end])
00887 start = end
00888 end += length
00889 if python3:
00890 self.action_result.result.cluster.header.frame_id = str[start:end].decode('utf-8')
00891 else:
00892 self.action_result.result.cluster.header.frame_id = str[start:end]
00893 _x = self
00894 start = end
00895 end += 8
00896 (_x.action_result.result.cluster.height, _x.action_result.result.cluster.width,) = _struct_2I.unpack(str[start:end])
00897 start = end
00898 end += 4
00899 (length,) = _struct_I.unpack(str[start:end])
00900 self.action_result.result.cluster.fields = []
00901 for i in range(0, length):
00902 val1 = sensor_msgs.msg.PointField()
00903 start = end
00904 end += 4
00905 (length,) = _struct_I.unpack(str[start:end])
00906 start = end
00907 end += length
00908 if python3:
00909 val1.name = str[start:end].decode('utf-8')
00910 else:
00911 val1.name = str[start:end]
00912 _x = val1
00913 start = end
00914 end += 9
00915 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end])
00916 self.action_result.result.cluster.fields.append(val1)
00917 _x = self
00918 start = end
00919 end += 9
00920 (_x.action_result.result.cluster.is_bigendian, _x.action_result.result.cluster.point_step, _x.action_result.result.cluster.row_step,) = _struct_B2I.unpack(str[start:end])
00921 self.action_result.result.cluster.is_bigendian = bool(self.action_result.result.cluster.is_bigendian)
00922 start = end
00923 end += 4
00924 (length,) = _struct_I.unpack(str[start:end])
00925 start = end
00926 end += length
00927 self.action_result.result.cluster.data = str[start:end]
00928 start = end
00929 end += 1
00930 (self.action_result.result.cluster.is_dense,) = _struct_B.unpack(str[start:end])
00931 self.action_result.result.cluster.is_dense = bool(self.action_result.result.cluster.is_dense)
00932 start = end
00933 end += 4
00934 (length,) = _struct_I.unpack(str[start:end])
00935 start = end
00936 end += length
00937 if python3:
00938 self.action_result.result.collision_name = str[start:end].decode('utf-8')
00939 else:
00940 self.action_result.result.collision_name = str[start:end]
00941 _x = self
00942 start = end
00943 end += 12
00944 (_x.action_feedback.header.seq, _x.action_feedback.header.stamp.secs, _x.action_feedback.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00945 start = end
00946 end += 4
00947 (length,) = _struct_I.unpack(str[start:end])
00948 start = end
00949 end += length
00950 if python3:
00951 self.action_feedback.header.frame_id = str[start:end].decode('utf-8')
00952 else:
00953 self.action_feedback.header.frame_id = str[start:end]
00954 _x = self
00955 start = end
00956 end += 8
00957 (_x.action_feedback.status.goal_id.stamp.secs, _x.action_feedback.status.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
00958 start = end
00959 end += 4
00960 (length,) = _struct_I.unpack(str[start:end])
00961 start = end
00962 end += length
00963 if python3:
00964 self.action_feedback.status.goal_id.id = str[start:end].decode('utf-8')
00965 else:
00966 self.action_feedback.status.goal_id.id = str[start:end]
00967 start = end
00968 end += 1
00969 (self.action_feedback.status.status,) = _struct_B.unpack(str[start:end])
00970 start = end
00971 end += 4
00972 (length,) = _struct_I.unpack(str[start:end])
00973 start = end
00974 end += length
00975 if python3:
00976 self.action_feedback.status.text = str[start:end].decode('utf-8')
00977 else:
00978 self.action_feedback.status.text = str[start:end]
00979 _x = self
00980 start = end
00981 end += 8
00982 (_x.action_feedback.feedback.phase, _x.action_feedback.feedback.rotate_ind,) = _struct_2i.unpack(str[start:end])
00983 return self
00984 except struct.error as e:
00985 raise genpy.DeserializationError(e)
00986
00987 _struct_I = genpy.struct_I
00988 _struct_7d3B3I = struct.Struct("<7d3B3I")
00989 _struct_3d3I = struct.Struct("<3d3I")
00990 _struct_B = struct.Struct("<B")
00991 _struct_IBI = struct.Struct("<IBI")
00992 _struct_2i = struct.Struct("<2i")
00993 _struct_3I = struct.Struct("<3I")
00994 _struct_B2I = struct.Struct("<B2I")
00995 _struct_2I = struct.Struct("<2I")