00001 """autogenerated by genpy from iri_wam_arm_navigation/SimplePoseAction.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 arm_navigation_msgs.msg
00008 import actionlib_msgs.msg
00009 import geometry_msgs.msg
00010 import iri_wam_arm_navigation.msg
00011 import genpy
00012 import std_msgs.msg
00013
00014 class SimplePoseAction(genpy.Message):
00015 _md5sum = "52146bcfd0f04c9d19a9ed68e60ad2f5"
00016 _type = "iri_wam_arm_navigation/SimplePoseAction"
00017 _has_header = False
00018 _full_text = """# ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00019
00020 SimplePoseActionGoal action_goal
00021 SimplePoseActionResult action_result
00022 SimplePoseActionFeedback action_feedback
00023
00024 ================================================================================
00025 MSG: iri_wam_arm_navigation/SimplePoseActionGoal
00026 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00027
00028 Header header
00029 actionlib_msgs/GoalID goal_id
00030 SimplePoseGoal 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: iri_wam_arm_navigation/SimplePoseGoal
00065 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00066 #Goal Definition
00067 geometry_msgs/PoseStamped goal
00068 string frame_id # This is the name of link to move, and not necessary is the same frame_id of PoseStamped
00069
00070 ================================================================================
00071 MSG: geometry_msgs/PoseStamped
00072 # A Pose with reference coordinate frame and timestamp
00073 Header header
00074 Pose pose
00075
00076 ================================================================================
00077 MSG: geometry_msgs/Pose
00078 # A representation of pose in free space, composed of postion and orientation.
00079 Point position
00080 Quaternion orientation
00081
00082 ================================================================================
00083 MSG: geometry_msgs/Point
00084 # This contains the position of a point in free space
00085 float64 x
00086 float64 y
00087 float64 z
00088
00089 ================================================================================
00090 MSG: geometry_msgs/Quaternion
00091 # This represents an orientation in free space in quaternion form.
00092
00093 float64 x
00094 float64 y
00095 float64 z
00096 float64 w
00097
00098 ================================================================================
00099 MSG: iri_wam_arm_navigation/SimplePoseActionResult
00100 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00101
00102 Header header
00103 actionlib_msgs/GoalStatus status
00104 SimplePoseResult result
00105
00106 ================================================================================
00107 MSG: actionlib_msgs/GoalStatus
00108 GoalID goal_id
00109 uint8 status
00110 uint8 PENDING = 0 # The goal has yet to be processed by the action server
00111 uint8 ACTIVE = 1 # The goal is currently being processed by the action server
00112 uint8 PREEMPTED = 2 # The goal received a cancel request after it started executing
00113 # and has since completed its execution (Terminal State)
00114 uint8 SUCCEEDED = 3 # The goal was achieved successfully by the action server (Terminal State)
00115 uint8 ABORTED = 4 # The goal was aborted during execution by the action server due
00116 # to some failure (Terminal State)
00117 uint8 REJECTED = 5 # The goal was rejected by the action server without being processed,
00118 # because the goal was unattainable or invalid (Terminal State)
00119 uint8 PREEMPTING = 6 # The goal received a cancel request after it started executing
00120 # and has not yet completed execution
00121 uint8 RECALLING = 7 # The goal received a cancel request before it started executing,
00122 # but the action server has not yet confirmed that the goal is canceled
00123 uint8 RECALLED = 8 # The goal received a cancel request before it started executing
00124 # and was successfully cancelled (Terminal State)
00125 uint8 LOST = 9 # An action client can determine that a goal is LOST. This should not be
00126 # sent over the wire by an action server
00127
00128 #Allow for the user to associate a string with GoalStatus for debugging
00129 string text
00130
00131
00132 ================================================================================
00133 MSG: iri_wam_arm_navigation/SimplePoseResult
00134 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00135 #Result Definition
00136 # An error code reflecting what went wrong
00137 arm_navigation_msgs/ArmNavigationErrorCodes error_code
00138
00139 ================================================================================
00140 MSG: arm_navigation_msgs/ArmNavigationErrorCodes
00141 int32 val
00142
00143 # overall behavior
00144 int32 PLANNING_FAILED=-1
00145 int32 SUCCESS=1
00146 int32 TIMED_OUT=-2
00147
00148 # start state errors
00149 int32 START_STATE_IN_COLLISION=-3
00150 int32 START_STATE_VIOLATES_PATH_CONSTRAINTS=-4
00151
00152 # goal errors
00153 int32 GOAL_IN_COLLISION=-5
00154 int32 GOAL_VIOLATES_PATH_CONSTRAINTS=-6
00155
00156 # robot state
00157 int32 INVALID_ROBOT_STATE=-7
00158 int32 INCOMPLETE_ROBOT_STATE=-8
00159
00160 # planning request errors
00161 int32 INVALID_PLANNER_ID=-9
00162 int32 INVALID_NUM_PLANNING_ATTEMPTS=-10
00163 int32 INVALID_ALLOWED_PLANNING_TIME=-11
00164 int32 INVALID_GROUP_NAME=-12
00165 int32 INVALID_GOAL_JOINT_CONSTRAINTS=-13
00166 int32 INVALID_GOAL_POSITION_CONSTRAINTS=-14
00167 int32 INVALID_GOAL_ORIENTATION_CONSTRAINTS=-15
00168 int32 INVALID_PATH_JOINT_CONSTRAINTS=-16
00169 int32 INVALID_PATH_POSITION_CONSTRAINTS=-17
00170 int32 INVALID_PATH_ORIENTATION_CONSTRAINTS=-18
00171
00172 # state/trajectory monitor errors
00173 int32 INVALID_TRAJECTORY=-19
00174 int32 INVALID_INDEX=-20
00175 int32 JOINT_LIMITS_VIOLATED=-21
00176 int32 PATH_CONSTRAINTS_VIOLATED=-22
00177 int32 COLLISION_CONSTRAINTS_VIOLATED=-23
00178 int32 GOAL_CONSTRAINTS_VIOLATED=-24
00179 int32 JOINTS_NOT_MOVING=-25
00180 int32 TRAJECTORY_CONTROLLER_FAILED=-26
00181
00182 # system errors
00183 int32 FRAME_TRANSFORM_FAILURE=-27
00184 int32 COLLISION_CHECKING_UNAVAILABLE=-28
00185 int32 ROBOT_STATE_STALE=-29
00186 int32 SENSOR_INFO_STALE=-30
00187
00188 # kinematics errors
00189 int32 NO_IK_SOLUTION=-31
00190 int32 INVALID_LINK_NAME=-32
00191 int32 IK_LINK_IN_COLLISION=-33
00192 int32 NO_FK_SOLUTION=-34
00193 int32 KINEMATICS_STATE_IN_COLLISION=-35
00194
00195 # general errors
00196 int32 INVALID_TIMEOUT=-36
00197
00198
00199 ================================================================================
00200 MSG: iri_wam_arm_navigation/SimplePoseActionFeedback
00201 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00202
00203 Header header
00204 actionlib_msgs/GoalStatus status
00205 SimplePoseFeedback feedback
00206
00207 ================================================================================
00208 MSG: iri_wam_arm_navigation/SimplePoseFeedback
00209 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00210 #Feedback Definition
00211 bool succesed
00212 string state
00213
00214
00215 """
00216 __slots__ = ['action_goal','action_result','action_feedback']
00217 _slot_types = ['iri_wam_arm_navigation/SimplePoseActionGoal','iri_wam_arm_navigation/SimplePoseActionResult','iri_wam_arm_navigation/SimplePoseActionFeedback']
00218
00219 def __init__(self, *args, **kwds):
00220 """
00221 Constructor. Any message fields that are implicitly/explicitly
00222 set to None will be assigned a default value. The recommend
00223 use is keyword arguments as this is more robust to future message
00224 changes. You cannot mix in-order arguments and keyword arguments.
00225
00226 The available fields are:
00227 action_goal,action_result,action_feedback
00228
00229 :param args: complete set of field values, in .msg order
00230 :param kwds: use keyword arguments corresponding to message field names
00231 to set specific fields.
00232 """
00233 if args or kwds:
00234 super(SimplePoseAction, self).__init__(*args, **kwds)
00235
00236 if self.action_goal is None:
00237 self.action_goal = iri_wam_arm_navigation.msg.SimplePoseActionGoal()
00238 if self.action_result is None:
00239 self.action_result = iri_wam_arm_navigation.msg.SimplePoseActionResult()
00240 if self.action_feedback is None:
00241 self.action_feedback = iri_wam_arm_navigation.msg.SimplePoseActionFeedback()
00242 else:
00243 self.action_goal = iri_wam_arm_navigation.msg.SimplePoseActionGoal()
00244 self.action_result = iri_wam_arm_navigation.msg.SimplePoseActionResult()
00245 self.action_feedback = iri_wam_arm_navigation.msg.SimplePoseActionFeedback()
00246
00247 def _get_types(self):
00248 """
00249 internal API method
00250 """
00251 return self._slot_types
00252
00253 def serialize(self, buff):
00254 """
00255 serialize message into buffer
00256 :param buff: buffer, ``StringIO``
00257 """
00258 try:
00259 _x = self
00260 buff.write(_struct_3I.pack(_x.action_goal.header.seq, _x.action_goal.header.stamp.secs, _x.action_goal.header.stamp.nsecs))
00261 _x = self.action_goal.header.frame_id
00262 length = len(_x)
00263 if python3 or type(_x) == unicode:
00264 _x = _x.encode('utf-8')
00265 length = len(_x)
00266 buff.write(struct.pack('<I%ss'%length, length, _x))
00267 _x = self
00268 buff.write(_struct_2I.pack(_x.action_goal.goal_id.stamp.secs, _x.action_goal.goal_id.stamp.nsecs))
00269 _x = self.action_goal.goal_id.id
00270 length = len(_x)
00271 if python3 or type(_x) == unicode:
00272 _x = _x.encode('utf-8')
00273 length = len(_x)
00274 buff.write(struct.pack('<I%ss'%length, length, _x))
00275 _x = self
00276 buff.write(_struct_3I.pack(_x.action_goal.goal.goal.header.seq, _x.action_goal.goal.goal.header.stamp.secs, _x.action_goal.goal.goal.header.stamp.nsecs))
00277 _x = self.action_goal.goal.goal.header.frame_id
00278 length = len(_x)
00279 if python3 or type(_x) == unicode:
00280 _x = _x.encode('utf-8')
00281 length = len(_x)
00282 buff.write(struct.pack('<I%ss'%length, length, _x))
00283 _x = self
00284 buff.write(_struct_7d.pack(_x.action_goal.goal.goal.pose.position.x, _x.action_goal.goal.goal.pose.position.y, _x.action_goal.goal.goal.pose.position.z, _x.action_goal.goal.goal.pose.orientation.x, _x.action_goal.goal.goal.pose.orientation.y, _x.action_goal.goal.goal.pose.orientation.z, _x.action_goal.goal.goal.pose.orientation.w))
00285 _x = self.action_goal.goal.frame_id
00286 length = len(_x)
00287 if python3 or type(_x) == unicode:
00288 _x = _x.encode('utf-8')
00289 length = len(_x)
00290 buff.write(struct.pack('<I%ss'%length, length, _x))
00291 _x = self
00292 buff.write(_struct_3I.pack(_x.action_result.header.seq, _x.action_result.header.stamp.secs, _x.action_result.header.stamp.nsecs))
00293 _x = self.action_result.header.frame_id
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 _x = self
00300 buff.write(_struct_2I.pack(_x.action_result.status.goal_id.stamp.secs, _x.action_result.status.goal_id.stamp.nsecs))
00301 _x = self.action_result.status.goal_id.id
00302 length = len(_x)
00303 if python3 or type(_x) == unicode:
00304 _x = _x.encode('utf-8')
00305 length = len(_x)
00306 buff.write(struct.pack('<I%ss'%length, length, _x))
00307 buff.write(_struct_B.pack(self.action_result.status.status))
00308 _x = self.action_result.status.text
00309 length = len(_x)
00310 if python3 or type(_x) == unicode:
00311 _x = _x.encode('utf-8')
00312 length = len(_x)
00313 buff.write(struct.pack('<I%ss'%length, length, _x))
00314 _x = self
00315 buff.write(_struct_i3I.pack(_x.action_result.result.error_code.val, _x.action_feedback.header.seq, _x.action_feedback.header.stamp.secs, _x.action_feedback.header.stamp.nsecs))
00316 _x = self.action_feedback.header.frame_id
00317 length = len(_x)
00318 if python3 or type(_x) == unicode:
00319 _x = _x.encode('utf-8')
00320 length = len(_x)
00321 buff.write(struct.pack('<I%ss'%length, length, _x))
00322 _x = self
00323 buff.write(_struct_2I.pack(_x.action_feedback.status.goal_id.stamp.secs, _x.action_feedback.status.goal_id.stamp.nsecs))
00324 _x = self.action_feedback.status.goal_id.id
00325 length = len(_x)
00326 if python3 or type(_x) == unicode:
00327 _x = _x.encode('utf-8')
00328 length = len(_x)
00329 buff.write(struct.pack('<I%ss'%length, length, _x))
00330 buff.write(_struct_B.pack(self.action_feedback.status.status))
00331 _x = self.action_feedback.status.text
00332 length = len(_x)
00333 if python3 or type(_x) == unicode:
00334 _x = _x.encode('utf-8')
00335 length = len(_x)
00336 buff.write(struct.pack('<I%ss'%length, length, _x))
00337 buff.write(_struct_B.pack(self.action_feedback.feedback.succesed))
00338 _x = self.action_feedback.feedback.state
00339 length = len(_x)
00340 if python3 or type(_x) == unicode:
00341 _x = _x.encode('utf-8')
00342 length = len(_x)
00343 buff.write(struct.pack('<I%ss'%length, length, _x))
00344 except struct.error as se: self._check_types(se)
00345 except TypeError as te: self._check_types(te)
00346
00347 def deserialize(self, str):
00348 """
00349 unpack serialized message in str into this message instance
00350 :param str: byte array of serialized message, ``str``
00351 """
00352 try:
00353 if self.action_goal is None:
00354 self.action_goal = iri_wam_arm_navigation.msg.SimplePoseActionGoal()
00355 if self.action_result is None:
00356 self.action_result = iri_wam_arm_navigation.msg.SimplePoseActionResult()
00357 if self.action_feedback is None:
00358 self.action_feedback = iri_wam_arm_navigation.msg.SimplePoseActionFeedback()
00359 end = 0
00360 _x = self
00361 start = end
00362 end += 12
00363 (_x.action_goal.header.seq, _x.action_goal.header.stamp.secs, _x.action_goal.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00364 start = end
00365 end += 4
00366 (length,) = _struct_I.unpack(str[start:end])
00367 start = end
00368 end += length
00369 if python3:
00370 self.action_goal.header.frame_id = str[start:end].decode('utf-8')
00371 else:
00372 self.action_goal.header.frame_id = str[start:end]
00373 _x = self
00374 start = end
00375 end += 8
00376 (_x.action_goal.goal_id.stamp.secs, _x.action_goal.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
00377 start = end
00378 end += 4
00379 (length,) = _struct_I.unpack(str[start:end])
00380 start = end
00381 end += length
00382 if python3:
00383 self.action_goal.goal_id.id = str[start:end].decode('utf-8')
00384 else:
00385 self.action_goal.goal_id.id = str[start:end]
00386 _x = self
00387 start = end
00388 end += 12
00389 (_x.action_goal.goal.goal.header.seq, _x.action_goal.goal.goal.header.stamp.secs, _x.action_goal.goal.goal.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00390 start = end
00391 end += 4
00392 (length,) = _struct_I.unpack(str[start:end])
00393 start = end
00394 end += length
00395 if python3:
00396 self.action_goal.goal.goal.header.frame_id = str[start:end].decode('utf-8')
00397 else:
00398 self.action_goal.goal.goal.header.frame_id = str[start:end]
00399 _x = self
00400 start = end
00401 end += 56
00402 (_x.action_goal.goal.goal.pose.position.x, _x.action_goal.goal.goal.pose.position.y, _x.action_goal.goal.goal.pose.position.z, _x.action_goal.goal.goal.pose.orientation.x, _x.action_goal.goal.goal.pose.orientation.y, _x.action_goal.goal.goal.pose.orientation.z, _x.action_goal.goal.goal.pose.orientation.w,) = _struct_7d.unpack(str[start:end])
00403 start = end
00404 end += 4
00405 (length,) = _struct_I.unpack(str[start:end])
00406 start = end
00407 end += length
00408 if python3:
00409 self.action_goal.goal.frame_id = str[start:end].decode('utf-8')
00410 else:
00411 self.action_goal.goal.frame_id = str[start:end]
00412 _x = self
00413 start = end
00414 end += 12
00415 (_x.action_result.header.seq, _x.action_result.header.stamp.secs, _x.action_result.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00416 start = end
00417 end += 4
00418 (length,) = _struct_I.unpack(str[start:end])
00419 start = end
00420 end += length
00421 if python3:
00422 self.action_result.header.frame_id = str[start:end].decode('utf-8')
00423 else:
00424 self.action_result.header.frame_id = str[start:end]
00425 _x = self
00426 start = end
00427 end += 8
00428 (_x.action_result.status.goal_id.stamp.secs, _x.action_result.status.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
00429 start = end
00430 end += 4
00431 (length,) = _struct_I.unpack(str[start:end])
00432 start = end
00433 end += length
00434 if python3:
00435 self.action_result.status.goal_id.id = str[start:end].decode('utf-8')
00436 else:
00437 self.action_result.status.goal_id.id = str[start:end]
00438 start = end
00439 end += 1
00440 (self.action_result.status.status,) = _struct_B.unpack(str[start:end])
00441 start = end
00442 end += 4
00443 (length,) = _struct_I.unpack(str[start:end])
00444 start = end
00445 end += length
00446 if python3:
00447 self.action_result.status.text = str[start:end].decode('utf-8')
00448 else:
00449 self.action_result.status.text = str[start:end]
00450 _x = self
00451 start = end
00452 end += 16
00453 (_x.action_result.result.error_code.val, _x.action_feedback.header.seq, _x.action_feedback.header.stamp.secs, _x.action_feedback.header.stamp.nsecs,) = _struct_i3I.unpack(str[start:end])
00454 start = end
00455 end += 4
00456 (length,) = _struct_I.unpack(str[start:end])
00457 start = end
00458 end += length
00459 if python3:
00460 self.action_feedback.header.frame_id = str[start:end].decode('utf-8')
00461 else:
00462 self.action_feedback.header.frame_id = str[start:end]
00463 _x = self
00464 start = end
00465 end += 8
00466 (_x.action_feedback.status.goal_id.stamp.secs, _x.action_feedback.status.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
00467 start = end
00468 end += 4
00469 (length,) = _struct_I.unpack(str[start:end])
00470 start = end
00471 end += length
00472 if python3:
00473 self.action_feedback.status.goal_id.id = str[start:end].decode('utf-8')
00474 else:
00475 self.action_feedback.status.goal_id.id = str[start:end]
00476 start = end
00477 end += 1
00478 (self.action_feedback.status.status,) = _struct_B.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_feedback.status.text = str[start:end].decode('utf-8')
00486 else:
00487 self.action_feedback.status.text = str[start:end]
00488 start = end
00489 end += 1
00490 (self.action_feedback.feedback.succesed,) = _struct_B.unpack(str[start:end])
00491 self.action_feedback.feedback.succesed = bool(self.action_feedback.feedback.succesed)
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_feedback.feedback.state = str[start:end].decode('utf-8')
00499 else:
00500 self.action_feedback.feedback.state = str[start:end]
00501 return self
00502 except struct.error as e:
00503 raise genpy.DeserializationError(e)
00504
00505
00506 def serialize_numpy(self, buff, numpy):
00507 """
00508 serialize message with numpy array types into buffer
00509 :param buff: buffer, ``StringIO``
00510 :param numpy: numpy python module
00511 """
00512 try:
00513 _x = self
00514 buff.write(_struct_3I.pack(_x.action_goal.header.seq, _x.action_goal.header.stamp.secs, _x.action_goal.header.stamp.nsecs))
00515 _x = self.action_goal.header.frame_id
00516 length = len(_x)
00517 if python3 or type(_x) == unicode:
00518 _x = _x.encode('utf-8')
00519 length = len(_x)
00520 buff.write(struct.pack('<I%ss'%length, length, _x))
00521 _x = self
00522 buff.write(_struct_2I.pack(_x.action_goal.goal_id.stamp.secs, _x.action_goal.goal_id.stamp.nsecs))
00523 _x = self.action_goal.goal_id.id
00524 length = len(_x)
00525 if python3 or type(_x) == unicode:
00526 _x = _x.encode('utf-8')
00527 length = len(_x)
00528 buff.write(struct.pack('<I%ss'%length, length, _x))
00529 _x = self
00530 buff.write(_struct_3I.pack(_x.action_goal.goal.goal.header.seq, _x.action_goal.goal.goal.header.stamp.secs, _x.action_goal.goal.goal.header.stamp.nsecs))
00531 _x = self.action_goal.goal.goal.header.frame_id
00532 length = len(_x)
00533 if python3 or type(_x) == unicode:
00534 _x = _x.encode('utf-8')
00535 length = len(_x)
00536 buff.write(struct.pack('<I%ss'%length, length, _x))
00537 _x = self
00538 buff.write(_struct_7d.pack(_x.action_goal.goal.goal.pose.position.x, _x.action_goal.goal.goal.pose.position.y, _x.action_goal.goal.goal.pose.position.z, _x.action_goal.goal.goal.pose.orientation.x, _x.action_goal.goal.goal.pose.orientation.y, _x.action_goal.goal.goal.pose.orientation.z, _x.action_goal.goal.goal.pose.orientation.w))
00539 _x = self.action_goal.goal.frame_id
00540 length = len(_x)
00541 if python3 or type(_x) == unicode:
00542 _x = _x.encode('utf-8')
00543 length = len(_x)
00544 buff.write(struct.pack('<I%ss'%length, length, _x))
00545 _x = self
00546 buff.write(_struct_3I.pack(_x.action_result.header.seq, _x.action_result.header.stamp.secs, _x.action_result.header.stamp.nsecs))
00547 _x = self.action_result.header.frame_id
00548 length = len(_x)
00549 if python3 or type(_x) == unicode:
00550 _x = _x.encode('utf-8')
00551 length = len(_x)
00552 buff.write(struct.pack('<I%ss'%length, length, _x))
00553 _x = self
00554 buff.write(_struct_2I.pack(_x.action_result.status.goal_id.stamp.secs, _x.action_result.status.goal_id.stamp.nsecs))
00555 _x = self.action_result.status.goal_id.id
00556 length = len(_x)
00557 if python3 or type(_x) == unicode:
00558 _x = _x.encode('utf-8')
00559 length = len(_x)
00560 buff.write(struct.pack('<I%ss'%length, length, _x))
00561 buff.write(_struct_B.pack(self.action_result.status.status))
00562 _x = self.action_result.status.text
00563 length = len(_x)
00564 if python3 or type(_x) == unicode:
00565 _x = _x.encode('utf-8')
00566 length = len(_x)
00567 buff.write(struct.pack('<I%ss'%length, length, _x))
00568 _x = self
00569 buff.write(_struct_i3I.pack(_x.action_result.result.error_code.val, _x.action_feedback.header.seq, _x.action_feedback.header.stamp.secs, _x.action_feedback.header.stamp.nsecs))
00570 _x = self.action_feedback.header.frame_id
00571 length = len(_x)
00572 if python3 or type(_x) == unicode:
00573 _x = _x.encode('utf-8')
00574 length = len(_x)
00575 buff.write(struct.pack('<I%ss'%length, length, _x))
00576 _x = self
00577 buff.write(_struct_2I.pack(_x.action_feedback.status.goal_id.stamp.secs, _x.action_feedback.status.goal_id.stamp.nsecs))
00578 _x = self.action_feedback.status.goal_id.id
00579 length = len(_x)
00580 if python3 or type(_x) == unicode:
00581 _x = _x.encode('utf-8')
00582 length = len(_x)
00583 buff.write(struct.pack('<I%ss'%length, length, _x))
00584 buff.write(_struct_B.pack(self.action_feedback.status.status))
00585 _x = self.action_feedback.status.text
00586 length = len(_x)
00587 if python3 or type(_x) == unicode:
00588 _x = _x.encode('utf-8')
00589 length = len(_x)
00590 buff.write(struct.pack('<I%ss'%length, length, _x))
00591 buff.write(_struct_B.pack(self.action_feedback.feedback.succesed))
00592 _x = self.action_feedback.feedback.state
00593 length = len(_x)
00594 if python3 or type(_x) == unicode:
00595 _x = _x.encode('utf-8')
00596 length = len(_x)
00597 buff.write(struct.pack('<I%ss'%length, length, _x))
00598 except struct.error as se: self._check_types(se)
00599 except TypeError as te: self._check_types(te)
00600
00601 def deserialize_numpy(self, str, numpy):
00602 """
00603 unpack serialized message in str into this message instance using numpy for array types
00604 :param str: byte array of serialized message, ``str``
00605 :param numpy: numpy python module
00606 """
00607 try:
00608 if self.action_goal is None:
00609 self.action_goal = iri_wam_arm_navigation.msg.SimplePoseActionGoal()
00610 if self.action_result is None:
00611 self.action_result = iri_wam_arm_navigation.msg.SimplePoseActionResult()
00612 if self.action_feedback is None:
00613 self.action_feedback = iri_wam_arm_navigation.msg.SimplePoseActionFeedback()
00614 end = 0
00615 _x = self
00616 start = end
00617 end += 12
00618 (_x.action_goal.header.seq, _x.action_goal.header.stamp.secs, _x.action_goal.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00619 start = end
00620 end += 4
00621 (length,) = _struct_I.unpack(str[start:end])
00622 start = end
00623 end += length
00624 if python3:
00625 self.action_goal.header.frame_id = str[start:end].decode('utf-8')
00626 else:
00627 self.action_goal.header.frame_id = str[start:end]
00628 _x = self
00629 start = end
00630 end += 8
00631 (_x.action_goal.goal_id.stamp.secs, _x.action_goal.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
00632 start = end
00633 end += 4
00634 (length,) = _struct_I.unpack(str[start:end])
00635 start = end
00636 end += length
00637 if python3:
00638 self.action_goal.goal_id.id = str[start:end].decode('utf-8')
00639 else:
00640 self.action_goal.goal_id.id = str[start:end]
00641 _x = self
00642 start = end
00643 end += 12
00644 (_x.action_goal.goal.goal.header.seq, _x.action_goal.goal.goal.header.stamp.secs, _x.action_goal.goal.goal.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00645 start = end
00646 end += 4
00647 (length,) = _struct_I.unpack(str[start:end])
00648 start = end
00649 end += length
00650 if python3:
00651 self.action_goal.goal.goal.header.frame_id = str[start:end].decode('utf-8')
00652 else:
00653 self.action_goal.goal.goal.header.frame_id = str[start:end]
00654 _x = self
00655 start = end
00656 end += 56
00657 (_x.action_goal.goal.goal.pose.position.x, _x.action_goal.goal.goal.pose.position.y, _x.action_goal.goal.goal.pose.position.z, _x.action_goal.goal.goal.pose.orientation.x, _x.action_goal.goal.goal.pose.orientation.y, _x.action_goal.goal.goal.pose.orientation.z, _x.action_goal.goal.goal.pose.orientation.w,) = _struct_7d.unpack(str[start:end])
00658 start = end
00659 end += 4
00660 (length,) = _struct_I.unpack(str[start:end])
00661 start = end
00662 end += length
00663 if python3:
00664 self.action_goal.goal.frame_id = str[start:end].decode('utf-8')
00665 else:
00666 self.action_goal.goal.frame_id = str[start:end]
00667 _x = self
00668 start = end
00669 end += 12
00670 (_x.action_result.header.seq, _x.action_result.header.stamp.secs, _x.action_result.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00671 start = end
00672 end += 4
00673 (length,) = _struct_I.unpack(str[start:end])
00674 start = end
00675 end += length
00676 if python3:
00677 self.action_result.header.frame_id = str[start:end].decode('utf-8')
00678 else:
00679 self.action_result.header.frame_id = str[start:end]
00680 _x = self
00681 start = end
00682 end += 8
00683 (_x.action_result.status.goal_id.stamp.secs, _x.action_result.status.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
00684 start = end
00685 end += 4
00686 (length,) = _struct_I.unpack(str[start:end])
00687 start = end
00688 end += length
00689 if python3:
00690 self.action_result.status.goal_id.id = str[start:end].decode('utf-8')
00691 else:
00692 self.action_result.status.goal_id.id = str[start:end]
00693 start = end
00694 end += 1
00695 (self.action_result.status.status,) = _struct_B.unpack(str[start:end])
00696 start = end
00697 end += 4
00698 (length,) = _struct_I.unpack(str[start:end])
00699 start = end
00700 end += length
00701 if python3:
00702 self.action_result.status.text = str[start:end].decode('utf-8')
00703 else:
00704 self.action_result.status.text = str[start:end]
00705 _x = self
00706 start = end
00707 end += 16
00708 (_x.action_result.result.error_code.val, _x.action_feedback.header.seq, _x.action_feedback.header.stamp.secs, _x.action_feedback.header.stamp.nsecs,) = _struct_i3I.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_feedback.header.frame_id = str[start:end].decode('utf-8')
00716 else:
00717 self.action_feedback.header.frame_id = str[start:end]
00718 _x = self
00719 start = end
00720 end += 8
00721 (_x.action_feedback.status.goal_id.stamp.secs, _x.action_feedback.status.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_feedback.status.goal_id.id = str[start:end].decode('utf-8')
00729 else:
00730 self.action_feedback.status.goal_id.id = str[start:end]
00731 start = end
00732 end += 1
00733 (self.action_feedback.status.status,) = _struct_B.unpack(str[start:end])
00734 start = end
00735 end += 4
00736 (length,) = _struct_I.unpack(str[start:end])
00737 start = end
00738 end += length
00739 if python3:
00740 self.action_feedback.status.text = str[start:end].decode('utf-8')
00741 else:
00742 self.action_feedback.status.text = str[start:end]
00743 start = end
00744 end += 1
00745 (self.action_feedback.feedback.succesed,) = _struct_B.unpack(str[start:end])
00746 self.action_feedback.feedback.succesed = bool(self.action_feedback.feedback.succesed)
00747 start = end
00748 end += 4
00749 (length,) = _struct_I.unpack(str[start:end])
00750 start = end
00751 end += length
00752 if python3:
00753 self.action_feedback.feedback.state = str[start:end].decode('utf-8')
00754 else:
00755 self.action_feedback.feedback.state = str[start:end]
00756 return self
00757 except struct.error as e:
00758 raise genpy.DeserializationError(e)
00759
00760 _struct_I = genpy.struct_I
00761 _struct_3I = struct.Struct("<3I")
00762 _struct_7d = struct.Struct("<7d")
00763 _struct_2I = struct.Struct("<2I")
00764 _struct_B = struct.Struct("<B")
00765 _struct_i3I = struct.Struct("<i3I")