00001 """autogenerated by genpy from cogman_msgs/ArmHandAction.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 cogman_msgs.msg
00009 import geometry_msgs.msg
00010 import vision_msgs.msg
00011 import genpy
00012 import std_msgs.msg
00013
00014 class ArmHandAction(genpy.Message):
00015 _md5sum = "47caad1eeab6545c2c3e92dff9f336b8"
00016 _type = "cogman_msgs/ArmHandAction"
00017 _has_header = False
00018 _full_text = """# ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00019
00020 ArmHandActionGoal action_goal
00021 ArmHandActionResult action_result
00022 ArmHandActionFeedback action_feedback
00023
00024 ================================================================================
00025 MSG: cogman_msgs/ArmHandActionGoal
00026 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00027
00028 Header header
00029 actionlib_msgs/GoalID goal_id
00030 ArmHandGoal 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: cogman_msgs/ArmHandGoal
00065 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00066 string command # 'arm_cart_loid', 'arm_cart_pose', 'arm_cart_name', 'arm_joints', 'arm_joint_name', 'reach_primitive', 'hand_primitive', 'lift', 'put_down'
00067 string pose_name # When moving to pre-defined poses, cointains the name
00068 float32[] joint_angles # If command is 'arm_joints', the joint angles
00069 geometry_msgs/Pose end_effector_pose # command=arm_cart_pose: where the arm should go
00070 uint64 end_effector_loid # one lo_id where the arm should go
00071 string hand_primitive # Allowed strings: open open_thumb90 3pinch 2pinch cup_top peace handshake noop. Used when command is 'finger_primitive'
00072 string object_type # If command is 'reach_primitive', it will define how to grasp (orientation ...)
00073 uint64[] obstacle_ids # List of lo_id's corresponding to obstacles
00074 float32 distance # For reach_primitive: distance from grasp point to approach point (m). For 'lift', distance to lift up (m)
00075 float32 supporting_plane # Height of the table (m)
00076
00077 ================================================================================
00078 MSG: geometry_msgs/Pose
00079 # A representation of pose in free space, composed of postion and orientation.
00080 Point position
00081 Quaternion orientation
00082
00083 ================================================================================
00084 MSG: geometry_msgs/Point
00085 # This contains the position of a point in free space
00086 float64 x
00087 float64 y
00088 float64 z
00089
00090 ================================================================================
00091 MSG: geometry_msgs/Quaternion
00092 # This represents an orientation in free space in quaternion form.
00093
00094 float64 x
00095 float64 y
00096 float64 z
00097 float64 w
00098
00099 ================================================================================
00100 MSG: cogman_msgs/ArmHandActionResult
00101 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00102
00103 Header header
00104 actionlib_msgs/GoalStatus status
00105 ArmHandResult result
00106
00107 ================================================================================
00108 MSG: actionlib_msgs/GoalStatus
00109 GoalID goal_id
00110 uint8 status
00111 uint8 PENDING = 0 # The goal has yet to be processed by the action server
00112 uint8 ACTIVE = 1 # The goal is currently being processed by the action server
00113 uint8 PREEMPTED = 2 # The goal received a cancel request after it started executing
00114 # and has since completed its execution (Terminal State)
00115 uint8 SUCCEEDED = 3 # The goal was achieved successfully by the action server (Terminal State)
00116 uint8 ABORTED = 4 # The goal was aborted during execution by the action server due
00117 # to some failure (Terminal State)
00118 uint8 REJECTED = 5 # The goal was rejected by the action server without being processed,
00119 # because the goal was unattainable or invalid (Terminal State)
00120 uint8 PREEMPTING = 6 # The goal received a cancel request after it started executing
00121 # and has not yet completed execution
00122 uint8 RECALLING = 7 # The goal received a cancel request before it started executing,
00123 # but the action server has not yet confirmed that the goal is canceled
00124 uint8 RECALLED = 8 # The goal received a cancel request before it started executing
00125 # and was successfully cancelled (Terminal State)
00126 uint8 LOST = 9 # An action client can determine that a goal is LOST. This should not be
00127 # sent over the wire by an action server
00128
00129 #Allow for the user to associate a string with GoalStatus for debugging
00130 string text
00131
00132
00133 ================================================================================
00134 MSG: cogman_msgs/ArmHandResult
00135 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00136
00137 #result
00138 float32 distance_to_goal
00139 string situation #Allowed: grasped slipped released unreachable ik_deadlock need_to_move_base
00140 vision_msgs/system_error error #Description of error
00141 uint64[] better_base_ids #List of lo_id's where we could grasp better
00142
00143 ================================================================================
00144 MSG: vision_msgs/system_error
00145 uint64 MANIPULATION_POSE_UNREACHABLE = 64
00146 uint64 GRASP_FAILED = 128 # Grasp into the void
00147 uint64 OBJECT_NOT_FOUND = 256
00148 uint64 VISION_PRIMITIVE_FAILED = 512
00149 uint64 CONTRADICTING_TACTILE_FEEDBACK = 1024 # Collide without expecting it
00150 uint64 CONTRADICTING_VISION_RESULTS = 2048
00151 uint64 GRASP_FAILED_AND_CRASHED = 4096 # Throwing something out of the way
00152 uint64 JLO_ERROR = 8192 # Could not get position
00153 uint64 VECTOR_FIELD_CANT_REACH = 16384 # The arm got stuck along the way, did not reach the final grasping pose
00154
00155 uint64 error_id # One of the error constants defined above
00156 string node_name # The node causing this error
00157 string error_description # Further information about the error
00158
00159 ================================================================================
00160 MSG: cogman_msgs/ArmHandActionFeedback
00161 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00162
00163 Header header
00164 actionlib_msgs/GoalStatus status
00165 ArmHandFeedback feedback
00166
00167 ================================================================================
00168 MSG: cogman_msgs/ArmHandFeedback
00169 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00170 #feedback
00171 float32 distance_to_goal
00172
00173
00174
00175 """
00176 __slots__ = ['action_goal','action_result','action_feedback']
00177 _slot_types = ['cogman_msgs/ArmHandActionGoal','cogman_msgs/ArmHandActionResult','cogman_msgs/ArmHandActionFeedback']
00178
00179 def __init__(self, *args, **kwds):
00180 """
00181 Constructor. Any message fields that are implicitly/explicitly
00182 set to None will be assigned a default value. The recommend
00183 use is keyword arguments as this is more robust to future message
00184 changes. You cannot mix in-order arguments and keyword arguments.
00185
00186 The available fields are:
00187 action_goal,action_result,action_feedback
00188
00189 :param args: complete set of field values, in .msg order
00190 :param kwds: use keyword arguments corresponding to message field names
00191 to set specific fields.
00192 """
00193 if args or kwds:
00194 super(ArmHandAction, self).__init__(*args, **kwds)
00195
00196 if self.action_goal is None:
00197 self.action_goal = cogman_msgs.msg.ArmHandActionGoal()
00198 if self.action_result is None:
00199 self.action_result = cogman_msgs.msg.ArmHandActionResult()
00200 if self.action_feedback is None:
00201 self.action_feedback = cogman_msgs.msg.ArmHandActionFeedback()
00202 else:
00203 self.action_goal = cogman_msgs.msg.ArmHandActionGoal()
00204 self.action_result = cogman_msgs.msg.ArmHandActionResult()
00205 self.action_feedback = cogman_msgs.msg.ArmHandActionFeedback()
00206
00207 def _get_types(self):
00208 """
00209 internal API method
00210 """
00211 return self._slot_types
00212
00213 def serialize(self, buff):
00214 """
00215 serialize message into buffer
00216 :param buff: buffer, ``StringIO``
00217 """
00218 try:
00219 _x = self
00220 buff.write(_struct_3I.pack(_x.action_goal.header.seq, _x.action_goal.header.stamp.secs, _x.action_goal.header.stamp.nsecs))
00221 _x = self.action_goal.header.frame_id
00222 length = len(_x)
00223 if python3 or type(_x) == unicode:
00224 _x = _x.encode('utf-8')
00225 length = len(_x)
00226 buff.write(struct.pack('<I%ss'%length, length, _x))
00227 _x = self
00228 buff.write(_struct_2I.pack(_x.action_goal.goal_id.stamp.secs, _x.action_goal.goal_id.stamp.nsecs))
00229 _x = self.action_goal.goal_id.id
00230 length = len(_x)
00231 if python3 or type(_x) == unicode:
00232 _x = _x.encode('utf-8')
00233 length = len(_x)
00234 buff.write(struct.pack('<I%ss'%length, length, _x))
00235 _x = self.action_goal.goal.command
00236 length = len(_x)
00237 if python3 or type(_x) == unicode:
00238 _x = _x.encode('utf-8')
00239 length = len(_x)
00240 buff.write(struct.pack('<I%ss'%length, length, _x))
00241 _x = self.action_goal.goal.pose_name
00242 length = len(_x)
00243 if python3 or type(_x) == unicode:
00244 _x = _x.encode('utf-8')
00245 length = len(_x)
00246 buff.write(struct.pack('<I%ss'%length, length, _x))
00247 length = len(self.action_goal.goal.joint_angles)
00248 buff.write(_struct_I.pack(length))
00249 pattern = '<%sf'%length
00250 buff.write(struct.pack(pattern, *self.action_goal.goal.joint_angles))
00251 _x = self
00252 buff.write(_struct_7dQ.pack(_x.action_goal.goal.end_effector_pose.position.x, _x.action_goal.goal.end_effector_pose.position.y, _x.action_goal.goal.end_effector_pose.position.z, _x.action_goal.goal.end_effector_pose.orientation.x, _x.action_goal.goal.end_effector_pose.orientation.y, _x.action_goal.goal.end_effector_pose.orientation.z, _x.action_goal.goal.end_effector_pose.orientation.w, _x.action_goal.goal.end_effector_loid))
00253 _x = self.action_goal.goal.hand_primitive
00254 length = len(_x)
00255 if python3 or type(_x) == unicode:
00256 _x = _x.encode('utf-8')
00257 length = len(_x)
00258 buff.write(struct.pack('<I%ss'%length, length, _x))
00259 _x = self.action_goal.goal.object_type
00260 length = len(_x)
00261 if python3 or type(_x) == unicode:
00262 _x = _x.encode('utf-8')
00263 length = len(_x)
00264 buff.write(struct.pack('<I%ss'%length, length, _x))
00265 length = len(self.action_goal.goal.obstacle_ids)
00266 buff.write(_struct_I.pack(length))
00267 pattern = '<%sQ'%length
00268 buff.write(struct.pack(pattern, *self.action_goal.goal.obstacle_ids))
00269 _x = self
00270 buff.write(_struct_2f3I.pack(_x.action_goal.goal.distance, _x.action_goal.goal.supporting_plane, _x.action_result.header.seq, _x.action_result.header.stamp.secs, _x.action_result.header.stamp.nsecs))
00271 _x = self.action_result.header.frame_id
00272 length = len(_x)
00273 if python3 or type(_x) == unicode:
00274 _x = _x.encode('utf-8')
00275 length = len(_x)
00276 buff.write(struct.pack('<I%ss'%length, length, _x))
00277 _x = self
00278 buff.write(_struct_2I.pack(_x.action_result.status.goal_id.stamp.secs, _x.action_result.status.goal_id.stamp.nsecs))
00279 _x = self.action_result.status.goal_id.id
00280 length = len(_x)
00281 if python3 or type(_x) == unicode:
00282 _x = _x.encode('utf-8')
00283 length = len(_x)
00284 buff.write(struct.pack('<I%ss'%length, length, _x))
00285 buff.write(_struct_B.pack(self.action_result.status.status))
00286 _x = self.action_result.status.text
00287 length = len(_x)
00288 if python3 or type(_x) == unicode:
00289 _x = _x.encode('utf-8')
00290 length = len(_x)
00291 buff.write(struct.pack('<I%ss'%length, length, _x))
00292 buff.write(_struct_f.pack(self.action_result.result.distance_to_goal))
00293 _x = self.action_result.result.situation
00294 length = len(_x)
00295 if python3 or type(_x) == unicode:
00296 _x = _x.encode('utf-8')
00297 length = len(_x)
00298 buff.write(struct.pack('<I%ss'%length, length, _x))
00299 buff.write(_struct_Q.pack(self.action_result.result.error.error_id))
00300 _x = self.action_result.result.error.node_name
00301 length = len(_x)
00302 if python3 or type(_x) == unicode:
00303 _x = _x.encode('utf-8')
00304 length = len(_x)
00305 buff.write(struct.pack('<I%ss'%length, length, _x))
00306 _x = self.action_result.result.error.error_description
00307 length = len(_x)
00308 if python3 or type(_x) == unicode:
00309 _x = _x.encode('utf-8')
00310 length = len(_x)
00311 buff.write(struct.pack('<I%ss'%length, length, _x))
00312 length = len(self.action_result.result.better_base_ids)
00313 buff.write(_struct_I.pack(length))
00314 pattern = '<%sQ'%length
00315 buff.write(struct.pack(pattern, *self.action_result.result.better_base_ids))
00316 _x = self
00317 buff.write(_struct_3I.pack(_x.action_feedback.header.seq, _x.action_feedback.header.stamp.secs, _x.action_feedback.header.stamp.nsecs))
00318 _x = self.action_feedback.header.frame_id
00319 length = len(_x)
00320 if python3 or type(_x) == unicode:
00321 _x = _x.encode('utf-8')
00322 length = len(_x)
00323 buff.write(struct.pack('<I%ss'%length, length, _x))
00324 _x = self
00325 buff.write(_struct_2I.pack(_x.action_feedback.status.goal_id.stamp.secs, _x.action_feedback.status.goal_id.stamp.nsecs))
00326 _x = self.action_feedback.status.goal_id.id
00327 length = len(_x)
00328 if python3 or type(_x) == unicode:
00329 _x = _x.encode('utf-8')
00330 length = len(_x)
00331 buff.write(struct.pack('<I%ss'%length, length, _x))
00332 buff.write(_struct_B.pack(self.action_feedback.status.status))
00333 _x = self.action_feedback.status.text
00334 length = len(_x)
00335 if python3 or type(_x) == unicode:
00336 _x = _x.encode('utf-8')
00337 length = len(_x)
00338 buff.write(struct.pack('<I%ss'%length, length, _x))
00339 buff.write(_struct_f.pack(self.action_feedback.feedback.distance_to_goal))
00340 except struct.error as se: self._check_types(se)
00341 except TypeError as te: self._check_types(te)
00342
00343 def deserialize(self, str):
00344 """
00345 unpack serialized message in str into this message instance
00346 :param str: byte array of serialized message, ``str``
00347 """
00348 try:
00349 if self.action_goal is None:
00350 self.action_goal = cogman_msgs.msg.ArmHandActionGoal()
00351 if self.action_result is None:
00352 self.action_result = cogman_msgs.msg.ArmHandActionResult()
00353 if self.action_feedback is None:
00354 self.action_feedback = cogman_msgs.msg.ArmHandActionFeedback()
00355 end = 0
00356 _x = self
00357 start = end
00358 end += 12
00359 (_x.action_goal.header.seq, _x.action_goal.header.stamp.secs, _x.action_goal.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00360 start = end
00361 end += 4
00362 (length,) = _struct_I.unpack(str[start:end])
00363 start = end
00364 end += length
00365 if python3:
00366 self.action_goal.header.frame_id = str[start:end].decode('utf-8')
00367 else:
00368 self.action_goal.header.frame_id = str[start:end]
00369 _x = self
00370 start = end
00371 end += 8
00372 (_x.action_goal.goal_id.stamp.secs, _x.action_goal.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
00373 start = end
00374 end += 4
00375 (length,) = _struct_I.unpack(str[start:end])
00376 start = end
00377 end += length
00378 if python3:
00379 self.action_goal.goal_id.id = str[start:end].decode('utf-8')
00380 else:
00381 self.action_goal.goal_id.id = str[start:end]
00382 start = end
00383 end += 4
00384 (length,) = _struct_I.unpack(str[start:end])
00385 start = end
00386 end += length
00387 if python3:
00388 self.action_goal.goal.command = str[start:end].decode('utf-8')
00389 else:
00390 self.action_goal.goal.command = str[start:end]
00391 start = end
00392 end += 4
00393 (length,) = _struct_I.unpack(str[start:end])
00394 start = end
00395 end += length
00396 if python3:
00397 self.action_goal.goal.pose_name = str[start:end].decode('utf-8')
00398 else:
00399 self.action_goal.goal.pose_name = str[start:end]
00400 start = end
00401 end += 4
00402 (length,) = _struct_I.unpack(str[start:end])
00403 pattern = '<%sf'%length
00404 start = end
00405 end += struct.calcsize(pattern)
00406 self.action_goal.goal.joint_angles = struct.unpack(pattern, str[start:end])
00407 _x = self
00408 start = end
00409 end += 64
00410 (_x.action_goal.goal.end_effector_pose.position.x, _x.action_goal.goal.end_effector_pose.position.y, _x.action_goal.goal.end_effector_pose.position.z, _x.action_goal.goal.end_effector_pose.orientation.x, _x.action_goal.goal.end_effector_pose.orientation.y, _x.action_goal.goal.end_effector_pose.orientation.z, _x.action_goal.goal.end_effector_pose.orientation.w, _x.action_goal.goal.end_effector_loid,) = _struct_7dQ.unpack(str[start:end])
00411 start = end
00412 end += 4
00413 (length,) = _struct_I.unpack(str[start:end])
00414 start = end
00415 end += length
00416 if python3:
00417 self.action_goal.goal.hand_primitive = str[start:end].decode('utf-8')
00418 else:
00419 self.action_goal.goal.hand_primitive = str[start:end]
00420 start = end
00421 end += 4
00422 (length,) = _struct_I.unpack(str[start:end])
00423 start = end
00424 end += length
00425 if python3:
00426 self.action_goal.goal.object_type = str[start:end].decode('utf-8')
00427 else:
00428 self.action_goal.goal.object_type = str[start:end]
00429 start = end
00430 end += 4
00431 (length,) = _struct_I.unpack(str[start:end])
00432 pattern = '<%sQ'%length
00433 start = end
00434 end += struct.calcsize(pattern)
00435 self.action_goal.goal.obstacle_ids = struct.unpack(pattern, str[start:end])
00436 _x = self
00437 start = end
00438 end += 20
00439 (_x.action_goal.goal.distance, _x.action_goal.goal.supporting_plane, _x.action_result.header.seq, _x.action_result.header.stamp.secs, _x.action_result.header.stamp.nsecs,) = _struct_2f3I.unpack(str[start:end])
00440 start = end
00441 end += 4
00442 (length,) = _struct_I.unpack(str[start:end])
00443 start = end
00444 end += length
00445 if python3:
00446 self.action_result.header.frame_id = str[start:end].decode('utf-8')
00447 else:
00448 self.action_result.header.frame_id = str[start:end]
00449 _x = self
00450 start = end
00451 end += 8
00452 (_x.action_result.status.goal_id.stamp.secs, _x.action_result.status.goal_id.stamp.nsecs,) = _struct_2I.unpack(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_result.status.goal_id.id = str[start:end].decode('utf-8')
00460 else:
00461 self.action_result.status.goal_id.id = str[start:end]
00462 start = end
00463 end += 1
00464 (self.action_result.status.status,) = _struct_B.unpack(str[start:end])
00465 start = end
00466 end += 4
00467 (length,) = _struct_I.unpack(str[start:end])
00468 start = end
00469 end += length
00470 if python3:
00471 self.action_result.status.text = str[start:end].decode('utf-8')
00472 else:
00473 self.action_result.status.text = str[start:end]
00474 start = end
00475 end += 4
00476 (self.action_result.result.distance_to_goal,) = _struct_f.unpack(str[start:end])
00477 start = end
00478 end += 4
00479 (length,) = _struct_I.unpack(str[start:end])
00480 start = end
00481 end += length
00482 if python3:
00483 self.action_result.result.situation = str[start:end].decode('utf-8')
00484 else:
00485 self.action_result.result.situation = str[start:end]
00486 start = end
00487 end += 8
00488 (self.action_result.result.error.error_id,) = _struct_Q.unpack(str[start:end])
00489 start = end
00490 end += 4
00491 (length,) = _struct_I.unpack(str[start:end])
00492 start = end
00493 end += length
00494 if python3:
00495 self.action_result.result.error.node_name = str[start:end].decode('utf-8')
00496 else:
00497 self.action_result.result.error.node_name = str[start:end]
00498 start = end
00499 end += 4
00500 (length,) = _struct_I.unpack(str[start:end])
00501 start = end
00502 end += length
00503 if python3:
00504 self.action_result.result.error.error_description = str[start:end].decode('utf-8')
00505 else:
00506 self.action_result.result.error.error_description = str[start:end]
00507 start = end
00508 end += 4
00509 (length,) = _struct_I.unpack(str[start:end])
00510 pattern = '<%sQ'%length
00511 start = end
00512 end += struct.calcsize(pattern)
00513 self.action_result.result.better_base_ids = struct.unpack(pattern, str[start:end])
00514 _x = self
00515 start = end
00516 end += 12
00517 (_x.action_feedback.header.seq, _x.action_feedback.header.stamp.secs, _x.action_feedback.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00518 start = end
00519 end += 4
00520 (length,) = _struct_I.unpack(str[start:end])
00521 start = end
00522 end += length
00523 if python3:
00524 self.action_feedback.header.frame_id = str[start:end].decode('utf-8')
00525 else:
00526 self.action_feedback.header.frame_id = str[start:end]
00527 _x = self
00528 start = end
00529 end += 8
00530 (_x.action_feedback.status.goal_id.stamp.secs, _x.action_feedback.status.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
00531 start = end
00532 end += 4
00533 (length,) = _struct_I.unpack(str[start:end])
00534 start = end
00535 end += length
00536 if python3:
00537 self.action_feedback.status.goal_id.id = str[start:end].decode('utf-8')
00538 else:
00539 self.action_feedback.status.goal_id.id = str[start:end]
00540 start = end
00541 end += 1
00542 (self.action_feedback.status.status,) = _struct_B.unpack(str[start:end])
00543 start = end
00544 end += 4
00545 (length,) = _struct_I.unpack(str[start:end])
00546 start = end
00547 end += length
00548 if python3:
00549 self.action_feedback.status.text = str[start:end].decode('utf-8')
00550 else:
00551 self.action_feedback.status.text = str[start:end]
00552 start = end
00553 end += 4
00554 (self.action_feedback.feedback.distance_to_goal,) = _struct_f.unpack(str[start:end])
00555 return self
00556 except struct.error as e:
00557 raise genpy.DeserializationError(e)
00558
00559
00560 def serialize_numpy(self, buff, numpy):
00561 """
00562 serialize message with numpy array types into buffer
00563 :param buff: buffer, ``StringIO``
00564 :param numpy: numpy python module
00565 """
00566 try:
00567 _x = self
00568 buff.write(_struct_3I.pack(_x.action_goal.header.seq, _x.action_goal.header.stamp.secs, _x.action_goal.header.stamp.nsecs))
00569 _x = self.action_goal.header.frame_id
00570 length = len(_x)
00571 if python3 or type(_x) == unicode:
00572 _x = _x.encode('utf-8')
00573 length = len(_x)
00574 buff.write(struct.pack('<I%ss'%length, length, _x))
00575 _x = self
00576 buff.write(_struct_2I.pack(_x.action_goal.goal_id.stamp.secs, _x.action_goal.goal_id.stamp.nsecs))
00577 _x = self.action_goal.goal_id.id
00578 length = len(_x)
00579 if python3 or type(_x) == unicode:
00580 _x = _x.encode('utf-8')
00581 length = len(_x)
00582 buff.write(struct.pack('<I%ss'%length, length, _x))
00583 _x = self.action_goal.goal.command
00584 length = len(_x)
00585 if python3 or type(_x) == unicode:
00586 _x = _x.encode('utf-8')
00587 length = len(_x)
00588 buff.write(struct.pack('<I%ss'%length, length, _x))
00589 _x = self.action_goal.goal.pose_name
00590 length = len(_x)
00591 if python3 or type(_x) == unicode:
00592 _x = _x.encode('utf-8')
00593 length = len(_x)
00594 buff.write(struct.pack('<I%ss'%length, length, _x))
00595 length = len(self.action_goal.goal.joint_angles)
00596 buff.write(_struct_I.pack(length))
00597 pattern = '<%sf'%length
00598 buff.write(self.action_goal.goal.joint_angles.tostring())
00599 _x = self
00600 buff.write(_struct_7dQ.pack(_x.action_goal.goal.end_effector_pose.position.x, _x.action_goal.goal.end_effector_pose.position.y, _x.action_goal.goal.end_effector_pose.position.z, _x.action_goal.goal.end_effector_pose.orientation.x, _x.action_goal.goal.end_effector_pose.orientation.y, _x.action_goal.goal.end_effector_pose.orientation.z, _x.action_goal.goal.end_effector_pose.orientation.w, _x.action_goal.goal.end_effector_loid))
00601 _x = self.action_goal.goal.hand_primitive
00602 length = len(_x)
00603 if python3 or type(_x) == unicode:
00604 _x = _x.encode('utf-8')
00605 length = len(_x)
00606 buff.write(struct.pack('<I%ss'%length, length, _x))
00607 _x = self.action_goal.goal.object_type
00608 length = len(_x)
00609 if python3 or type(_x) == unicode:
00610 _x = _x.encode('utf-8')
00611 length = len(_x)
00612 buff.write(struct.pack('<I%ss'%length, length, _x))
00613 length = len(self.action_goal.goal.obstacle_ids)
00614 buff.write(_struct_I.pack(length))
00615 pattern = '<%sQ'%length
00616 buff.write(self.action_goal.goal.obstacle_ids.tostring())
00617 _x = self
00618 buff.write(_struct_2f3I.pack(_x.action_goal.goal.distance, _x.action_goal.goal.supporting_plane, _x.action_result.header.seq, _x.action_result.header.stamp.secs, _x.action_result.header.stamp.nsecs))
00619 _x = self.action_result.header.frame_id
00620 length = len(_x)
00621 if python3 or type(_x) == unicode:
00622 _x = _x.encode('utf-8')
00623 length = len(_x)
00624 buff.write(struct.pack('<I%ss'%length, length, _x))
00625 _x = self
00626 buff.write(_struct_2I.pack(_x.action_result.status.goal_id.stamp.secs, _x.action_result.status.goal_id.stamp.nsecs))
00627 _x = self.action_result.status.goal_id.id
00628 length = len(_x)
00629 if python3 or type(_x) == unicode:
00630 _x = _x.encode('utf-8')
00631 length = len(_x)
00632 buff.write(struct.pack('<I%ss'%length, length, _x))
00633 buff.write(_struct_B.pack(self.action_result.status.status))
00634 _x = self.action_result.status.text
00635 length = len(_x)
00636 if python3 or type(_x) == unicode:
00637 _x = _x.encode('utf-8')
00638 length = len(_x)
00639 buff.write(struct.pack('<I%ss'%length, length, _x))
00640 buff.write(_struct_f.pack(self.action_result.result.distance_to_goal))
00641 _x = self.action_result.result.situation
00642 length = len(_x)
00643 if python3 or type(_x) == unicode:
00644 _x = _x.encode('utf-8')
00645 length = len(_x)
00646 buff.write(struct.pack('<I%ss'%length, length, _x))
00647 buff.write(_struct_Q.pack(self.action_result.result.error.error_id))
00648 _x = self.action_result.result.error.node_name
00649 length = len(_x)
00650 if python3 or type(_x) == unicode:
00651 _x = _x.encode('utf-8')
00652 length = len(_x)
00653 buff.write(struct.pack('<I%ss'%length, length, _x))
00654 _x = self.action_result.result.error.error_description
00655 length = len(_x)
00656 if python3 or type(_x) == unicode:
00657 _x = _x.encode('utf-8')
00658 length = len(_x)
00659 buff.write(struct.pack('<I%ss'%length, length, _x))
00660 length = len(self.action_result.result.better_base_ids)
00661 buff.write(_struct_I.pack(length))
00662 pattern = '<%sQ'%length
00663 buff.write(self.action_result.result.better_base_ids.tostring())
00664 _x = self
00665 buff.write(_struct_3I.pack(_x.action_feedback.header.seq, _x.action_feedback.header.stamp.secs, _x.action_feedback.header.stamp.nsecs))
00666 _x = self.action_feedback.header.frame_id
00667 length = len(_x)
00668 if python3 or type(_x) == unicode:
00669 _x = _x.encode('utf-8')
00670 length = len(_x)
00671 buff.write(struct.pack('<I%ss'%length, length, _x))
00672 _x = self
00673 buff.write(_struct_2I.pack(_x.action_feedback.status.goal_id.stamp.secs, _x.action_feedback.status.goal_id.stamp.nsecs))
00674 _x = self.action_feedback.status.goal_id.id
00675 length = len(_x)
00676 if python3 or type(_x) == unicode:
00677 _x = _x.encode('utf-8')
00678 length = len(_x)
00679 buff.write(struct.pack('<I%ss'%length, length, _x))
00680 buff.write(_struct_B.pack(self.action_feedback.status.status))
00681 _x = self.action_feedback.status.text
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 buff.write(_struct_f.pack(self.action_feedback.feedback.distance_to_goal))
00688 except struct.error as se: self._check_types(se)
00689 except TypeError as te: self._check_types(te)
00690
00691 def deserialize_numpy(self, str, numpy):
00692 """
00693 unpack serialized message in str into this message instance using numpy for array types
00694 :param str: byte array of serialized message, ``str``
00695 :param numpy: numpy python module
00696 """
00697 try:
00698 if self.action_goal is None:
00699 self.action_goal = cogman_msgs.msg.ArmHandActionGoal()
00700 if self.action_result is None:
00701 self.action_result = cogman_msgs.msg.ArmHandActionResult()
00702 if self.action_feedback is None:
00703 self.action_feedback = cogman_msgs.msg.ArmHandActionFeedback()
00704 end = 0
00705 _x = self
00706 start = end
00707 end += 12
00708 (_x.action_goal.header.seq, _x.action_goal.header.stamp.secs, _x.action_goal.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00709 start = end
00710 end += 4
00711 (length,) = _struct_I.unpack(str[start:end])
00712 start = end
00713 end += length
00714 if python3:
00715 self.action_goal.header.frame_id = str[start:end].decode('utf-8')
00716 else:
00717 self.action_goal.header.frame_id = str[start:end]
00718 _x = self
00719 start = end
00720 end += 8
00721 (_x.action_goal.goal_id.stamp.secs, _x.action_goal.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
00722 start = end
00723 end += 4
00724 (length,) = _struct_I.unpack(str[start:end])
00725 start = end
00726 end += length
00727 if python3:
00728 self.action_goal.goal_id.id = str[start:end].decode('utf-8')
00729 else:
00730 self.action_goal.goal_id.id = str[start:end]
00731 start = end
00732 end += 4
00733 (length,) = _struct_I.unpack(str[start:end])
00734 start = end
00735 end += length
00736 if python3:
00737 self.action_goal.goal.command = str[start:end].decode('utf-8')
00738 else:
00739 self.action_goal.goal.command = str[start:end]
00740 start = end
00741 end += 4
00742 (length,) = _struct_I.unpack(str[start:end])
00743 start = end
00744 end += length
00745 if python3:
00746 self.action_goal.goal.pose_name = str[start:end].decode('utf-8')
00747 else:
00748 self.action_goal.goal.pose_name = str[start:end]
00749 start = end
00750 end += 4
00751 (length,) = _struct_I.unpack(str[start:end])
00752 pattern = '<%sf'%length
00753 start = end
00754 end += struct.calcsize(pattern)
00755 self.action_goal.goal.joint_angles = numpy.frombuffer(str[start:end], dtype=numpy.float32, count=length)
00756 _x = self
00757 start = end
00758 end += 64
00759 (_x.action_goal.goal.end_effector_pose.position.x, _x.action_goal.goal.end_effector_pose.position.y, _x.action_goal.goal.end_effector_pose.position.z, _x.action_goal.goal.end_effector_pose.orientation.x, _x.action_goal.goal.end_effector_pose.orientation.y, _x.action_goal.goal.end_effector_pose.orientation.z, _x.action_goal.goal.end_effector_pose.orientation.w, _x.action_goal.goal.end_effector_loid,) = _struct_7dQ.unpack(str[start:end])
00760 start = end
00761 end += 4
00762 (length,) = _struct_I.unpack(str[start:end])
00763 start = end
00764 end += length
00765 if python3:
00766 self.action_goal.goal.hand_primitive = str[start:end].decode('utf-8')
00767 else:
00768 self.action_goal.goal.hand_primitive = str[start:end]
00769 start = end
00770 end += 4
00771 (length,) = _struct_I.unpack(str[start:end])
00772 start = end
00773 end += length
00774 if python3:
00775 self.action_goal.goal.object_type = str[start:end].decode('utf-8')
00776 else:
00777 self.action_goal.goal.object_type = str[start:end]
00778 start = end
00779 end += 4
00780 (length,) = _struct_I.unpack(str[start:end])
00781 pattern = '<%sQ'%length
00782 start = end
00783 end += struct.calcsize(pattern)
00784 self.action_goal.goal.obstacle_ids = numpy.frombuffer(str[start:end], dtype=numpy.uint64, count=length)
00785 _x = self
00786 start = end
00787 end += 20
00788 (_x.action_goal.goal.distance, _x.action_goal.goal.supporting_plane, _x.action_result.header.seq, _x.action_result.header.stamp.secs, _x.action_result.header.stamp.nsecs,) = _struct_2f3I.unpack(str[start:end])
00789 start = end
00790 end += 4
00791 (length,) = _struct_I.unpack(str[start:end])
00792 start = end
00793 end += length
00794 if python3:
00795 self.action_result.header.frame_id = str[start:end].decode('utf-8')
00796 else:
00797 self.action_result.header.frame_id = str[start:end]
00798 _x = self
00799 start = end
00800 end += 8
00801 (_x.action_result.status.goal_id.stamp.secs, _x.action_result.status.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
00802 start = end
00803 end += 4
00804 (length,) = _struct_I.unpack(str[start:end])
00805 start = end
00806 end += length
00807 if python3:
00808 self.action_result.status.goal_id.id = str[start:end].decode('utf-8')
00809 else:
00810 self.action_result.status.goal_id.id = str[start:end]
00811 start = end
00812 end += 1
00813 (self.action_result.status.status,) = _struct_B.unpack(str[start:end])
00814 start = end
00815 end += 4
00816 (length,) = _struct_I.unpack(str[start:end])
00817 start = end
00818 end += length
00819 if python3:
00820 self.action_result.status.text = str[start:end].decode('utf-8')
00821 else:
00822 self.action_result.status.text = str[start:end]
00823 start = end
00824 end += 4
00825 (self.action_result.result.distance_to_goal,) = _struct_f.unpack(str[start:end])
00826 start = end
00827 end += 4
00828 (length,) = _struct_I.unpack(str[start:end])
00829 start = end
00830 end += length
00831 if python3:
00832 self.action_result.result.situation = str[start:end].decode('utf-8')
00833 else:
00834 self.action_result.result.situation = str[start:end]
00835 start = end
00836 end += 8
00837 (self.action_result.result.error.error_id,) = _struct_Q.unpack(str[start:end])
00838 start = end
00839 end += 4
00840 (length,) = _struct_I.unpack(str[start:end])
00841 start = end
00842 end += length
00843 if python3:
00844 self.action_result.result.error.node_name = str[start:end].decode('utf-8')
00845 else:
00846 self.action_result.result.error.node_name = str[start:end]
00847 start = end
00848 end += 4
00849 (length,) = _struct_I.unpack(str[start:end])
00850 start = end
00851 end += length
00852 if python3:
00853 self.action_result.result.error.error_description = str[start:end].decode('utf-8')
00854 else:
00855 self.action_result.result.error.error_description = str[start:end]
00856 start = end
00857 end += 4
00858 (length,) = _struct_I.unpack(str[start:end])
00859 pattern = '<%sQ'%length
00860 start = end
00861 end += struct.calcsize(pattern)
00862 self.action_result.result.better_base_ids = numpy.frombuffer(str[start:end], dtype=numpy.uint64, count=length)
00863 _x = self
00864 start = end
00865 end += 12
00866 (_x.action_feedback.header.seq, _x.action_feedback.header.stamp.secs, _x.action_feedback.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00867 start = end
00868 end += 4
00869 (length,) = _struct_I.unpack(str[start:end])
00870 start = end
00871 end += length
00872 if python3:
00873 self.action_feedback.header.frame_id = str[start:end].decode('utf-8')
00874 else:
00875 self.action_feedback.header.frame_id = str[start:end]
00876 _x = self
00877 start = end
00878 end += 8
00879 (_x.action_feedback.status.goal_id.stamp.secs, _x.action_feedback.status.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
00880 start = end
00881 end += 4
00882 (length,) = _struct_I.unpack(str[start:end])
00883 start = end
00884 end += length
00885 if python3:
00886 self.action_feedback.status.goal_id.id = str[start:end].decode('utf-8')
00887 else:
00888 self.action_feedback.status.goal_id.id = str[start:end]
00889 start = end
00890 end += 1
00891 (self.action_feedback.status.status,) = _struct_B.unpack(str[start:end])
00892 start = end
00893 end += 4
00894 (length,) = _struct_I.unpack(str[start:end])
00895 start = end
00896 end += length
00897 if python3:
00898 self.action_feedback.status.text = str[start:end].decode('utf-8')
00899 else:
00900 self.action_feedback.status.text = str[start:end]
00901 start = end
00902 end += 4
00903 (self.action_feedback.feedback.distance_to_goal,) = _struct_f.unpack(str[start:end])
00904 return self
00905 except struct.error as e:
00906 raise genpy.DeserializationError(e)
00907
00908 _struct_I = genpy.struct_I
00909 _struct_B = struct.Struct("<B")
00910 _struct_2f3I = struct.Struct("<2f3I")
00911 _struct_f = struct.Struct("<f")
00912 _struct_Q = struct.Struct("<Q")
00913 _struct_7dQ = struct.Struct("<7dQ")
00914 _struct_3I = struct.Struct("<3I")
00915 _struct_2I = struct.Struct("<2I")