00001 """autogenerated by genpy from object_manipulation_msgs/GraspHandPostureExecutionAction.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 object_manipulation_msgs.msg
00008 import actionlib_msgs.msg
00009 import manipulation_msgs.msg
00010 import geometry_msgs.msg
00011 import sensor_msgs.msg
00012 import genpy
00013 import std_msgs.msg
00014
00015 class GraspHandPostureExecutionAction(genpy.Message):
00016 _md5sum = "ce7f0e50f3ee13e183420cd4d5559794"
00017 _type = "object_manipulation_msgs/GraspHandPostureExecutionAction"
00018 _has_header = False
00019 _full_text = """# ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00020
00021 GraspHandPostureExecutionActionGoal action_goal
00022 GraspHandPostureExecutionActionResult action_result
00023 GraspHandPostureExecutionActionFeedback action_feedback
00024
00025 ================================================================================
00026 MSG: object_manipulation_msgs/GraspHandPostureExecutionActionGoal
00027 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00028
00029 Header header
00030 actionlib_msgs/GoalID goal_id
00031 GraspHandPostureExecutionGoal goal
00032
00033 ================================================================================
00034 MSG: std_msgs/Header
00035 # Standard metadata for higher-level stamped data types.
00036 # This is generally used to communicate timestamped data
00037 # in a particular coordinate frame.
00038 #
00039 # sequence ID: consecutively increasing ID
00040 uint32 seq
00041 #Two-integer timestamp that is expressed as:
00042 # * stamp.secs: seconds (stamp_secs) since epoch
00043 # * stamp.nsecs: nanoseconds since stamp_secs
00044 # time-handling sugar is provided by the client library
00045 time stamp
00046 #Frame this data is associated with
00047 # 0: no frame
00048 # 1: global frame
00049 string frame_id
00050
00051 ================================================================================
00052 MSG: actionlib_msgs/GoalID
00053 # The stamp should store the time at which this goal was requested.
00054 # It is used by an action server when it tries to preempt all
00055 # goals that were requested before a certain time
00056 time stamp
00057
00058 # The id provides a way to associate feedback and
00059 # result message with specific goal requests. The id
00060 # specified must be unique.
00061 string id
00062
00063
00064 ================================================================================
00065 MSG: object_manipulation_msgs/GraspHandPostureExecutionGoal
00066 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00067 # an action for requesting the finger posture part of grasp be physically carried out by a hand
00068 # the name of the arm being used is not in here, as this will be sent to a specific action server
00069 # for each arm
00070
00071 # the grasp to be executed
00072 manipulation_msgs/Grasp grasp
00073
00074 # the goal of this action
00075 # requests that the hand be set in the pre-grasp posture
00076 int32 PRE_GRASP=1
00077 # requests that the hand execute the actual grasp
00078 int32 GRASP=2
00079 # requests that the hand open to release the object
00080 int32 RELEASE=3
00081 int32 goal
00082
00083 # the max contact force to use (<=0 if no desired max)
00084 float32 max_contact_force
00085
00086
00087 ================================================================================
00088 MSG: manipulation_msgs/Grasp
00089 # A name for this grasp
00090 string id
00091
00092 # The internal posture of the hand for the pre-grasp
00093 # only positions are used
00094 sensor_msgs/JointState pre_grasp_posture
00095
00096 # The internal posture of the hand for the grasp
00097 # positions and efforts are used
00098 sensor_msgs/JointState grasp_posture
00099
00100 # The position of the end-effector for the grasp relative to a reference frame
00101 # (that is always specified elsewhere, not in this message)
00102 geometry_msgs/PoseStamped grasp_pose
00103
00104 # The estimated probability of success for this grasp, or some other
00105 # measure of how "good" it is.
00106 float64 grasp_quality
00107
00108 # The approach motion
00109 GripperTranslation approach
00110
00111 # The retreat motion
00112 GripperTranslation retreat
00113
00114 # the maximum contact force to use while grasping (<=0 to disable)
00115 float32 max_contact_force
00116
00117 # an optional list of obstacles that we have semantic information about
00118 # and that can be touched/pushed/moved in the course of grasping
00119 string[] allowed_touch_objects
00120
00121 ================================================================================
00122 MSG: sensor_msgs/JointState
00123 # This is a message that holds data to describe the state of a set of torque controlled joints.
00124 #
00125 # The state of each joint (revolute or prismatic) is defined by:
00126 # * the position of the joint (rad or m),
00127 # * the velocity of the joint (rad/s or m/s) and
00128 # * the effort that is applied in the joint (Nm or N).
00129 #
00130 # Each joint is uniquely identified by its name
00131 # The header specifies the time at which the joint states were recorded. All the joint states
00132 # in one message have to be recorded at the same time.
00133 #
00134 # This message consists of a multiple arrays, one for each part of the joint state.
00135 # The goal is to make each of the fields optional. When e.g. your joints have no
00136 # effort associated with them, you can leave the effort array empty.
00137 #
00138 # All arrays in this message should have the same size, or be empty.
00139 # This is the only way to uniquely associate the joint name with the correct
00140 # states.
00141
00142
00143 Header header
00144
00145 string[] name
00146 float64[] position
00147 float64[] velocity
00148 float64[] effort
00149
00150 ================================================================================
00151 MSG: geometry_msgs/PoseStamped
00152 # A Pose with reference coordinate frame and timestamp
00153 Header header
00154 Pose pose
00155
00156 ================================================================================
00157 MSG: geometry_msgs/Pose
00158 # A representation of pose in free space, composed of postion and orientation.
00159 Point position
00160 Quaternion orientation
00161
00162 ================================================================================
00163 MSG: geometry_msgs/Point
00164 # This contains the position of a point in free space
00165 float64 x
00166 float64 y
00167 float64 z
00168
00169 ================================================================================
00170 MSG: geometry_msgs/Quaternion
00171 # This represents an orientation in free space in quaternion form.
00172
00173 float64 x
00174 float64 y
00175 float64 z
00176 float64 w
00177
00178 ================================================================================
00179 MSG: manipulation_msgs/GripperTranslation
00180 # defines a translation for the gripper, used in pickup or place tasks
00181 # for example for lifting an object off a table or approaching the table for placing
00182
00183 # the direction of the translation
00184 geometry_msgs/Vector3Stamped direction
00185
00186 # the desired translation distance
00187 float32 desired_distance
00188
00189 # the min distance that must be considered feasible before the
00190 # grasp is even attempted
00191 float32 min_distance
00192
00193 ================================================================================
00194 MSG: geometry_msgs/Vector3Stamped
00195 # This represents a Vector3 with reference coordinate frame and timestamp
00196 Header header
00197 Vector3 vector
00198
00199 ================================================================================
00200 MSG: geometry_msgs/Vector3
00201 # This represents a vector in free space.
00202
00203 float64 x
00204 float64 y
00205 float64 z
00206 ================================================================================
00207 MSG: object_manipulation_msgs/GraspHandPostureExecutionActionResult
00208 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00209
00210 Header header
00211 actionlib_msgs/GoalStatus status
00212 GraspHandPostureExecutionResult result
00213
00214 ================================================================================
00215 MSG: actionlib_msgs/GoalStatus
00216 GoalID goal_id
00217 uint8 status
00218 uint8 PENDING = 0 # The goal has yet to be processed by the action server
00219 uint8 ACTIVE = 1 # The goal is currently being processed by the action server
00220 uint8 PREEMPTED = 2 # The goal received a cancel request after it started executing
00221 # and has since completed its execution (Terminal State)
00222 uint8 SUCCEEDED = 3 # The goal was achieved successfully by the action server (Terminal State)
00223 uint8 ABORTED = 4 # The goal was aborted during execution by the action server due
00224 # to some failure (Terminal State)
00225 uint8 REJECTED = 5 # The goal was rejected by the action server without being processed,
00226 # because the goal was unattainable or invalid (Terminal State)
00227 uint8 PREEMPTING = 6 # The goal received a cancel request after it started executing
00228 # and has not yet completed execution
00229 uint8 RECALLING = 7 # The goal received a cancel request before it started executing,
00230 # but the action server has not yet confirmed that the goal is canceled
00231 uint8 RECALLED = 8 # The goal received a cancel request before it started executing
00232 # and was successfully cancelled (Terminal State)
00233 uint8 LOST = 9 # An action client can determine that a goal is LOST. This should not be
00234 # sent over the wire by an action server
00235
00236 #Allow for the user to associate a string with GoalStatus for debugging
00237 string text
00238
00239
00240 ================================================================================
00241 MSG: object_manipulation_msgs/GraspHandPostureExecutionResult
00242 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00243 # the result of the action
00244 ManipulationResult result
00245
00246
00247 ================================================================================
00248 MSG: object_manipulation_msgs/ManipulationResult
00249 # Result codes for manipulation tasks
00250
00251 # task completed as expected
00252 # generally means you can proceed as planned
00253 int32 SUCCESS = 1
00254
00255 # task not possible (e.g. out of reach or obstacles in the way)
00256 # generally means that the world was not disturbed, so you can try another task
00257 int32 UNFEASIBLE = -1
00258
00259 # task was thought possible, but failed due to unexpected events during execution
00260 # it is likely that the world was disturbed, so you are encouraged to refresh
00261 # your sensed world model before proceeding to another task
00262 int32 FAILED = -2
00263
00264 # a lower level error prevented task completion (e.g. joint controller not responding)
00265 # generally requires human attention
00266 int32 ERROR = -3
00267
00268 # means that at some point during execution we ended up in a state that the collision-aware
00269 # arm navigation module will not move out of. The world was likely not disturbed, but you
00270 # probably need a new collision map to move the arm out of the stuck position
00271 int32 ARM_MOVEMENT_PREVENTED = -4
00272
00273 # specific to grasp actions
00274 # the object was grasped successfully, but the lift attempt could not achieve the minimum lift distance requested
00275 # it is likely that the collision environment will see collisions between the hand/object and the support surface
00276 int32 LIFT_FAILED = -5
00277
00278 # specific to place actions
00279 # the object was placed successfully, but the retreat attempt could not achieve the minimum retreat distance requested
00280 # it is likely that the collision environment will see collisions between the hand and the object
00281 int32 RETREAT_FAILED = -6
00282
00283 # indicates that somewhere along the line a human said "wait, stop, this is bad, go back and do something else"
00284 int32 CANCELLED = -7
00285
00286 # the actual value of this error code
00287 int32 value
00288
00289 ================================================================================
00290 MSG: object_manipulation_msgs/GraspHandPostureExecutionActionFeedback
00291 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00292
00293 Header header
00294 actionlib_msgs/GoalStatus status
00295 GraspHandPostureExecutionFeedback feedback
00296
00297 ================================================================================
00298 MSG: object_manipulation_msgs/GraspHandPostureExecutionFeedback
00299 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00300 # empty for now
00301
00302
00303
00304 """
00305 __slots__ = ['action_goal','action_result','action_feedback']
00306 _slot_types = ['object_manipulation_msgs/GraspHandPostureExecutionActionGoal','object_manipulation_msgs/GraspHandPostureExecutionActionResult','object_manipulation_msgs/GraspHandPostureExecutionActionFeedback']
00307
00308 def __init__(self, *args, **kwds):
00309 """
00310 Constructor. Any message fields that are implicitly/explicitly
00311 set to None will be assigned a default value. The recommend
00312 use is keyword arguments as this is more robust to future message
00313 changes. You cannot mix in-order arguments and keyword arguments.
00314
00315 The available fields are:
00316 action_goal,action_result,action_feedback
00317
00318 :param args: complete set of field values, in .msg order
00319 :param kwds: use keyword arguments corresponding to message field names
00320 to set specific fields.
00321 """
00322 if args or kwds:
00323 super(GraspHandPostureExecutionAction, self).__init__(*args, **kwds)
00324
00325 if self.action_goal is None:
00326 self.action_goal = object_manipulation_msgs.msg.GraspHandPostureExecutionActionGoal()
00327 if self.action_result is None:
00328 self.action_result = object_manipulation_msgs.msg.GraspHandPostureExecutionActionResult()
00329 if self.action_feedback is None:
00330 self.action_feedback = object_manipulation_msgs.msg.GraspHandPostureExecutionActionFeedback()
00331 else:
00332 self.action_goal = object_manipulation_msgs.msg.GraspHandPostureExecutionActionGoal()
00333 self.action_result = object_manipulation_msgs.msg.GraspHandPostureExecutionActionResult()
00334 self.action_feedback = object_manipulation_msgs.msg.GraspHandPostureExecutionActionFeedback()
00335
00336 def _get_types(self):
00337 """
00338 internal API method
00339 """
00340 return self._slot_types
00341
00342 def serialize(self, buff):
00343 """
00344 serialize message into buffer
00345 :param buff: buffer, ``StringIO``
00346 """
00347 try:
00348 _x = self
00349 buff.write(_struct_3I.pack(_x.action_goal.header.seq, _x.action_goal.header.stamp.secs, _x.action_goal.header.stamp.nsecs))
00350 _x = self.action_goal.header.frame_id
00351 length = len(_x)
00352 if python3 or type(_x) == unicode:
00353 _x = _x.encode('utf-8')
00354 length = len(_x)
00355 buff.write(struct.pack('<I%ss'%length, length, _x))
00356 _x = self
00357 buff.write(_struct_2I.pack(_x.action_goal.goal_id.stamp.secs, _x.action_goal.goal_id.stamp.nsecs))
00358 _x = self.action_goal.goal_id.id
00359 length = len(_x)
00360 if python3 or type(_x) == unicode:
00361 _x = _x.encode('utf-8')
00362 length = len(_x)
00363 buff.write(struct.pack('<I%ss'%length, length, _x))
00364 _x = self.action_goal.goal.grasp.id
00365 length = len(_x)
00366 if python3 or type(_x) == unicode:
00367 _x = _x.encode('utf-8')
00368 length = len(_x)
00369 buff.write(struct.pack('<I%ss'%length, length, _x))
00370 _x = self
00371 buff.write(_struct_3I.pack(_x.action_goal.goal.grasp.pre_grasp_posture.header.seq, _x.action_goal.goal.grasp.pre_grasp_posture.header.stamp.secs, _x.action_goal.goal.grasp.pre_grasp_posture.header.stamp.nsecs))
00372 _x = self.action_goal.goal.grasp.pre_grasp_posture.header.frame_id
00373 length = len(_x)
00374 if python3 or type(_x) == unicode:
00375 _x = _x.encode('utf-8')
00376 length = len(_x)
00377 buff.write(struct.pack('<I%ss'%length, length, _x))
00378 length = len(self.action_goal.goal.grasp.pre_grasp_posture.name)
00379 buff.write(_struct_I.pack(length))
00380 for val1 in self.action_goal.goal.grasp.pre_grasp_posture.name:
00381 length = len(val1)
00382 if python3 or type(val1) == unicode:
00383 val1 = val1.encode('utf-8')
00384 length = len(val1)
00385 buff.write(struct.pack('<I%ss'%length, length, val1))
00386 length = len(self.action_goal.goal.grasp.pre_grasp_posture.position)
00387 buff.write(_struct_I.pack(length))
00388 pattern = '<%sd'%length
00389 buff.write(struct.pack(pattern, *self.action_goal.goal.grasp.pre_grasp_posture.position))
00390 length = len(self.action_goal.goal.grasp.pre_grasp_posture.velocity)
00391 buff.write(_struct_I.pack(length))
00392 pattern = '<%sd'%length
00393 buff.write(struct.pack(pattern, *self.action_goal.goal.grasp.pre_grasp_posture.velocity))
00394 length = len(self.action_goal.goal.grasp.pre_grasp_posture.effort)
00395 buff.write(_struct_I.pack(length))
00396 pattern = '<%sd'%length
00397 buff.write(struct.pack(pattern, *self.action_goal.goal.grasp.pre_grasp_posture.effort))
00398 _x = self
00399 buff.write(_struct_3I.pack(_x.action_goal.goal.grasp.grasp_posture.header.seq, _x.action_goal.goal.grasp.grasp_posture.header.stamp.secs, _x.action_goal.goal.grasp.grasp_posture.header.stamp.nsecs))
00400 _x = self.action_goal.goal.grasp.grasp_posture.header.frame_id
00401 length = len(_x)
00402 if python3 or type(_x) == unicode:
00403 _x = _x.encode('utf-8')
00404 length = len(_x)
00405 buff.write(struct.pack('<I%ss'%length, length, _x))
00406 length = len(self.action_goal.goal.grasp.grasp_posture.name)
00407 buff.write(_struct_I.pack(length))
00408 for val1 in self.action_goal.goal.grasp.grasp_posture.name:
00409 length = len(val1)
00410 if python3 or type(val1) == unicode:
00411 val1 = val1.encode('utf-8')
00412 length = len(val1)
00413 buff.write(struct.pack('<I%ss'%length, length, val1))
00414 length = len(self.action_goal.goal.grasp.grasp_posture.position)
00415 buff.write(_struct_I.pack(length))
00416 pattern = '<%sd'%length
00417 buff.write(struct.pack(pattern, *self.action_goal.goal.grasp.grasp_posture.position))
00418 length = len(self.action_goal.goal.grasp.grasp_posture.velocity)
00419 buff.write(_struct_I.pack(length))
00420 pattern = '<%sd'%length
00421 buff.write(struct.pack(pattern, *self.action_goal.goal.grasp.grasp_posture.velocity))
00422 length = len(self.action_goal.goal.grasp.grasp_posture.effort)
00423 buff.write(_struct_I.pack(length))
00424 pattern = '<%sd'%length
00425 buff.write(struct.pack(pattern, *self.action_goal.goal.grasp.grasp_posture.effort))
00426 _x = self
00427 buff.write(_struct_3I.pack(_x.action_goal.goal.grasp.grasp_pose.header.seq, _x.action_goal.goal.grasp.grasp_pose.header.stamp.secs, _x.action_goal.goal.grasp.grasp_pose.header.stamp.nsecs))
00428 _x = self.action_goal.goal.grasp.grasp_pose.header.frame_id
00429 length = len(_x)
00430 if python3 or type(_x) == unicode:
00431 _x = _x.encode('utf-8')
00432 length = len(_x)
00433 buff.write(struct.pack('<I%ss'%length, length, _x))
00434 _x = self
00435 buff.write(_struct_8d3I.pack(_x.action_goal.goal.grasp.grasp_pose.pose.position.x, _x.action_goal.goal.grasp.grasp_pose.pose.position.y, _x.action_goal.goal.grasp.grasp_pose.pose.position.z, _x.action_goal.goal.grasp.grasp_pose.pose.orientation.x, _x.action_goal.goal.grasp.grasp_pose.pose.orientation.y, _x.action_goal.goal.grasp.grasp_pose.pose.orientation.z, _x.action_goal.goal.grasp.grasp_pose.pose.orientation.w, _x.action_goal.goal.grasp.grasp_quality, _x.action_goal.goal.grasp.approach.direction.header.seq, _x.action_goal.goal.grasp.approach.direction.header.stamp.secs, _x.action_goal.goal.grasp.approach.direction.header.stamp.nsecs))
00436 _x = self.action_goal.goal.grasp.approach.direction.header.frame_id
00437 length = len(_x)
00438 if python3 or type(_x) == unicode:
00439 _x = _x.encode('utf-8')
00440 length = len(_x)
00441 buff.write(struct.pack('<I%ss'%length, length, _x))
00442 _x = self
00443 buff.write(_struct_3d2f3I.pack(_x.action_goal.goal.grasp.approach.direction.vector.x, _x.action_goal.goal.grasp.approach.direction.vector.y, _x.action_goal.goal.grasp.approach.direction.vector.z, _x.action_goal.goal.grasp.approach.desired_distance, _x.action_goal.goal.grasp.approach.min_distance, _x.action_goal.goal.grasp.retreat.direction.header.seq, _x.action_goal.goal.grasp.retreat.direction.header.stamp.secs, _x.action_goal.goal.grasp.retreat.direction.header.stamp.nsecs))
00444 _x = self.action_goal.goal.grasp.retreat.direction.header.frame_id
00445 length = len(_x)
00446 if python3 or type(_x) == unicode:
00447 _x = _x.encode('utf-8')
00448 length = len(_x)
00449 buff.write(struct.pack('<I%ss'%length, length, _x))
00450 _x = self
00451 buff.write(_struct_3d3f.pack(_x.action_goal.goal.grasp.retreat.direction.vector.x, _x.action_goal.goal.grasp.retreat.direction.vector.y, _x.action_goal.goal.grasp.retreat.direction.vector.z, _x.action_goal.goal.grasp.retreat.desired_distance, _x.action_goal.goal.grasp.retreat.min_distance, _x.action_goal.goal.grasp.max_contact_force))
00452 length = len(self.action_goal.goal.grasp.allowed_touch_objects)
00453 buff.write(_struct_I.pack(length))
00454 for val1 in self.action_goal.goal.grasp.allowed_touch_objects:
00455 length = len(val1)
00456 if python3 or type(val1) == unicode:
00457 val1 = val1.encode('utf-8')
00458 length = len(val1)
00459 buff.write(struct.pack('<I%ss'%length, length, val1))
00460 _x = self
00461 buff.write(_struct_if3I.pack(_x.action_goal.goal.goal, _x.action_goal.goal.max_contact_force, _x.action_result.header.seq, _x.action_result.header.stamp.secs, _x.action_result.header.stamp.nsecs))
00462 _x = self.action_result.header.frame_id
00463 length = len(_x)
00464 if python3 or type(_x) == unicode:
00465 _x = _x.encode('utf-8')
00466 length = len(_x)
00467 buff.write(struct.pack('<I%ss'%length, length, _x))
00468 _x = self
00469 buff.write(_struct_2I.pack(_x.action_result.status.goal_id.stamp.secs, _x.action_result.status.goal_id.stamp.nsecs))
00470 _x = self.action_result.status.goal_id.id
00471 length = len(_x)
00472 if python3 or type(_x) == unicode:
00473 _x = _x.encode('utf-8')
00474 length = len(_x)
00475 buff.write(struct.pack('<I%ss'%length, length, _x))
00476 buff.write(_struct_B.pack(self.action_result.status.status))
00477 _x = self.action_result.status.text
00478 length = len(_x)
00479 if python3 or type(_x) == unicode:
00480 _x = _x.encode('utf-8')
00481 length = len(_x)
00482 buff.write(struct.pack('<I%ss'%length, length, _x))
00483 _x = self
00484 buff.write(_struct_i3I.pack(_x.action_result.result.result.value, _x.action_feedback.header.seq, _x.action_feedback.header.stamp.secs, _x.action_feedback.header.stamp.nsecs))
00485 _x = self.action_feedback.header.frame_id
00486 length = len(_x)
00487 if python3 or type(_x) == unicode:
00488 _x = _x.encode('utf-8')
00489 length = len(_x)
00490 buff.write(struct.pack('<I%ss'%length, length, _x))
00491 _x = self
00492 buff.write(_struct_2I.pack(_x.action_feedback.status.goal_id.stamp.secs, _x.action_feedback.status.goal_id.stamp.nsecs))
00493 _x = self.action_feedback.status.goal_id.id
00494 length = len(_x)
00495 if python3 or type(_x) == unicode:
00496 _x = _x.encode('utf-8')
00497 length = len(_x)
00498 buff.write(struct.pack('<I%ss'%length, length, _x))
00499 buff.write(_struct_B.pack(self.action_feedback.status.status))
00500 _x = self.action_feedback.status.text
00501 length = len(_x)
00502 if python3 or type(_x) == unicode:
00503 _x = _x.encode('utf-8')
00504 length = len(_x)
00505 buff.write(struct.pack('<I%ss'%length, length, _x))
00506 except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
00507 except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
00508
00509 def deserialize(self, str):
00510 """
00511 unpack serialized message in str into this message instance
00512 :param str: byte array of serialized message, ``str``
00513 """
00514 try:
00515 if self.action_goal is None:
00516 self.action_goal = object_manipulation_msgs.msg.GraspHandPostureExecutionActionGoal()
00517 if self.action_result is None:
00518 self.action_result = object_manipulation_msgs.msg.GraspHandPostureExecutionActionResult()
00519 if self.action_feedback is None:
00520 self.action_feedback = object_manipulation_msgs.msg.GraspHandPostureExecutionActionFeedback()
00521 end = 0
00522 _x = self
00523 start = end
00524 end += 12
00525 (_x.action_goal.header.seq, _x.action_goal.header.stamp.secs, _x.action_goal.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00526 start = end
00527 end += 4
00528 (length,) = _struct_I.unpack(str[start:end])
00529 start = end
00530 end += length
00531 if python3:
00532 self.action_goal.header.frame_id = str[start:end].decode('utf-8')
00533 else:
00534 self.action_goal.header.frame_id = str[start:end]
00535 _x = self
00536 start = end
00537 end += 8
00538 (_x.action_goal.goal_id.stamp.secs, _x.action_goal.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
00539 start = end
00540 end += 4
00541 (length,) = _struct_I.unpack(str[start:end])
00542 start = end
00543 end += length
00544 if python3:
00545 self.action_goal.goal_id.id = str[start:end].decode('utf-8')
00546 else:
00547 self.action_goal.goal_id.id = str[start:end]
00548 start = end
00549 end += 4
00550 (length,) = _struct_I.unpack(str[start:end])
00551 start = end
00552 end += length
00553 if python3:
00554 self.action_goal.goal.grasp.id = str[start:end].decode('utf-8')
00555 else:
00556 self.action_goal.goal.grasp.id = str[start:end]
00557 _x = self
00558 start = end
00559 end += 12
00560 (_x.action_goal.goal.grasp.pre_grasp_posture.header.seq, _x.action_goal.goal.grasp.pre_grasp_posture.header.stamp.secs, _x.action_goal.goal.grasp.pre_grasp_posture.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00561 start = end
00562 end += 4
00563 (length,) = _struct_I.unpack(str[start:end])
00564 start = end
00565 end += length
00566 if python3:
00567 self.action_goal.goal.grasp.pre_grasp_posture.header.frame_id = str[start:end].decode('utf-8')
00568 else:
00569 self.action_goal.goal.grasp.pre_grasp_posture.header.frame_id = str[start:end]
00570 start = end
00571 end += 4
00572 (length,) = _struct_I.unpack(str[start:end])
00573 self.action_goal.goal.grasp.pre_grasp_posture.name = []
00574 for i in range(0, length):
00575 start = end
00576 end += 4
00577 (length,) = _struct_I.unpack(str[start:end])
00578 start = end
00579 end += length
00580 if python3:
00581 val1 = str[start:end].decode('utf-8')
00582 else:
00583 val1 = str[start:end]
00584 self.action_goal.goal.grasp.pre_grasp_posture.name.append(val1)
00585 start = end
00586 end += 4
00587 (length,) = _struct_I.unpack(str[start:end])
00588 pattern = '<%sd'%length
00589 start = end
00590 end += struct.calcsize(pattern)
00591 self.action_goal.goal.grasp.pre_grasp_posture.position = struct.unpack(pattern, str[start:end])
00592 start = end
00593 end += 4
00594 (length,) = _struct_I.unpack(str[start:end])
00595 pattern = '<%sd'%length
00596 start = end
00597 end += struct.calcsize(pattern)
00598 self.action_goal.goal.grasp.pre_grasp_posture.velocity = struct.unpack(pattern, str[start:end])
00599 start = end
00600 end += 4
00601 (length,) = _struct_I.unpack(str[start:end])
00602 pattern = '<%sd'%length
00603 start = end
00604 end += struct.calcsize(pattern)
00605 self.action_goal.goal.grasp.pre_grasp_posture.effort = struct.unpack(pattern, str[start:end])
00606 _x = self
00607 start = end
00608 end += 12
00609 (_x.action_goal.goal.grasp.grasp_posture.header.seq, _x.action_goal.goal.grasp.grasp_posture.header.stamp.secs, _x.action_goal.goal.grasp.grasp_posture.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00610 start = end
00611 end += 4
00612 (length,) = _struct_I.unpack(str[start:end])
00613 start = end
00614 end += length
00615 if python3:
00616 self.action_goal.goal.grasp.grasp_posture.header.frame_id = str[start:end].decode('utf-8')
00617 else:
00618 self.action_goal.goal.grasp.grasp_posture.header.frame_id = str[start:end]
00619 start = end
00620 end += 4
00621 (length,) = _struct_I.unpack(str[start:end])
00622 self.action_goal.goal.grasp.grasp_posture.name = []
00623 for i in range(0, length):
00624 start = end
00625 end += 4
00626 (length,) = _struct_I.unpack(str[start:end])
00627 start = end
00628 end += length
00629 if python3:
00630 val1 = str[start:end].decode('utf-8')
00631 else:
00632 val1 = str[start:end]
00633 self.action_goal.goal.grasp.grasp_posture.name.append(val1)
00634 start = end
00635 end += 4
00636 (length,) = _struct_I.unpack(str[start:end])
00637 pattern = '<%sd'%length
00638 start = end
00639 end += struct.calcsize(pattern)
00640 self.action_goal.goal.grasp.grasp_posture.position = struct.unpack(pattern, str[start:end])
00641 start = end
00642 end += 4
00643 (length,) = _struct_I.unpack(str[start:end])
00644 pattern = '<%sd'%length
00645 start = end
00646 end += struct.calcsize(pattern)
00647 self.action_goal.goal.grasp.grasp_posture.velocity = struct.unpack(pattern, str[start:end])
00648 start = end
00649 end += 4
00650 (length,) = _struct_I.unpack(str[start:end])
00651 pattern = '<%sd'%length
00652 start = end
00653 end += struct.calcsize(pattern)
00654 self.action_goal.goal.grasp.grasp_posture.effort = struct.unpack(pattern, str[start:end])
00655 _x = self
00656 start = end
00657 end += 12
00658 (_x.action_goal.goal.grasp.grasp_pose.header.seq, _x.action_goal.goal.grasp.grasp_pose.header.stamp.secs, _x.action_goal.goal.grasp.grasp_pose.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00659 start = end
00660 end += 4
00661 (length,) = _struct_I.unpack(str[start:end])
00662 start = end
00663 end += length
00664 if python3:
00665 self.action_goal.goal.grasp.grasp_pose.header.frame_id = str[start:end].decode('utf-8')
00666 else:
00667 self.action_goal.goal.grasp.grasp_pose.header.frame_id = str[start:end]
00668 _x = self
00669 start = end
00670 end += 76
00671 (_x.action_goal.goal.grasp.grasp_pose.pose.position.x, _x.action_goal.goal.grasp.grasp_pose.pose.position.y, _x.action_goal.goal.grasp.grasp_pose.pose.position.z, _x.action_goal.goal.grasp.grasp_pose.pose.orientation.x, _x.action_goal.goal.grasp.grasp_pose.pose.orientation.y, _x.action_goal.goal.grasp.grasp_pose.pose.orientation.z, _x.action_goal.goal.grasp.grasp_pose.pose.orientation.w, _x.action_goal.goal.grasp.grasp_quality, _x.action_goal.goal.grasp.approach.direction.header.seq, _x.action_goal.goal.grasp.approach.direction.header.stamp.secs, _x.action_goal.goal.grasp.approach.direction.header.stamp.nsecs,) = _struct_8d3I.unpack(str[start:end])
00672 start = end
00673 end += 4
00674 (length,) = _struct_I.unpack(str[start:end])
00675 start = end
00676 end += length
00677 if python3:
00678 self.action_goal.goal.grasp.approach.direction.header.frame_id = str[start:end].decode('utf-8')
00679 else:
00680 self.action_goal.goal.grasp.approach.direction.header.frame_id = str[start:end]
00681 _x = self
00682 start = end
00683 end += 44
00684 (_x.action_goal.goal.grasp.approach.direction.vector.x, _x.action_goal.goal.grasp.approach.direction.vector.y, _x.action_goal.goal.grasp.approach.direction.vector.z, _x.action_goal.goal.grasp.approach.desired_distance, _x.action_goal.goal.grasp.approach.min_distance, _x.action_goal.goal.grasp.retreat.direction.header.seq, _x.action_goal.goal.grasp.retreat.direction.header.stamp.secs, _x.action_goal.goal.grasp.retreat.direction.header.stamp.nsecs,) = _struct_3d2f3I.unpack(str[start:end])
00685 start = end
00686 end += 4
00687 (length,) = _struct_I.unpack(str[start:end])
00688 start = end
00689 end += length
00690 if python3:
00691 self.action_goal.goal.grasp.retreat.direction.header.frame_id = str[start:end].decode('utf-8')
00692 else:
00693 self.action_goal.goal.grasp.retreat.direction.header.frame_id = str[start:end]
00694 _x = self
00695 start = end
00696 end += 36
00697 (_x.action_goal.goal.grasp.retreat.direction.vector.x, _x.action_goal.goal.grasp.retreat.direction.vector.y, _x.action_goal.goal.grasp.retreat.direction.vector.z, _x.action_goal.goal.grasp.retreat.desired_distance, _x.action_goal.goal.grasp.retreat.min_distance, _x.action_goal.goal.grasp.max_contact_force,) = _struct_3d3f.unpack(str[start:end])
00698 start = end
00699 end += 4
00700 (length,) = _struct_I.unpack(str[start:end])
00701 self.action_goal.goal.grasp.allowed_touch_objects = []
00702 for i in range(0, length):
00703 start = end
00704 end += 4
00705 (length,) = _struct_I.unpack(str[start:end])
00706 start = end
00707 end += length
00708 if python3:
00709 val1 = str[start:end].decode('utf-8')
00710 else:
00711 val1 = str[start:end]
00712 self.action_goal.goal.grasp.allowed_touch_objects.append(val1)
00713 _x = self
00714 start = end
00715 end += 20
00716 (_x.action_goal.goal.goal, _x.action_goal.goal.max_contact_force, _x.action_result.header.seq, _x.action_result.header.stamp.secs, _x.action_result.header.stamp.nsecs,) = _struct_if3I.unpack(str[start:end])
00717 start = end
00718 end += 4
00719 (length,) = _struct_I.unpack(str[start:end])
00720 start = end
00721 end += length
00722 if python3:
00723 self.action_result.header.frame_id = str[start:end].decode('utf-8')
00724 else:
00725 self.action_result.header.frame_id = str[start:end]
00726 _x = self
00727 start = end
00728 end += 8
00729 (_x.action_result.status.goal_id.stamp.secs, _x.action_result.status.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
00730 start = end
00731 end += 4
00732 (length,) = _struct_I.unpack(str[start:end])
00733 start = end
00734 end += length
00735 if python3:
00736 self.action_result.status.goal_id.id = str[start:end].decode('utf-8')
00737 else:
00738 self.action_result.status.goal_id.id = str[start:end]
00739 start = end
00740 end += 1
00741 (self.action_result.status.status,) = _struct_B.unpack(str[start:end])
00742 start = end
00743 end += 4
00744 (length,) = _struct_I.unpack(str[start:end])
00745 start = end
00746 end += length
00747 if python3:
00748 self.action_result.status.text = str[start:end].decode('utf-8')
00749 else:
00750 self.action_result.status.text = str[start:end]
00751 _x = self
00752 start = end
00753 end += 16
00754 (_x.action_result.result.result.value, _x.action_feedback.header.seq, _x.action_feedback.header.stamp.secs, _x.action_feedback.header.stamp.nsecs,) = _struct_i3I.unpack(str[start:end])
00755 start = end
00756 end += 4
00757 (length,) = _struct_I.unpack(str[start:end])
00758 start = end
00759 end += length
00760 if python3:
00761 self.action_feedback.header.frame_id = str[start:end].decode('utf-8')
00762 else:
00763 self.action_feedback.header.frame_id = str[start:end]
00764 _x = self
00765 start = end
00766 end += 8
00767 (_x.action_feedback.status.goal_id.stamp.secs, _x.action_feedback.status.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
00768 start = end
00769 end += 4
00770 (length,) = _struct_I.unpack(str[start:end])
00771 start = end
00772 end += length
00773 if python3:
00774 self.action_feedback.status.goal_id.id = str[start:end].decode('utf-8')
00775 else:
00776 self.action_feedback.status.goal_id.id = str[start:end]
00777 start = end
00778 end += 1
00779 (self.action_feedback.status.status,) = _struct_B.unpack(str[start:end])
00780 start = end
00781 end += 4
00782 (length,) = _struct_I.unpack(str[start:end])
00783 start = end
00784 end += length
00785 if python3:
00786 self.action_feedback.status.text = str[start:end].decode('utf-8')
00787 else:
00788 self.action_feedback.status.text = str[start:end]
00789 return self
00790 except struct.error as e:
00791 raise genpy.DeserializationError(e)
00792
00793
00794 def serialize_numpy(self, buff, numpy):
00795 """
00796 serialize message with numpy array types into buffer
00797 :param buff: buffer, ``StringIO``
00798 :param numpy: numpy python module
00799 """
00800 try:
00801 _x = self
00802 buff.write(_struct_3I.pack(_x.action_goal.header.seq, _x.action_goal.header.stamp.secs, _x.action_goal.header.stamp.nsecs))
00803 _x = self.action_goal.header.frame_id
00804 length = len(_x)
00805 if python3 or type(_x) == unicode:
00806 _x = _x.encode('utf-8')
00807 length = len(_x)
00808 buff.write(struct.pack('<I%ss'%length, length, _x))
00809 _x = self
00810 buff.write(_struct_2I.pack(_x.action_goal.goal_id.stamp.secs, _x.action_goal.goal_id.stamp.nsecs))
00811 _x = self.action_goal.goal_id.id
00812 length = len(_x)
00813 if python3 or type(_x) == unicode:
00814 _x = _x.encode('utf-8')
00815 length = len(_x)
00816 buff.write(struct.pack('<I%ss'%length, length, _x))
00817 _x = self.action_goal.goal.grasp.id
00818 length = len(_x)
00819 if python3 or type(_x) == unicode:
00820 _x = _x.encode('utf-8')
00821 length = len(_x)
00822 buff.write(struct.pack('<I%ss'%length, length, _x))
00823 _x = self
00824 buff.write(_struct_3I.pack(_x.action_goal.goal.grasp.pre_grasp_posture.header.seq, _x.action_goal.goal.grasp.pre_grasp_posture.header.stamp.secs, _x.action_goal.goal.grasp.pre_grasp_posture.header.stamp.nsecs))
00825 _x = self.action_goal.goal.grasp.pre_grasp_posture.header.frame_id
00826 length = len(_x)
00827 if python3 or type(_x) == unicode:
00828 _x = _x.encode('utf-8')
00829 length = len(_x)
00830 buff.write(struct.pack('<I%ss'%length, length, _x))
00831 length = len(self.action_goal.goal.grasp.pre_grasp_posture.name)
00832 buff.write(_struct_I.pack(length))
00833 for val1 in self.action_goal.goal.grasp.pre_grasp_posture.name:
00834 length = len(val1)
00835 if python3 or type(val1) == unicode:
00836 val1 = val1.encode('utf-8')
00837 length = len(val1)
00838 buff.write(struct.pack('<I%ss'%length, length, val1))
00839 length = len(self.action_goal.goal.grasp.pre_grasp_posture.position)
00840 buff.write(_struct_I.pack(length))
00841 pattern = '<%sd'%length
00842 buff.write(self.action_goal.goal.grasp.pre_grasp_posture.position.tostring())
00843 length = len(self.action_goal.goal.grasp.pre_grasp_posture.velocity)
00844 buff.write(_struct_I.pack(length))
00845 pattern = '<%sd'%length
00846 buff.write(self.action_goal.goal.grasp.pre_grasp_posture.velocity.tostring())
00847 length = len(self.action_goal.goal.grasp.pre_grasp_posture.effort)
00848 buff.write(_struct_I.pack(length))
00849 pattern = '<%sd'%length
00850 buff.write(self.action_goal.goal.grasp.pre_grasp_posture.effort.tostring())
00851 _x = self
00852 buff.write(_struct_3I.pack(_x.action_goal.goal.grasp.grasp_posture.header.seq, _x.action_goal.goal.grasp.grasp_posture.header.stamp.secs, _x.action_goal.goal.grasp.grasp_posture.header.stamp.nsecs))
00853 _x = self.action_goal.goal.grasp.grasp_posture.header.frame_id
00854 length = len(_x)
00855 if python3 or type(_x) == unicode:
00856 _x = _x.encode('utf-8')
00857 length = len(_x)
00858 buff.write(struct.pack('<I%ss'%length, length, _x))
00859 length = len(self.action_goal.goal.grasp.grasp_posture.name)
00860 buff.write(_struct_I.pack(length))
00861 for val1 in self.action_goal.goal.grasp.grasp_posture.name:
00862 length = len(val1)
00863 if python3 or type(val1) == unicode:
00864 val1 = val1.encode('utf-8')
00865 length = len(val1)
00866 buff.write(struct.pack('<I%ss'%length, length, val1))
00867 length = len(self.action_goal.goal.grasp.grasp_posture.position)
00868 buff.write(_struct_I.pack(length))
00869 pattern = '<%sd'%length
00870 buff.write(self.action_goal.goal.grasp.grasp_posture.position.tostring())
00871 length = len(self.action_goal.goal.grasp.grasp_posture.velocity)
00872 buff.write(_struct_I.pack(length))
00873 pattern = '<%sd'%length
00874 buff.write(self.action_goal.goal.grasp.grasp_posture.velocity.tostring())
00875 length = len(self.action_goal.goal.grasp.grasp_posture.effort)
00876 buff.write(_struct_I.pack(length))
00877 pattern = '<%sd'%length
00878 buff.write(self.action_goal.goal.grasp.grasp_posture.effort.tostring())
00879 _x = self
00880 buff.write(_struct_3I.pack(_x.action_goal.goal.grasp.grasp_pose.header.seq, _x.action_goal.goal.grasp.grasp_pose.header.stamp.secs, _x.action_goal.goal.grasp.grasp_pose.header.stamp.nsecs))
00881 _x = self.action_goal.goal.grasp.grasp_pose.header.frame_id
00882 length = len(_x)
00883 if python3 or type(_x) == unicode:
00884 _x = _x.encode('utf-8')
00885 length = len(_x)
00886 buff.write(struct.pack('<I%ss'%length, length, _x))
00887 _x = self
00888 buff.write(_struct_8d3I.pack(_x.action_goal.goal.grasp.grasp_pose.pose.position.x, _x.action_goal.goal.grasp.grasp_pose.pose.position.y, _x.action_goal.goal.grasp.grasp_pose.pose.position.z, _x.action_goal.goal.grasp.grasp_pose.pose.orientation.x, _x.action_goal.goal.grasp.grasp_pose.pose.orientation.y, _x.action_goal.goal.grasp.grasp_pose.pose.orientation.z, _x.action_goal.goal.grasp.grasp_pose.pose.orientation.w, _x.action_goal.goal.grasp.grasp_quality, _x.action_goal.goal.grasp.approach.direction.header.seq, _x.action_goal.goal.grasp.approach.direction.header.stamp.secs, _x.action_goal.goal.grasp.approach.direction.header.stamp.nsecs))
00889 _x = self.action_goal.goal.grasp.approach.direction.header.frame_id
00890 length = len(_x)
00891 if python3 or type(_x) == unicode:
00892 _x = _x.encode('utf-8')
00893 length = len(_x)
00894 buff.write(struct.pack('<I%ss'%length, length, _x))
00895 _x = self
00896 buff.write(_struct_3d2f3I.pack(_x.action_goal.goal.grasp.approach.direction.vector.x, _x.action_goal.goal.grasp.approach.direction.vector.y, _x.action_goal.goal.grasp.approach.direction.vector.z, _x.action_goal.goal.grasp.approach.desired_distance, _x.action_goal.goal.grasp.approach.min_distance, _x.action_goal.goal.grasp.retreat.direction.header.seq, _x.action_goal.goal.grasp.retreat.direction.header.stamp.secs, _x.action_goal.goal.grasp.retreat.direction.header.stamp.nsecs))
00897 _x = self.action_goal.goal.grasp.retreat.direction.header.frame_id
00898 length = len(_x)
00899 if python3 or type(_x) == unicode:
00900 _x = _x.encode('utf-8')
00901 length = len(_x)
00902 buff.write(struct.pack('<I%ss'%length, length, _x))
00903 _x = self
00904 buff.write(_struct_3d3f.pack(_x.action_goal.goal.grasp.retreat.direction.vector.x, _x.action_goal.goal.grasp.retreat.direction.vector.y, _x.action_goal.goal.grasp.retreat.direction.vector.z, _x.action_goal.goal.grasp.retreat.desired_distance, _x.action_goal.goal.grasp.retreat.min_distance, _x.action_goal.goal.grasp.max_contact_force))
00905 length = len(self.action_goal.goal.grasp.allowed_touch_objects)
00906 buff.write(_struct_I.pack(length))
00907 for val1 in self.action_goal.goal.grasp.allowed_touch_objects:
00908 length = len(val1)
00909 if python3 or type(val1) == unicode:
00910 val1 = val1.encode('utf-8')
00911 length = len(val1)
00912 buff.write(struct.pack('<I%ss'%length, length, val1))
00913 _x = self
00914 buff.write(_struct_if3I.pack(_x.action_goal.goal.goal, _x.action_goal.goal.max_contact_force, _x.action_result.header.seq, _x.action_result.header.stamp.secs, _x.action_result.header.stamp.nsecs))
00915 _x = self.action_result.header.frame_id
00916 length = len(_x)
00917 if python3 or type(_x) == unicode:
00918 _x = _x.encode('utf-8')
00919 length = len(_x)
00920 buff.write(struct.pack('<I%ss'%length, length, _x))
00921 _x = self
00922 buff.write(_struct_2I.pack(_x.action_result.status.goal_id.stamp.secs, _x.action_result.status.goal_id.stamp.nsecs))
00923 _x = self.action_result.status.goal_id.id
00924 length = len(_x)
00925 if python3 or type(_x) == unicode:
00926 _x = _x.encode('utf-8')
00927 length = len(_x)
00928 buff.write(struct.pack('<I%ss'%length, length, _x))
00929 buff.write(_struct_B.pack(self.action_result.status.status))
00930 _x = self.action_result.status.text
00931 length = len(_x)
00932 if python3 or type(_x) == unicode:
00933 _x = _x.encode('utf-8')
00934 length = len(_x)
00935 buff.write(struct.pack('<I%ss'%length, length, _x))
00936 _x = self
00937 buff.write(_struct_i3I.pack(_x.action_result.result.result.value, _x.action_feedback.header.seq, _x.action_feedback.header.stamp.secs, _x.action_feedback.header.stamp.nsecs))
00938 _x = self.action_feedback.header.frame_id
00939 length = len(_x)
00940 if python3 or type(_x) == unicode:
00941 _x = _x.encode('utf-8')
00942 length = len(_x)
00943 buff.write(struct.pack('<I%ss'%length, length, _x))
00944 _x = self
00945 buff.write(_struct_2I.pack(_x.action_feedback.status.goal_id.stamp.secs, _x.action_feedback.status.goal_id.stamp.nsecs))
00946 _x = self.action_feedback.status.goal_id.id
00947 length = len(_x)
00948 if python3 or type(_x) == unicode:
00949 _x = _x.encode('utf-8')
00950 length = len(_x)
00951 buff.write(struct.pack('<I%ss'%length, length, _x))
00952 buff.write(_struct_B.pack(self.action_feedback.status.status))
00953 _x = self.action_feedback.status.text
00954 length = len(_x)
00955 if python3 or type(_x) == unicode:
00956 _x = _x.encode('utf-8')
00957 length = len(_x)
00958 buff.write(struct.pack('<I%ss'%length, length, _x))
00959 except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
00960 except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
00961
00962 def deserialize_numpy(self, str, numpy):
00963 """
00964 unpack serialized message in str into this message instance using numpy for array types
00965 :param str: byte array of serialized message, ``str``
00966 :param numpy: numpy python module
00967 """
00968 try:
00969 if self.action_goal is None:
00970 self.action_goal = object_manipulation_msgs.msg.GraspHandPostureExecutionActionGoal()
00971 if self.action_result is None:
00972 self.action_result = object_manipulation_msgs.msg.GraspHandPostureExecutionActionResult()
00973 if self.action_feedback is None:
00974 self.action_feedback = object_manipulation_msgs.msg.GraspHandPostureExecutionActionFeedback()
00975 end = 0
00976 _x = self
00977 start = end
00978 end += 12
00979 (_x.action_goal.header.seq, _x.action_goal.header.stamp.secs, _x.action_goal.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00980 start = end
00981 end += 4
00982 (length,) = _struct_I.unpack(str[start:end])
00983 start = end
00984 end += length
00985 if python3:
00986 self.action_goal.header.frame_id = str[start:end].decode('utf-8')
00987 else:
00988 self.action_goal.header.frame_id = str[start:end]
00989 _x = self
00990 start = end
00991 end += 8
00992 (_x.action_goal.goal_id.stamp.secs, _x.action_goal.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
00993 start = end
00994 end += 4
00995 (length,) = _struct_I.unpack(str[start:end])
00996 start = end
00997 end += length
00998 if python3:
00999 self.action_goal.goal_id.id = str[start:end].decode('utf-8')
01000 else:
01001 self.action_goal.goal_id.id = str[start:end]
01002 start = end
01003 end += 4
01004 (length,) = _struct_I.unpack(str[start:end])
01005 start = end
01006 end += length
01007 if python3:
01008 self.action_goal.goal.grasp.id = str[start:end].decode('utf-8')
01009 else:
01010 self.action_goal.goal.grasp.id = str[start:end]
01011 _x = self
01012 start = end
01013 end += 12
01014 (_x.action_goal.goal.grasp.pre_grasp_posture.header.seq, _x.action_goal.goal.grasp.pre_grasp_posture.header.stamp.secs, _x.action_goal.goal.grasp.pre_grasp_posture.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01015 start = end
01016 end += 4
01017 (length,) = _struct_I.unpack(str[start:end])
01018 start = end
01019 end += length
01020 if python3:
01021 self.action_goal.goal.grasp.pre_grasp_posture.header.frame_id = str[start:end].decode('utf-8')
01022 else:
01023 self.action_goal.goal.grasp.pre_grasp_posture.header.frame_id = str[start:end]
01024 start = end
01025 end += 4
01026 (length,) = _struct_I.unpack(str[start:end])
01027 self.action_goal.goal.grasp.pre_grasp_posture.name = []
01028 for i in range(0, length):
01029 start = end
01030 end += 4
01031 (length,) = _struct_I.unpack(str[start:end])
01032 start = end
01033 end += length
01034 if python3:
01035 val1 = str[start:end].decode('utf-8')
01036 else:
01037 val1 = str[start:end]
01038 self.action_goal.goal.grasp.pre_grasp_posture.name.append(val1)
01039 start = end
01040 end += 4
01041 (length,) = _struct_I.unpack(str[start:end])
01042 pattern = '<%sd'%length
01043 start = end
01044 end += struct.calcsize(pattern)
01045 self.action_goal.goal.grasp.pre_grasp_posture.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01046 start = end
01047 end += 4
01048 (length,) = _struct_I.unpack(str[start:end])
01049 pattern = '<%sd'%length
01050 start = end
01051 end += struct.calcsize(pattern)
01052 self.action_goal.goal.grasp.pre_grasp_posture.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01053 start = end
01054 end += 4
01055 (length,) = _struct_I.unpack(str[start:end])
01056 pattern = '<%sd'%length
01057 start = end
01058 end += struct.calcsize(pattern)
01059 self.action_goal.goal.grasp.pre_grasp_posture.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01060 _x = self
01061 start = end
01062 end += 12
01063 (_x.action_goal.goal.grasp.grasp_posture.header.seq, _x.action_goal.goal.grasp.grasp_posture.header.stamp.secs, _x.action_goal.goal.grasp.grasp_posture.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01064 start = end
01065 end += 4
01066 (length,) = _struct_I.unpack(str[start:end])
01067 start = end
01068 end += length
01069 if python3:
01070 self.action_goal.goal.grasp.grasp_posture.header.frame_id = str[start:end].decode('utf-8')
01071 else:
01072 self.action_goal.goal.grasp.grasp_posture.header.frame_id = str[start:end]
01073 start = end
01074 end += 4
01075 (length,) = _struct_I.unpack(str[start:end])
01076 self.action_goal.goal.grasp.grasp_posture.name = []
01077 for i in range(0, length):
01078 start = end
01079 end += 4
01080 (length,) = _struct_I.unpack(str[start:end])
01081 start = end
01082 end += length
01083 if python3:
01084 val1 = str[start:end].decode('utf-8')
01085 else:
01086 val1 = str[start:end]
01087 self.action_goal.goal.grasp.grasp_posture.name.append(val1)
01088 start = end
01089 end += 4
01090 (length,) = _struct_I.unpack(str[start:end])
01091 pattern = '<%sd'%length
01092 start = end
01093 end += struct.calcsize(pattern)
01094 self.action_goal.goal.grasp.grasp_posture.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01095 start = end
01096 end += 4
01097 (length,) = _struct_I.unpack(str[start:end])
01098 pattern = '<%sd'%length
01099 start = end
01100 end += struct.calcsize(pattern)
01101 self.action_goal.goal.grasp.grasp_posture.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01102 start = end
01103 end += 4
01104 (length,) = _struct_I.unpack(str[start:end])
01105 pattern = '<%sd'%length
01106 start = end
01107 end += struct.calcsize(pattern)
01108 self.action_goal.goal.grasp.grasp_posture.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01109 _x = self
01110 start = end
01111 end += 12
01112 (_x.action_goal.goal.grasp.grasp_pose.header.seq, _x.action_goal.goal.grasp.grasp_pose.header.stamp.secs, _x.action_goal.goal.grasp.grasp_pose.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01113 start = end
01114 end += 4
01115 (length,) = _struct_I.unpack(str[start:end])
01116 start = end
01117 end += length
01118 if python3:
01119 self.action_goal.goal.grasp.grasp_pose.header.frame_id = str[start:end].decode('utf-8')
01120 else:
01121 self.action_goal.goal.grasp.grasp_pose.header.frame_id = str[start:end]
01122 _x = self
01123 start = end
01124 end += 76
01125 (_x.action_goal.goal.grasp.grasp_pose.pose.position.x, _x.action_goal.goal.grasp.grasp_pose.pose.position.y, _x.action_goal.goal.grasp.grasp_pose.pose.position.z, _x.action_goal.goal.grasp.grasp_pose.pose.orientation.x, _x.action_goal.goal.grasp.grasp_pose.pose.orientation.y, _x.action_goal.goal.grasp.grasp_pose.pose.orientation.z, _x.action_goal.goal.grasp.grasp_pose.pose.orientation.w, _x.action_goal.goal.grasp.grasp_quality, _x.action_goal.goal.grasp.approach.direction.header.seq, _x.action_goal.goal.grasp.approach.direction.header.stamp.secs, _x.action_goal.goal.grasp.approach.direction.header.stamp.nsecs,) = _struct_8d3I.unpack(str[start:end])
01126 start = end
01127 end += 4
01128 (length,) = _struct_I.unpack(str[start:end])
01129 start = end
01130 end += length
01131 if python3:
01132 self.action_goal.goal.grasp.approach.direction.header.frame_id = str[start:end].decode('utf-8')
01133 else:
01134 self.action_goal.goal.grasp.approach.direction.header.frame_id = str[start:end]
01135 _x = self
01136 start = end
01137 end += 44
01138 (_x.action_goal.goal.grasp.approach.direction.vector.x, _x.action_goal.goal.grasp.approach.direction.vector.y, _x.action_goal.goal.grasp.approach.direction.vector.z, _x.action_goal.goal.grasp.approach.desired_distance, _x.action_goal.goal.grasp.approach.min_distance, _x.action_goal.goal.grasp.retreat.direction.header.seq, _x.action_goal.goal.grasp.retreat.direction.header.stamp.secs, _x.action_goal.goal.grasp.retreat.direction.header.stamp.nsecs,) = _struct_3d2f3I.unpack(str[start:end])
01139 start = end
01140 end += 4
01141 (length,) = _struct_I.unpack(str[start:end])
01142 start = end
01143 end += length
01144 if python3:
01145 self.action_goal.goal.grasp.retreat.direction.header.frame_id = str[start:end].decode('utf-8')
01146 else:
01147 self.action_goal.goal.grasp.retreat.direction.header.frame_id = str[start:end]
01148 _x = self
01149 start = end
01150 end += 36
01151 (_x.action_goal.goal.grasp.retreat.direction.vector.x, _x.action_goal.goal.grasp.retreat.direction.vector.y, _x.action_goal.goal.grasp.retreat.direction.vector.z, _x.action_goal.goal.grasp.retreat.desired_distance, _x.action_goal.goal.grasp.retreat.min_distance, _x.action_goal.goal.grasp.max_contact_force,) = _struct_3d3f.unpack(str[start:end])
01152 start = end
01153 end += 4
01154 (length,) = _struct_I.unpack(str[start:end])
01155 self.action_goal.goal.grasp.allowed_touch_objects = []
01156 for i in range(0, length):
01157 start = end
01158 end += 4
01159 (length,) = _struct_I.unpack(str[start:end])
01160 start = end
01161 end += length
01162 if python3:
01163 val1 = str[start:end].decode('utf-8')
01164 else:
01165 val1 = str[start:end]
01166 self.action_goal.goal.grasp.allowed_touch_objects.append(val1)
01167 _x = self
01168 start = end
01169 end += 20
01170 (_x.action_goal.goal.goal, _x.action_goal.goal.max_contact_force, _x.action_result.header.seq, _x.action_result.header.stamp.secs, _x.action_result.header.stamp.nsecs,) = _struct_if3I.unpack(str[start:end])
01171 start = end
01172 end += 4
01173 (length,) = _struct_I.unpack(str[start:end])
01174 start = end
01175 end += length
01176 if python3:
01177 self.action_result.header.frame_id = str[start:end].decode('utf-8')
01178 else:
01179 self.action_result.header.frame_id = str[start:end]
01180 _x = self
01181 start = end
01182 end += 8
01183 (_x.action_result.status.goal_id.stamp.secs, _x.action_result.status.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
01184 start = end
01185 end += 4
01186 (length,) = _struct_I.unpack(str[start:end])
01187 start = end
01188 end += length
01189 if python3:
01190 self.action_result.status.goal_id.id = str[start:end].decode('utf-8')
01191 else:
01192 self.action_result.status.goal_id.id = str[start:end]
01193 start = end
01194 end += 1
01195 (self.action_result.status.status,) = _struct_B.unpack(str[start:end])
01196 start = end
01197 end += 4
01198 (length,) = _struct_I.unpack(str[start:end])
01199 start = end
01200 end += length
01201 if python3:
01202 self.action_result.status.text = str[start:end].decode('utf-8')
01203 else:
01204 self.action_result.status.text = str[start:end]
01205 _x = self
01206 start = end
01207 end += 16
01208 (_x.action_result.result.result.value, _x.action_feedback.header.seq, _x.action_feedback.header.stamp.secs, _x.action_feedback.header.stamp.nsecs,) = _struct_i3I.unpack(str[start:end])
01209 start = end
01210 end += 4
01211 (length,) = _struct_I.unpack(str[start:end])
01212 start = end
01213 end += length
01214 if python3:
01215 self.action_feedback.header.frame_id = str[start:end].decode('utf-8')
01216 else:
01217 self.action_feedback.header.frame_id = str[start:end]
01218 _x = self
01219 start = end
01220 end += 8
01221 (_x.action_feedback.status.goal_id.stamp.secs, _x.action_feedback.status.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
01222 start = end
01223 end += 4
01224 (length,) = _struct_I.unpack(str[start:end])
01225 start = end
01226 end += length
01227 if python3:
01228 self.action_feedback.status.goal_id.id = str[start:end].decode('utf-8')
01229 else:
01230 self.action_feedback.status.goal_id.id = str[start:end]
01231 start = end
01232 end += 1
01233 (self.action_feedback.status.status,) = _struct_B.unpack(str[start:end])
01234 start = end
01235 end += 4
01236 (length,) = _struct_I.unpack(str[start:end])
01237 start = end
01238 end += length
01239 if python3:
01240 self.action_feedback.status.text = str[start:end].decode('utf-8')
01241 else:
01242 self.action_feedback.status.text = str[start:end]
01243 return self
01244 except struct.error as e:
01245 raise genpy.DeserializationError(e)
01246
01247 _struct_I = genpy.struct_I
01248 _struct_if3I = struct.Struct("<if3I")
01249 _struct_B = struct.Struct("<B")
01250 _struct_8d3I = struct.Struct("<8d3I")
01251 _struct_i3I = struct.Struct("<i3I")
01252 _struct_3d3f = struct.Struct("<3d3f")
01253 _struct_3I = struct.Struct("<3I")
01254 _struct_3d2f3I = struct.Struct("<3d2f3I")
01255 _struct_2I = struct.Struct("<2I")