00001 """autogenerated by genpy from head_monitor_msgs/HeadMonitorAction.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 trajectory_msgs.msg
00008 import arm_navigation_msgs.msg
00009 import actionlib_msgs.msg
00010 import geometry_msgs.msg
00011 import head_monitor_msgs.msg
00012 import sensor_msgs.msg
00013 import genpy
00014 import std_msgs.msg
00015
00016 class HeadMonitorAction(genpy.Message):
00017 _md5sum = "09344416f769d6f374dfa93a50a9bea8"
00018 _type = "head_monitor_msgs/HeadMonitorAction"
00019 _has_header = False
00020 _full_text = """# ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00021
00022 HeadMonitorActionGoal action_goal
00023 HeadMonitorActionResult action_result
00024 HeadMonitorActionFeedback action_feedback
00025
00026 ================================================================================
00027 MSG: head_monitor_msgs/HeadMonitorActionGoal
00028 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00029
00030 Header header
00031 actionlib_msgs/GoalID goal_id
00032 HeadMonitorGoal goal
00033
00034 ================================================================================
00035 MSG: std_msgs/Header
00036 # Standard metadata for higher-level stamped data types.
00037 # This is generally used to communicate timestamped data
00038 # in a particular coordinate frame.
00039 #
00040 # sequence ID: consecutively increasing ID
00041 uint32 seq
00042 #Two-integer timestamp that is expressed as:
00043 # * stamp.secs: seconds (stamp_secs) since epoch
00044 # * stamp.nsecs: nanoseconds since stamp_secs
00045 # time-handling sugar is provided by the client library
00046 time stamp
00047 #Frame this data is associated with
00048 # 0: no frame
00049 # 1: global frame
00050 string frame_id
00051
00052 ================================================================================
00053 MSG: actionlib_msgs/GoalID
00054 # The stamp should store the time at which this goal was requested.
00055 # It is used by an action server when it tries to preempt all
00056 # goals that were requested before a certain time
00057 time stamp
00058
00059 # The id provides a way to associate feedback and
00060 # result message with specific goal requests. The id
00061 # specified must be unique.
00062 string id
00063
00064
00065 ================================================================================
00066 MSG: head_monitor_msgs/HeadMonitorGoal
00067 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00068 string group_name
00069 trajectory_msgs/JointTrajectory joint_trajectory
00070
00071 #goal definition
00072 duration time_offset
00073 string target_link
00074
00075 ================================================================================
00076 MSG: trajectory_msgs/JointTrajectory
00077 Header header
00078 string[] joint_names
00079 JointTrajectoryPoint[] points
00080 ================================================================================
00081 MSG: trajectory_msgs/JointTrajectoryPoint
00082 float64[] positions
00083 float64[] velocities
00084 float64[] accelerations
00085 duration time_from_start
00086 ================================================================================
00087 MSG: head_monitor_msgs/HeadMonitorActionResult
00088 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00089
00090 Header header
00091 actionlib_msgs/GoalStatus status
00092 HeadMonitorResult result
00093
00094 ================================================================================
00095 MSG: actionlib_msgs/GoalStatus
00096 GoalID goal_id
00097 uint8 status
00098 uint8 PENDING = 0 # The goal has yet to be processed by the action server
00099 uint8 ACTIVE = 1 # The goal is currently being processed by the action server
00100 uint8 PREEMPTED = 2 # The goal received a cancel request after it started executing
00101 # and has since completed its execution (Terminal State)
00102 uint8 SUCCEEDED = 3 # The goal was achieved successfully by the action server (Terminal State)
00103 uint8 ABORTED = 4 # The goal was aborted during execution by the action server due
00104 # to some failure (Terminal State)
00105 uint8 REJECTED = 5 # The goal was rejected by the action server without being processed,
00106 # because the goal was unattainable or invalid (Terminal State)
00107 uint8 PREEMPTING = 6 # The goal received a cancel request after it started executing
00108 # and has not yet completed execution
00109 uint8 RECALLING = 7 # The goal received a cancel request before it started executing,
00110 # but the action server has not yet confirmed that the goal is canceled
00111 uint8 RECALLED = 8 # The goal received a cancel request before it started executing
00112 # and was successfully cancelled (Terminal State)
00113 uint8 LOST = 9 # An action client can determine that a goal is LOST. This should not be
00114 # sent over the wire by an action server
00115
00116 #Allow for the user to associate a string with GoalStatus for debugging
00117 string text
00118
00119
00120 ================================================================================
00121 MSG: head_monitor_msgs/HeadMonitorResult
00122 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00123 trajectory_msgs/JointTrajectory actual_trajectory
00124 arm_navigation_msgs/ArmNavigationErrorCodes error_code
00125
00126 ================================================================================
00127 MSG: arm_navigation_msgs/ArmNavigationErrorCodes
00128 int32 val
00129
00130 # overall behavior
00131 int32 PLANNING_FAILED=-1
00132 int32 SUCCESS=1
00133 int32 TIMED_OUT=-2
00134
00135 # start state errors
00136 int32 START_STATE_IN_COLLISION=-3
00137 int32 START_STATE_VIOLATES_PATH_CONSTRAINTS=-4
00138
00139 # goal errors
00140 int32 GOAL_IN_COLLISION=-5
00141 int32 GOAL_VIOLATES_PATH_CONSTRAINTS=-6
00142
00143 # robot state
00144 int32 INVALID_ROBOT_STATE=-7
00145 int32 INCOMPLETE_ROBOT_STATE=-8
00146
00147 # planning request errors
00148 int32 INVALID_PLANNER_ID=-9
00149 int32 INVALID_NUM_PLANNING_ATTEMPTS=-10
00150 int32 INVALID_ALLOWED_PLANNING_TIME=-11
00151 int32 INVALID_GROUP_NAME=-12
00152 int32 INVALID_GOAL_JOINT_CONSTRAINTS=-13
00153 int32 INVALID_GOAL_POSITION_CONSTRAINTS=-14
00154 int32 INVALID_GOAL_ORIENTATION_CONSTRAINTS=-15
00155 int32 INVALID_PATH_JOINT_CONSTRAINTS=-16
00156 int32 INVALID_PATH_POSITION_CONSTRAINTS=-17
00157 int32 INVALID_PATH_ORIENTATION_CONSTRAINTS=-18
00158
00159 # state/trajectory monitor errors
00160 int32 INVALID_TRAJECTORY=-19
00161 int32 INVALID_INDEX=-20
00162 int32 JOINT_LIMITS_VIOLATED=-21
00163 int32 PATH_CONSTRAINTS_VIOLATED=-22
00164 int32 COLLISION_CONSTRAINTS_VIOLATED=-23
00165 int32 GOAL_CONSTRAINTS_VIOLATED=-24
00166 int32 JOINTS_NOT_MOVING=-25
00167 int32 TRAJECTORY_CONTROLLER_FAILED=-26
00168
00169 # system errors
00170 int32 FRAME_TRANSFORM_FAILURE=-27
00171 int32 COLLISION_CHECKING_UNAVAILABLE=-28
00172 int32 ROBOT_STATE_STALE=-29
00173 int32 SENSOR_INFO_STALE=-30
00174
00175 # kinematics errors
00176 int32 NO_IK_SOLUTION=-31
00177 int32 INVALID_LINK_NAME=-32
00178 int32 IK_LINK_IN_COLLISION=-33
00179 int32 NO_FK_SOLUTION=-34
00180 int32 KINEMATICS_STATE_IN_COLLISION=-35
00181
00182 # general errors
00183 int32 INVALID_TIMEOUT=-36
00184
00185
00186 ================================================================================
00187 MSG: head_monitor_msgs/HeadMonitorActionFeedback
00188 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00189
00190 Header header
00191 actionlib_msgs/GoalStatus status
00192 HeadMonitorFeedback feedback
00193
00194 ================================================================================
00195 MSG: head_monitor_msgs/HeadMonitorFeedback
00196 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00197 arm_navigation_msgs/RobotState current_state
00198 arm_navigation_msgs/RobotState paused_trajectory_state
00199 arm_navigation_msgs/CollisionObject paused_collision_map
00200
00201
00202
00203 ================================================================================
00204 MSG: arm_navigation_msgs/RobotState
00205 # This message contains information about the robot state, i.e. the positions of its joints and links
00206 sensor_msgs/JointState joint_state
00207 arm_navigation_msgs/MultiDOFJointState multi_dof_joint_state
00208
00209 ================================================================================
00210 MSG: sensor_msgs/JointState
00211 # This is a message that holds data to describe the state of a set of torque controlled joints.
00212 #
00213 # The state of each joint (revolute or prismatic) is defined by:
00214 # * the position of the joint (rad or m),
00215 # * the velocity of the joint (rad/s or m/s) and
00216 # * the effort that is applied in the joint (Nm or N).
00217 #
00218 # Each joint is uniquely identified by its name
00219 # The header specifies the time at which the joint states were recorded. All the joint states
00220 # in one message have to be recorded at the same time.
00221 #
00222 # This message consists of a multiple arrays, one for each part of the joint state.
00223 # The goal is to make each of the fields optional. When e.g. your joints have no
00224 # effort associated with them, you can leave the effort array empty.
00225 #
00226 # All arrays in this message should have the same size, or be empty.
00227 # This is the only way to uniquely associate the joint name with the correct
00228 # states.
00229
00230
00231 Header header
00232
00233 string[] name
00234 float64[] position
00235 float64[] velocity
00236 float64[] effort
00237
00238 ================================================================================
00239 MSG: arm_navigation_msgs/MultiDOFJointState
00240 #A representation of a multi-dof joint state
00241 time stamp
00242 string[] joint_names
00243 string[] frame_ids
00244 string[] child_frame_ids
00245 geometry_msgs/Pose[] poses
00246
00247 ================================================================================
00248 MSG: geometry_msgs/Pose
00249 # A representation of pose in free space, composed of postion and orientation.
00250 Point position
00251 Quaternion orientation
00252
00253 ================================================================================
00254 MSG: geometry_msgs/Point
00255 # This contains the position of a point in free space
00256 float64 x
00257 float64 y
00258 float64 z
00259
00260 ================================================================================
00261 MSG: geometry_msgs/Quaternion
00262 # This represents an orientation in free space in quaternion form.
00263
00264 float64 x
00265 float64 y
00266 float64 z
00267 float64 w
00268
00269 ================================================================================
00270 MSG: arm_navigation_msgs/CollisionObject
00271 # a header, used for interpreting the poses
00272 Header header
00273
00274 # the id of the object
00275 string id
00276
00277 # The padding used for filtering points near the object.
00278 # This does not affect collision checking for the object.
00279 # Set to negative to get zero padding.
00280 float32 padding
00281
00282 #This contains what is to be done with the object
00283 CollisionObjectOperation operation
00284
00285 #the shapes associated with the object
00286 arm_navigation_msgs/Shape[] shapes
00287
00288 #the poses associated with the shapes - will be transformed using the header
00289 geometry_msgs/Pose[] poses
00290
00291 ================================================================================
00292 MSG: arm_navigation_msgs/CollisionObjectOperation
00293 #Puts the object into the environment
00294 #or updates the object if already added
00295 byte ADD=0
00296
00297 #Removes the object from the environment entirely
00298 byte REMOVE=1
00299
00300 #Only valid within the context of a CollisionAttachedObject message
00301 #Will be ignored if sent with an CollisionObject message
00302 #Takes an attached object, detaches from the attached link
00303 #But adds back in as regular object
00304 byte DETACH_AND_ADD_AS_OBJECT=2
00305
00306 #Only valid within the context of a CollisionAttachedObject message
00307 #Will be ignored if sent with an CollisionObject message
00308 #Takes current object in the environment and removes it as
00309 #a regular object
00310 byte ATTACH_AND_REMOVE_AS_OBJECT=3
00311
00312 # Byte code for operation
00313 byte operation
00314
00315 ================================================================================
00316 MSG: arm_navigation_msgs/Shape
00317 byte SPHERE=0
00318 byte BOX=1
00319 byte CYLINDER=2
00320 byte MESH=3
00321
00322 byte type
00323
00324
00325 #### define sphere, box, cylinder ####
00326 # the origin of each shape is considered at the shape's center
00327
00328 # for sphere
00329 # radius := dimensions[0]
00330
00331 # for cylinder
00332 # radius := dimensions[0]
00333 # length := dimensions[1]
00334 # the length is along the Z axis
00335
00336 # for box
00337 # size_x := dimensions[0]
00338 # size_y := dimensions[1]
00339 # size_z := dimensions[2]
00340 float64[] dimensions
00341
00342
00343 #### define mesh ####
00344
00345 # list of triangles; triangle k is defined by tre vertices located
00346 # at indices triangles[3k], triangles[3k+1], triangles[3k+2]
00347 int32[] triangles
00348 geometry_msgs/Point[] vertices
00349
00350 """
00351 __slots__ = ['action_goal','action_result','action_feedback']
00352 _slot_types = ['head_monitor_msgs/HeadMonitorActionGoal','head_monitor_msgs/HeadMonitorActionResult','head_monitor_msgs/HeadMonitorActionFeedback']
00353
00354 def __init__(self, *args, **kwds):
00355 """
00356 Constructor. Any message fields that are implicitly/explicitly
00357 set to None will be assigned a default value. The recommend
00358 use is keyword arguments as this is more robust to future message
00359 changes. You cannot mix in-order arguments and keyword arguments.
00360
00361 The available fields are:
00362 action_goal,action_result,action_feedback
00363
00364 :param args: complete set of field values, in .msg order
00365 :param kwds: use keyword arguments corresponding to message field names
00366 to set specific fields.
00367 """
00368 if args or kwds:
00369 super(HeadMonitorAction, self).__init__(*args, **kwds)
00370
00371 if self.action_goal is None:
00372 self.action_goal = head_monitor_msgs.msg.HeadMonitorActionGoal()
00373 if self.action_result is None:
00374 self.action_result = head_monitor_msgs.msg.HeadMonitorActionResult()
00375 if self.action_feedback is None:
00376 self.action_feedback = head_monitor_msgs.msg.HeadMonitorActionFeedback()
00377 else:
00378 self.action_goal = head_monitor_msgs.msg.HeadMonitorActionGoal()
00379 self.action_result = head_monitor_msgs.msg.HeadMonitorActionResult()
00380 self.action_feedback = head_monitor_msgs.msg.HeadMonitorActionFeedback()
00381
00382 def _get_types(self):
00383 """
00384 internal API method
00385 """
00386 return self._slot_types
00387
00388 def serialize(self, buff):
00389 """
00390 serialize message into buffer
00391 :param buff: buffer, ``StringIO``
00392 """
00393 try:
00394 _x = self
00395 buff.write(_struct_3I.pack(_x.action_goal.header.seq, _x.action_goal.header.stamp.secs, _x.action_goal.header.stamp.nsecs))
00396 _x = self.action_goal.header.frame_id
00397 length = len(_x)
00398 if python3 or type(_x) == unicode:
00399 _x = _x.encode('utf-8')
00400 length = len(_x)
00401 buff.write(struct.pack('<I%ss'%length, length, _x))
00402 _x = self
00403 buff.write(_struct_2I.pack(_x.action_goal.goal_id.stamp.secs, _x.action_goal.goal_id.stamp.nsecs))
00404 _x = self.action_goal.goal_id.id
00405 length = len(_x)
00406 if python3 or type(_x) == unicode:
00407 _x = _x.encode('utf-8')
00408 length = len(_x)
00409 buff.write(struct.pack('<I%ss'%length, length, _x))
00410 _x = self.action_goal.goal.group_name
00411 length = len(_x)
00412 if python3 or type(_x) == unicode:
00413 _x = _x.encode('utf-8')
00414 length = len(_x)
00415 buff.write(struct.pack('<I%ss'%length, length, _x))
00416 _x = self
00417 buff.write(_struct_3I.pack(_x.action_goal.goal.joint_trajectory.header.seq, _x.action_goal.goal.joint_trajectory.header.stamp.secs, _x.action_goal.goal.joint_trajectory.header.stamp.nsecs))
00418 _x = self.action_goal.goal.joint_trajectory.header.frame_id
00419 length = len(_x)
00420 if python3 or type(_x) == unicode:
00421 _x = _x.encode('utf-8')
00422 length = len(_x)
00423 buff.write(struct.pack('<I%ss'%length, length, _x))
00424 length = len(self.action_goal.goal.joint_trajectory.joint_names)
00425 buff.write(_struct_I.pack(length))
00426 for val1 in self.action_goal.goal.joint_trajectory.joint_names:
00427 length = len(val1)
00428 if python3 or type(val1) == unicode:
00429 val1 = val1.encode('utf-8')
00430 length = len(val1)
00431 buff.write(struct.pack('<I%ss'%length, length, val1))
00432 length = len(self.action_goal.goal.joint_trajectory.points)
00433 buff.write(_struct_I.pack(length))
00434 for val1 in self.action_goal.goal.joint_trajectory.points:
00435 length = len(val1.positions)
00436 buff.write(_struct_I.pack(length))
00437 pattern = '<%sd'%length
00438 buff.write(struct.pack(pattern, *val1.positions))
00439 length = len(val1.velocities)
00440 buff.write(_struct_I.pack(length))
00441 pattern = '<%sd'%length
00442 buff.write(struct.pack(pattern, *val1.velocities))
00443 length = len(val1.accelerations)
00444 buff.write(_struct_I.pack(length))
00445 pattern = '<%sd'%length
00446 buff.write(struct.pack(pattern, *val1.accelerations))
00447 _v1 = val1.time_from_start
00448 _x = _v1
00449 buff.write(_struct_2i.pack(_x.secs, _x.nsecs))
00450 _x = self
00451 buff.write(_struct_2i.pack(_x.action_goal.goal.time_offset.secs, _x.action_goal.goal.time_offset.nsecs))
00452 _x = self.action_goal.goal.target_link
00453 length = len(_x)
00454 if python3 or type(_x) == unicode:
00455 _x = _x.encode('utf-8')
00456 length = len(_x)
00457 buff.write(struct.pack('<I%ss'%length, length, _x))
00458 _x = self
00459 buff.write(_struct_3I.pack(_x.action_result.header.seq, _x.action_result.header.stamp.secs, _x.action_result.header.stamp.nsecs))
00460 _x = self.action_result.header.frame_id
00461 length = len(_x)
00462 if python3 or type(_x) == unicode:
00463 _x = _x.encode('utf-8')
00464 length = len(_x)
00465 buff.write(struct.pack('<I%ss'%length, length, _x))
00466 _x = self
00467 buff.write(_struct_2I.pack(_x.action_result.status.goal_id.stamp.secs, _x.action_result.status.goal_id.stamp.nsecs))
00468 _x = self.action_result.status.goal_id.id
00469 length = len(_x)
00470 if python3 or type(_x) == unicode:
00471 _x = _x.encode('utf-8')
00472 length = len(_x)
00473 buff.write(struct.pack('<I%ss'%length, length, _x))
00474 buff.write(_struct_B.pack(self.action_result.status.status))
00475 _x = self.action_result.status.text
00476 length = len(_x)
00477 if python3 or type(_x) == unicode:
00478 _x = _x.encode('utf-8')
00479 length = len(_x)
00480 buff.write(struct.pack('<I%ss'%length, length, _x))
00481 _x = self
00482 buff.write(_struct_3I.pack(_x.action_result.result.actual_trajectory.header.seq, _x.action_result.result.actual_trajectory.header.stamp.secs, _x.action_result.result.actual_trajectory.header.stamp.nsecs))
00483 _x = self.action_result.result.actual_trajectory.header.frame_id
00484 length = len(_x)
00485 if python3 or type(_x) == unicode:
00486 _x = _x.encode('utf-8')
00487 length = len(_x)
00488 buff.write(struct.pack('<I%ss'%length, length, _x))
00489 length = len(self.action_result.result.actual_trajectory.joint_names)
00490 buff.write(_struct_I.pack(length))
00491 for val1 in self.action_result.result.actual_trajectory.joint_names:
00492 length = len(val1)
00493 if python3 or type(val1) == unicode:
00494 val1 = val1.encode('utf-8')
00495 length = len(val1)
00496 buff.write(struct.pack('<I%ss'%length, length, val1))
00497 length = len(self.action_result.result.actual_trajectory.points)
00498 buff.write(_struct_I.pack(length))
00499 for val1 in self.action_result.result.actual_trajectory.points:
00500 length = len(val1.positions)
00501 buff.write(_struct_I.pack(length))
00502 pattern = '<%sd'%length
00503 buff.write(struct.pack(pattern, *val1.positions))
00504 length = len(val1.velocities)
00505 buff.write(_struct_I.pack(length))
00506 pattern = '<%sd'%length
00507 buff.write(struct.pack(pattern, *val1.velocities))
00508 length = len(val1.accelerations)
00509 buff.write(_struct_I.pack(length))
00510 pattern = '<%sd'%length
00511 buff.write(struct.pack(pattern, *val1.accelerations))
00512 _v2 = val1.time_from_start
00513 _x = _v2
00514 buff.write(_struct_2i.pack(_x.secs, _x.nsecs))
00515 _x = self
00516 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))
00517 _x = self.action_feedback.header.frame_id
00518 length = len(_x)
00519 if python3 or type(_x) == unicode:
00520 _x = _x.encode('utf-8')
00521 length = len(_x)
00522 buff.write(struct.pack('<I%ss'%length, length, _x))
00523 _x = self
00524 buff.write(_struct_2I.pack(_x.action_feedback.status.goal_id.stamp.secs, _x.action_feedback.status.goal_id.stamp.nsecs))
00525 _x = self.action_feedback.status.goal_id.id
00526 length = len(_x)
00527 if python3 or type(_x) == unicode:
00528 _x = _x.encode('utf-8')
00529 length = len(_x)
00530 buff.write(struct.pack('<I%ss'%length, length, _x))
00531 buff.write(_struct_B.pack(self.action_feedback.status.status))
00532 _x = self.action_feedback.status.text
00533 length = len(_x)
00534 if python3 or type(_x) == unicode:
00535 _x = _x.encode('utf-8')
00536 length = len(_x)
00537 buff.write(struct.pack('<I%ss'%length, length, _x))
00538 _x = self
00539 buff.write(_struct_3I.pack(_x.action_feedback.feedback.current_state.joint_state.header.seq, _x.action_feedback.feedback.current_state.joint_state.header.stamp.secs, _x.action_feedback.feedback.current_state.joint_state.header.stamp.nsecs))
00540 _x = self.action_feedback.feedback.current_state.joint_state.header.frame_id
00541 length = len(_x)
00542 if python3 or type(_x) == unicode:
00543 _x = _x.encode('utf-8')
00544 length = len(_x)
00545 buff.write(struct.pack('<I%ss'%length, length, _x))
00546 length = len(self.action_feedback.feedback.current_state.joint_state.name)
00547 buff.write(_struct_I.pack(length))
00548 for val1 in self.action_feedback.feedback.current_state.joint_state.name:
00549 length = len(val1)
00550 if python3 or type(val1) == unicode:
00551 val1 = val1.encode('utf-8')
00552 length = len(val1)
00553 buff.write(struct.pack('<I%ss'%length, length, val1))
00554 length = len(self.action_feedback.feedback.current_state.joint_state.position)
00555 buff.write(_struct_I.pack(length))
00556 pattern = '<%sd'%length
00557 buff.write(struct.pack(pattern, *self.action_feedback.feedback.current_state.joint_state.position))
00558 length = len(self.action_feedback.feedback.current_state.joint_state.velocity)
00559 buff.write(_struct_I.pack(length))
00560 pattern = '<%sd'%length
00561 buff.write(struct.pack(pattern, *self.action_feedback.feedback.current_state.joint_state.velocity))
00562 length = len(self.action_feedback.feedback.current_state.joint_state.effort)
00563 buff.write(_struct_I.pack(length))
00564 pattern = '<%sd'%length
00565 buff.write(struct.pack(pattern, *self.action_feedback.feedback.current_state.joint_state.effort))
00566 _x = self
00567 buff.write(_struct_2I.pack(_x.action_feedback.feedback.current_state.multi_dof_joint_state.stamp.secs, _x.action_feedback.feedback.current_state.multi_dof_joint_state.stamp.nsecs))
00568 length = len(self.action_feedback.feedback.current_state.multi_dof_joint_state.joint_names)
00569 buff.write(_struct_I.pack(length))
00570 for val1 in self.action_feedback.feedback.current_state.multi_dof_joint_state.joint_names:
00571 length = len(val1)
00572 if python3 or type(val1) == unicode:
00573 val1 = val1.encode('utf-8')
00574 length = len(val1)
00575 buff.write(struct.pack('<I%ss'%length, length, val1))
00576 length = len(self.action_feedback.feedback.current_state.multi_dof_joint_state.frame_ids)
00577 buff.write(_struct_I.pack(length))
00578 for val1 in self.action_feedback.feedback.current_state.multi_dof_joint_state.frame_ids:
00579 length = len(val1)
00580 if python3 or type(val1) == unicode:
00581 val1 = val1.encode('utf-8')
00582 length = len(val1)
00583 buff.write(struct.pack('<I%ss'%length, length, val1))
00584 length = len(self.action_feedback.feedback.current_state.multi_dof_joint_state.child_frame_ids)
00585 buff.write(_struct_I.pack(length))
00586 for val1 in self.action_feedback.feedback.current_state.multi_dof_joint_state.child_frame_ids:
00587 length = len(val1)
00588 if python3 or type(val1) == unicode:
00589 val1 = val1.encode('utf-8')
00590 length = len(val1)
00591 buff.write(struct.pack('<I%ss'%length, length, val1))
00592 length = len(self.action_feedback.feedback.current_state.multi_dof_joint_state.poses)
00593 buff.write(_struct_I.pack(length))
00594 for val1 in self.action_feedback.feedback.current_state.multi_dof_joint_state.poses:
00595 _v3 = val1.position
00596 _x = _v3
00597 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00598 _v4 = val1.orientation
00599 _x = _v4
00600 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00601 _x = self
00602 buff.write(_struct_3I.pack(_x.action_feedback.feedback.paused_trajectory_state.joint_state.header.seq, _x.action_feedback.feedback.paused_trajectory_state.joint_state.header.stamp.secs, _x.action_feedback.feedback.paused_trajectory_state.joint_state.header.stamp.nsecs))
00603 _x = self.action_feedback.feedback.paused_trajectory_state.joint_state.header.frame_id
00604 length = len(_x)
00605 if python3 or type(_x) == unicode:
00606 _x = _x.encode('utf-8')
00607 length = len(_x)
00608 buff.write(struct.pack('<I%ss'%length, length, _x))
00609 length = len(self.action_feedback.feedback.paused_trajectory_state.joint_state.name)
00610 buff.write(_struct_I.pack(length))
00611 for val1 in self.action_feedback.feedback.paused_trajectory_state.joint_state.name:
00612 length = len(val1)
00613 if python3 or type(val1) == unicode:
00614 val1 = val1.encode('utf-8')
00615 length = len(val1)
00616 buff.write(struct.pack('<I%ss'%length, length, val1))
00617 length = len(self.action_feedback.feedback.paused_trajectory_state.joint_state.position)
00618 buff.write(_struct_I.pack(length))
00619 pattern = '<%sd'%length
00620 buff.write(struct.pack(pattern, *self.action_feedback.feedback.paused_trajectory_state.joint_state.position))
00621 length = len(self.action_feedback.feedback.paused_trajectory_state.joint_state.velocity)
00622 buff.write(_struct_I.pack(length))
00623 pattern = '<%sd'%length
00624 buff.write(struct.pack(pattern, *self.action_feedback.feedback.paused_trajectory_state.joint_state.velocity))
00625 length = len(self.action_feedback.feedback.paused_trajectory_state.joint_state.effort)
00626 buff.write(_struct_I.pack(length))
00627 pattern = '<%sd'%length
00628 buff.write(struct.pack(pattern, *self.action_feedback.feedback.paused_trajectory_state.joint_state.effort))
00629 _x = self
00630 buff.write(_struct_2I.pack(_x.action_feedback.feedback.paused_trajectory_state.multi_dof_joint_state.stamp.secs, _x.action_feedback.feedback.paused_trajectory_state.multi_dof_joint_state.stamp.nsecs))
00631 length = len(self.action_feedback.feedback.paused_trajectory_state.multi_dof_joint_state.joint_names)
00632 buff.write(_struct_I.pack(length))
00633 for val1 in self.action_feedback.feedback.paused_trajectory_state.multi_dof_joint_state.joint_names:
00634 length = len(val1)
00635 if python3 or type(val1) == unicode:
00636 val1 = val1.encode('utf-8')
00637 length = len(val1)
00638 buff.write(struct.pack('<I%ss'%length, length, val1))
00639 length = len(self.action_feedback.feedback.paused_trajectory_state.multi_dof_joint_state.frame_ids)
00640 buff.write(_struct_I.pack(length))
00641 for val1 in self.action_feedback.feedback.paused_trajectory_state.multi_dof_joint_state.frame_ids:
00642 length = len(val1)
00643 if python3 or type(val1) == unicode:
00644 val1 = val1.encode('utf-8')
00645 length = len(val1)
00646 buff.write(struct.pack('<I%ss'%length, length, val1))
00647 length = len(self.action_feedback.feedback.paused_trajectory_state.multi_dof_joint_state.child_frame_ids)
00648 buff.write(_struct_I.pack(length))
00649 for val1 in self.action_feedback.feedback.paused_trajectory_state.multi_dof_joint_state.child_frame_ids:
00650 length = len(val1)
00651 if python3 or type(val1) == unicode:
00652 val1 = val1.encode('utf-8')
00653 length = len(val1)
00654 buff.write(struct.pack('<I%ss'%length, length, val1))
00655 length = len(self.action_feedback.feedback.paused_trajectory_state.multi_dof_joint_state.poses)
00656 buff.write(_struct_I.pack(length))
00657 for val1 in self.action_feedback.feedback.paused_trajectory_state.multi_dof_joint_state.poses:
00658 _v5 = val1.position
00659 _x = _v5
00660 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00661 _v6 = val1.orientation
00662 _x = _v6
00663 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00664 _x = self
00665 buff.write(_struct_3I.pack(_x.action_feedback.feedback.paused_collision_map.header.seq, _x.action_feedback.feedback.paused_collision_map.header.stamp.secs, _x.action_feedback.feedback.paused_collision_map.header.stamp.nsecs))
00666 _x = self.action_feedback.feedback.paused_collision_map.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.action_feedback.feedback.paused_collision_map.id
00673 length = len(_x)
00674 if python3 or type(_x) == unicode:
00675 _x = _x.encode('utf-8')
00676 length = len(_x)
00677 buff.write(struct.pack('<I%ss'%length, length, _x))
00678 _x = self
00679 buff.write(_struct_fb.pack(_x.action_feedback.feedback.paused_collision_map.padding, _x.action_feedback.feedback.paused_collision_map.operation.operation))
00680 length = len(self.action_feedback.feedback.paused_collision_map.shapes)
00681 buff.write(_struct_I.pack(length))
00682 for val1 in self.action_feedback.feedback.paused_collision_map.shapes:
00683 buff.write(_struct_b.pack(val1.type))
00684 length = len(val1.dimensions)
00685 buff.write(_struct_I.pack(length))
00686 pattern = '<%sd'%length
00687 buff.write(struct.pack(pattern, *val1.dimensions))
00688 length = len(val1.triangles)
00689 buff.write(_struct_I.pack(length))
00690 pattern = '<%si'%length
00691 buff.write(struct.pack(pattern, *val1.triangles))
00692 length = len(val1.vertices)
00693 buff.write(_struct_I.pack(length))
00694 for val2 in val1.vertices:
00695 _x = val2
00696 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00697 length = len(self.action_feedback.feedback.paused_collision_map.poses)
00698 buff.write(_struct_I.pack(length))
00699 for val1 in self.action_feedback.feedback.paused_collision_map.poses:
00700 _v7 = val1.position
00701 _x = _v7
00702 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00703 _v8 = val1.orientation
00704 _x = _v8
00705 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00706 except struct.error as se: self._check_types(se)
00707 except TypeError as te: self._check_types(te)
00708
00709 def deserialize(self, str):
00710 """
00711 unpack serialized message in str into this message instance
00712 :param str: byte array of serialized message, ``str``
00713 """
00714 try:
00715 if self.action_goal is None:
00716 self.action_goal = head_monitor_msgs.msg.HeadMonitorActionGoal()
00717 if self.action_result is None:
00718 self.action_result = head_monitor_msgs.msg.HeadMonitorActionResult()
00719 if self.action_feedback is None:
00720 self.action_feedback = head_monitor_msgs.msg.HeadMonitorActionFeedback()
00721 end = 0
00722 _x = self
00723 start = end
00724 end += 12
00725 (_x.action_goal.header.seq, _x.action_goal.header.stamp.secs, _x.action_goal.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00726 start = end
00727 end += 4
00728 (length,) = _struct_I.unpack(str[start:end])
00729 start = end
00730 end += length
00731 if python3:
00732 self.action_goal.header.frame_id = str[start:end].decode('utf-8')
00733 else:
00734 self.action_goal.header.frame_id = str[start:end]
00735 _x = self
00736 start = end
00737 end += 8
00738 (_x.action_goal.goal_id.stamp.secs, _x.action_goal.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
00739 start = end
00740 end += 4
00741 (length,) = _struct_I.unpack(str[start:end])
00742 start = end
00743 end += length
00744 if python3:
00745 self.action_goal.goal_id.id = str[start:end].decode('utf-8')
00746 else:
00747 self.action_goal.goal_id.id = str[start:end]
00748 start = end
00749 end += 4
00750 (length,) = _struct_I.unpack(str[start:end])
00751 start = end
00752 end += length
00753 if python3:
00754 self.action_goal.goal.group_name = str[start:end].decode('utf-8')
00755 else:
00756 self.action_goal.goal.group_name = str[start:end]
00757 _x = self
00758 start = end
00759 end += 12
00760 (_x.action_goal.goal.joint_trajectory.header.seq, _x.action_goal.goal.joint_trajectory.header.stamp.secs, _x.action_goal.goal.joint_trajectory.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00761 start = end
00762 end += 4
00763 (length,) = _struct_I.unpack(str[start:end])
00764 start = end
00765 end += length
00766 if python3:
00767 self.action_goal.goal.joint_trajectory.header.frame_id = str[start:end].decode('utf-8')
00768 else:
00769 self.action_goal.goal.joint_trajectory.header.frame_id = str[start:end]
00770 start = end
00771 end += 4
00772 (length,) = _struct_I.unpack(str[start:end])
00773 self.action_goal.goal.joint_trajectory.joint_names = []
00774 for i in range(0, length):
00775 start = end
00776 end += 4
00777 (length,) = _struct_I.unpack(str[start:end])
00778 start = end
00779 end += length
00780 if python3:
00781 val1 = str[start:end].decode('utf-8')
00782 else:
00783 val1 = str[start:end]
00784 self.action_goal.goal.joint_trajectory.joint_names.append(val1)
00785 start = end
00786 end += 4
00787 (length,) = _struct_I.unpack(str[start:end])
00788 self.action_goal.goal.joint_trajectory.points = []
00789 for i in range(0, length):
00790 val1 = trajectory_msgs.msg.JointTrajectoryPoint()
00791 start = end
00792 end += 4
00793 (length,) = _struct_I.unpack(str[start:end])
00794 pattern = '<%sd'%length
00795 start = end
00796 end += struct.calcsize(pattern)
00797 val1.positions = struct.unpack(pattern, str[start:end])
00798 start = end
00799 end += 4
00800 (length,) = _struct_I.unpack(str[start:end])
00801 pattern = '<%sd'%length
00802 start = end
00803 end += struct.calcsize(pattern)
00804 val1.velocities = struct.unpack(pattern, str[start:end])
00805 start = end
00806 end += 4
00807 (length,) = _struct_I.unpack(str[start:end])
00808 pattern = '<%sd'%length
00809 start = end
00810 end += struct.calcsize(pattern)
00811 val1.accelerations = struct.unpack(pattern, str[start:end])
00812 _v9 = val1.time_from_start
00813 _x = _v9
00814 start = end
00815 end += 8
00816 (_x.secs, _x.nsecs,) = _struct_2i.unpack(str[start:end])
00817 self.action_goal.goal.joint_trajectory.points.append(val1)
00818 _x = self
00819 start = end
00820 end += 8
00821 (_x.action_goal.goal.time_offset.secs, _x.action_goal.goal.time_offset.nsecs,) = _struct_2i.unpack(str[start:end])
00822 start = end
00823 end += 4
00824 (length,) = _struct_I.unpack(str[start:end])
00825 start = end
00826 end += length
00827 if python3:
00828 self.action_goal.goal.target_link = str[start:end].decode('utf-8')
00829 else:
00830 self.action_goal.goal.target_link = str[start:end]
00831 _x = self
00832 start = end
00833 end += 12
00834 (_x.action_result.header.seq, _x.action_result.header.stamp.secs, _x.action_result.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00835 start = end
00836 end += 4
00837 (length,) = _struct_I.unpack(str[start:end])
00838 start = end
00839 end += length
00840 if python3:
00841 self.action_result.header.frame_id = str[start:end].decode('utf-8')
00842 else:
00843 self.action_result.header.frame_id = str[start:end]
00844 _x = self
00845 start = end
00846 end += 8
00847 (_x.action_result.status.goal_id.stamp.secs, _x.action_result.status.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
00848 start = end
00849 end += 4
00850 (length,) = _struct_I.unpack(str[start:end])
00851 start = end
00852 end += length
00853 if python3:
00854 self.action_result.status.goal_id.id = str[start:end].decode('utf-8')
00855 else:
00856 self.action_result.status.goal_id.id = str[start:end]
00857 start = end
00858 end += 1
00859 (self.action_result.status.status,) = _struct_B.unpack(str[start:end])
00860 start = end
00861 end += 4
00862 (length,) = _struct_I.unpack(str[start:end])
00863 start = end
00864 end += length
00865 if python3:
00866 self.action_result.status.text = str[start:end].decode('utf-8')
00867 else:
00868 self.action_result.status.text = str[start:end]
00869 _x = self
00870 start = end
00871 end += 12
00872 (_x.action_result.result.actual_trajectory.header.seq, _x.action_result.result.actual_trajectory.header.stamp.secs, _x.action_result.result.actual_trajectory.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00873 start = end
00874 end += 4
00875 (length,) = _struct_I.unpack(str[start:end])
00876 start = end
00877 end += length
00878 if python3:
00879 self.action_result.result.actual_trajectory.header.frame_id = str[start:end].decode('utf-8')
00880 else:
00881 self.action_result.result.actual_trajectory.header.frame_id = str[start:end]
00882 start = end
00883 end += 4
00884 (length,) = _struct_I.unpack(str[start:end])
00885 self.action_result.result.actual_trajectory.joint_names = []
00886 for i in range(0, length):
00887 start = end
00888 end += 4
00889 (length,) = _struct_I.unpack(str[start:end])
00890 start = end
00891 end += length
00892 if python3:
00893 val1 = str[start:end].decode('utf-8')
00894 else:
00895 val1 = str[start:end]
00896 self.action_result.result.actual_trajectory.joint_names.append(val1)
00897 start = end
00898 end += 4
00899 (length,) = _struct_I.unpack(str[start:end])
00900 self.action_result.result.actual_trajectory.points = []
00901 for i in range(0, length):
00902 val1 = trajectory_msgs.msg.JointTrajectoryPoint()
00903 start = end
00904 end += 4
00905 (length,) = _struct_I.unpack(str[start:end])
00906 pattern = '<%sd'%length
00907 start = end
00908 end += struct.calcsize(pattern)
00909 val1.positions = struct.unpack(pattern, str[start:end])
00910 start = end
00911 end += 4
00912 (length,) = _struct_I.unpack(str[start:end])
00913 pattern = '<%sd'%length
00914 start = end
00915 end += struct.calcsize(pattern)
00916 val1.velocities = struct.unpack(pattern, str[start:end])
00917 start = end
00918 end += 4
00919 (length,) = _struct_I.unpack(str[start:end])
00920 pattern = '<%sd'%length
00921 start = end
00922 end += struct.calcsize(pattern)
00923 val1.accelerations = struct.unpack(pattern, str[start:end])
00924 _v10 = val1.time_from_start
00925 _x = _v10
00926 start = end
00927 end += 8
00928 (_x.secs, _x.nsecs,) = _struct_2i.unpack(str[start:end])
00929 self.action_result.result.actual_trajectory.points.append(val1)
00930 _x = self
00931 start = end
00932 end += 16
00933 (_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])
00934 start = end
00935 end += 4
00936 (length,) = _struct_I.unpack(str[start:end])
00937 start = end
00938 end += length
00939 if python3:
00940 self.action_feedback.header.frame_id = str[start:end].decode('utf-8')
00941 else:
00942 self.action_feedback.header.frame_id = str[start:end]
00943 _x = self
00944 start = end
00945 end += 8
00946 (_x.action_feedback.status.goal_id.stamp.secs, _x.action_feedback.status.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
00947 start = end
00948 end += 4
00949 (length,) = _struct_I.unpack(str[start:end])
00950 start = end
00951 end += length
00952 if python3:
00953 self.action_feedback.status.goal_id.id = str[start:end].decode('utf-8')
00954 else:
00955 self.action_feedback.status.goal_id.id = str[start:end]
00956 start = end
00957 end += 1
00958 (self.action_feedback.status.status,) = _struct_B.unpack(str[start:end])
00959 start = end
00960 end += 4
00961 (length,) = _struct_I.unpack(str[start:end])
00962 start = end
00963 end += length
00964 if python3:
00965 self.action_feedback.status.text = str[start:end].decode('utf-8')
00966 else:
00967 self.action_feedback.status.text = str[start:end]
00968 _x = self
00969 start = end
00970 end += 12
00971 (_x.action_feedback.feedback.current_state.joint_state.header.seq, _x.action_feedback.feedback.current_state.joint_state.header.stamp.secs, _x.action_feedback.feedback.current_state.joint_state.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00972 start = end
00973 end += 4
00974 (length,) = _struct_I.unpack(str[start:end])
00975 start = end
00976 end += length
00977 if python3:
00978 self.action_feedback.feedback.current_state.joint_state.header.frame_id = str[start:end].decode('utf-8')
00979 else:
00980 self.action_feedback.feedback.current_state.joint_state.header.frame_id = str[start:end]
00981 start = end
00982 end += 4
00983 (length,) = _struct_I.unpack(str[start:end])
00984 self.action_feedback.feedback.current_state.joint_state.name = []
00985 for i in range(0, length):
00986 start = end
00987 end += 4
00988 (length,) = _struct_I.unpack(str[start:end])
00989 start = end
00990 end += length
00991 if python3:
00992 val1 = str[start:end].decode('utf-8')
00993 else:
00994 val1 = str[start:end]
00995 self.action_feedback.feedback.current_state.joint_state.name.append(val1)
00996 start = end
00997 end += 4
00998 (length,) = _struct_I.unpack(str[start:end])
00999 pattern = '<%sd'%length
01000 start = end
01001 end += struct.calcsize(pattern)
01002 self.action_feedback.feedback.current_state.joint_state.position = struct.unpack(pattern, str[start:end])
01003 start = end
01004 end += 4
01005 (length,) = _struct_I.unpack(str[start:end])
01006 pattern = '<%sd'%length
01007 start = end
01008 end += struct.calcsize(pattern)
01009 self.action_feedback.feedback.current_state.joint_state.velocity = struct.unpack(pattern, str[start:end])
01010 start = end
01011 end += 4
01012 (length,) = _struct_I.unpack(str[start:end])
01013 pattern = '<%sd'%length
01014 start = end
01015 end += struct.calcsize(pattern)
01016 self.action_feedback.feedback.current_state.joint_state.effort = struct.unpack(pattern, str[start:end])
01017 _x = self
01018 start = end
01019 end += 8
01020 (_x.action_feedback.feedback.current_state.multi_dof_joint_state.stamp.secs, _x.action_feedback.feedback.current_state.multi_dof_joint_state.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
01021 start = end
01022 end += 4
01023 (length,) = _struct_I.unpack(str[start:end])
01024 self.action_feedback.feedback.current_state.multi_dof_joint_state.joint_names = []
01025 for i in range(0, length):
01026 start = end
01027 end += 4
01028 (length,) = _struct_I.unpack(str[start:end])
01029 start = end
01030 end += length
01031 if python3:
01032 val1 = str[start:end].decode('utf-8')
01033 else:
01034 val1 = str[start:end]
01035 self.action_feedback.feedback.current_state.multi_dof_joint_state.joint_names.append(val1)
01036 start = end
01037 end += 4
01038 (length,) = _struct_I.unpack(str[start:end])
01039 self.action_feedback.feedback.current_state.multi_dof_joint_state.frame_ids = []
01040 for i in range(0, length):
01041 start = end
01042 end += 4
01043 (length,) = _struct_I.unpack(str[start:end])
01044 start = end
01045 end += length
01046 if python3:
01047 val1 = str[start:end].decode('utf-8')
01048 else:
01049 val1 = str[start:end]
01050 self.action_feedback.feedback.current_state.multi_dof_joint_state.frame_ids.append(val1)
01051 start = end
01052 end += 4
01053 (length,) = _struct_I.unpack(str[start:end])
01054 self.action_feedback.feedback.current_state.multi_dof_joint_state.child_frame_ids = []
01055 for i in range(0, length):
01056 start = end
01057 end += 4
01058 (length,) = _struct_I.unpack(str[start:end])
01059 start = end
01060 end += length
01061 if python3:
01062 val1 = str[start:end].decode('utf-8')
01063 else:
01064 val1 = str[start:end]
01065 self.action_feedback.feedback.current_state.multi_dof_joint_state.child_frame_ids.append(val1)
01066 start = end
01067 end += 4
01068 (length,) = _struct_I.unpack(str[start:end])
01069 self.action_feedback.feedback.current_state.multi_dof_joint_state.poses = []
01070 for i in range(0, length):
01071 val1 = geometry_msgs.msg.Pose()
01072 _v11 = val1.position
01073 _x = _v11
01074 start = end
01075 end += 24
01076 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01077 _v12 = val1.orientation
01078 _x = _v12
01079 start = end
01080 end += 32
01081 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01082 self.action_feedback.feedback.current_state.multi_dof_joint_state.poses.append(val1)
01083 _x = self
01084 start = end
01085 end += 12
01086 (_x.action_feedback.feedback.paused_trajectory_state.joint_state.header.seq, _x.action_feedback.feedback.paused_trajectory_state.joint_state.header.stamp.secs, _x.action_feedback.feedback.paused_trajectory_state.joint_state.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01087 start = end
01088 end += 4
01089 (length,) = _struct_I.unpack(str[start:end])
01090 start = end
01091 end += length
01092 if python3:
01093 self.action_feedback.feedback.paused_trajectory_state.joint_state.header.frame_id = str[start:end].decode('utf-8')
01094 else:
01095 self.action_feedback.feedback.paused_trajectory_state.joint_state.header.frame_id = str[start:end]
01096 start = end
01097 end += 4
01098 (length,) = _struct_I.unpack(str[start:end])
01099 self.action_feedback.feedback.paused_trajectory_state.joint_state.name = []
01100 for i in range(0, length):
01101 start = end
01102 end += 4
01103 (length,) = _struct_I.unpack(str[start:end])
01104 start = end
01105 end += length
01106 if python3:
01107 val1 = str[start:end].decode('utf-8')
01108 else:
01109 val1 = str[start:end]
01110 self.action_feedback.feedback.paused_trajectory_state.joint_state.name.append(val1)
01111 start = end
01112 end += 4
01113 (length,) = _struct_I.unpack(str[start:end])
01114 pattern = '<%sd'%length
01115 start = end
01116 end += struct.calcsize(pattern)
01117 self.action_feedback.feedback.paused_trajectory_state.joint_state.position = struct.unpack(pattern, str[start:end])
01118 start = end
01119 end += 4
01120 (length,) = _struct_I.unpack(str[start:end])
01121 pattern = '<%sd'%length
01122 start = end
01123 end += struct.calcsize(pattern)
01124 self.action_feedback.feedback.paused_trajectory_state.joint_state.velocity = struct.unpack(pattern, str[start:end])
01125 start = end
01126 end += 4
01127 (length,) = _struct_I.unpack(str[start:end])
01128 pattern = '<%sd'%length
01129 start = end
01130 end += struct.calcsize(pattern)
01131 self.action_feedback.feedback.paused_trajectory_state.joint_state.effort = struct.unpack(pattern, str[start:end])
01132 _x = self
01133 start = end
01134 end += 8
01135 (_x.action_feedback.feedback.paused_trajectory_state.multi_dof_joint_state.stamp.secs, _x.action_feedback.feedback.paused_trajectory_state.multi_dof_joint_state.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
01136 start = end
01137 end += 4
01138 (length,) = _struct_I.unpack(str[start:end])
01139 self.action_feedback.feedback.paused_trajectory_state.multi_dof_joint_state.joint_names = []
01140 for i in range(0, length):
01141 start = end
01142 end += 4
01143 (length,) = _struct_I.unpack(str[start:end])
01144 start = end
01145 end += length
01146 if python3:
01147 val1 = str[start:end].decode('utf-8')
01148 else:
01149 val1 = str[start:end]
01150 self.action_feedback.feedback.paused_trajectory_state.multi_dof_joint_state.joint_names.append(val1)
01151 start = end
01152 end += 4
01153 (length,) = _struct_I.unpack(str[start:end])
01154 self.action_feedback.feedback.paused_trajectory_state.multi_dof_joint_state.frame_ids = []
01155 for i in range(0, length):
01156 start = end
01157 end += 4
01158 (length,) = _struct_I.unpack(str[start:end])
01159 start = end
01160 end += length
01161 if python3:
01162 val1 = str[start:end].decode('utf-8')
01163 else:
01164 val1 = str[start:end]
01165 self.action_feedback.feedback.paused_trajectory_state.multi_dof_joint_state.frame_ids.append(val1)
01166 start = end
01167 end += 4
01168 (length,) = _struct_I.unpack(str[start:end])
01169 self.action_feedback.feedback.paused_trajectory_state.multi_dof_joint_state.child_frame_ids = []
01170 for i in range(0, length):
01171 start = end
01172 end += 4
01173 (length,) = _struct_I.unpack(str[start:end])
01174 start = end
01175 end += length
01176 if python3:
01177 val1 = str[start:end].decode('utf-8')
01178 else:
01179 val1 = str[start:end]
01180 self.action_feedback.feedback.paused_trajectory_state.multi_dof_joint_state.child_frame_ids.append(val1)
01181 start = end
01182 end += 4
01183 (length,) = _struct_I.unpack(str[start:end])
01184 self.action_feedback.feedback.paused_trajectory_state.multi_dof_joint_state.poses = []
01185 for i in range(0, length):
01186 val1 = geometry_msgs.msg.Pose()
01187 _v13 = val1.position
01188 _x = _v13
01189 start = end
01190 end += 24
01191 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01192 _v14 = val1.orientation
01193 _x = _v14
01194 start = end
01195 end += 32
01196 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01197 self.action_feedback.feedback.paused_trajectory_state.multi_dof_joint_state.poses.append(val1)
01198 _x = self
01199 start = end
01200 end += 12
01201 (_x.action_feedback.feedback.paused_collision_map.header.seq, _x.action_feedback.feedback.paused_collision_map.header.stamp.secs, _x.action_feedback.feedback.paused_collision_map.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01202 start = end
01203 end += 4
01204 (length,) = _struct_I.unpack(str[start:end])
01205 start = end
01206 end += length
01207 if python3:
01208 self.action_feedback.feedback.paused_collision_map.header.frame_id = str[start:end].decode('utf-8')
01209 else:
01210 self.action_feedback.feedback.paused_collision_map.header.frame_id = str[start:end]
01211 start = end
01212 end += 4
01213 (length,) = _struct_I.unpack(str[start:end])
01214 start = end
01215 end += length
01216 if python3:
01217 self.action_feedback.feedback.paused_collision_map.id = str[start:end].decode('utf-8')
01218 else:
01219 self.action_feedback.feedback.paused_collision_map.id = str[start:end]
01220 _x = self
01221 start = end
01222 end += 5
01223 (_x.action_feedback.feedback.paused_collision_map.padding, _x.action_feedback.feedback.paused_collision_map.operation.operation,) = _struct_fb.unpack(str[start:end])
01224 start = end
01225 end += 4
01226 (length,) = _struct_I.unpack(str[start:end])
01227 self.action_feedback.feedback.paused_collision_map.shapes = []
01228 for i in range(0, length):
01229 val1 = arm_navigation_msgs.msg.Shape()
01230 start = end
01231 end += 1
01232 (val1.type,) = _struct_b.unpack(str[start:end])
01233 start = end
01234 end += 4
01235 (length,) = _struct_I.unpack(str[start:end])
01236 pattern = '<%sd'%length
01237 start = end
01238 end += struct.calcsize(pattern)
01239 val1.dimensions = struct.unpack(pattern, str[start:end])
01240 start = end
01241 end += 4
01242 (length,) = _struct_I.unpack(str[start:end])
01243 pattern = '<%si'%length
01244 start = end
01245 end += struct.calcsize(pattern)
01246 val1.triangles = struct.unpack(pattern, str[start:end])
01247 start = end
01248 end += 4
01249 (length,) = _struct_I.unpack(str[start:end])
01250 val1.vertices = []
01251 for i in range(0, length):
01252 val2 = geometry_msgs.msg.Point()
01253 _x = val2
01254 start = end
01255 end += 24
01256 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01257 val1.vertices.append(val2)
01258 self.action_feedback.feedback.paused_collision_map.shapes.append(val1)
01259 start = end
01260 end += 4
01261 (length,) = _struct_I.unpack(str[start:end])
01262 self.action_feedback.feedback.paused_collision_map.poses = []
01263 for i in range(0, length):
01264 val1 = geometry_msgs.msg.Pose()
01265 _v15 = val1.position
01266 _x = _v15
01267 start = end
01268 end += 24
01269 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01270 _v16 = val1.orientation
01271 _x = _v16
01272 start = end
01273 end += 32
01274 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01275 self.action_feedback.feedback.paused_collision_map.poses.append(val1)
01276 return self
01277 except struct.error as e:
01278 raise genpy.DeserializationError(e)
01279
01280
01281 def serialize_numpy(self, buff, numpy):
01282 """
01283 serialize message with numpy array types into buffer
01284 :param buff: buffer, ``StringIO``
01285 :param numpy: numpy python module
01286 """
01287 try:
01288 _x = self
01289 buff.write(_struct_3I.pack(_x.action_goal.header.seq, _x.action_goal.header.stamp.secs, _x.action_goal.header.stamp.nsecs))
01290 _x = self.action_goal.header.frame_id
01291 length = len(_x)
01292 if python3 or type(_x) == unicode:
01293 _x = _x.encode('utf-8')
01294 length = len(_x)
01295 buff.write(struct.pack('<I%ss'%length, length, _x))
01296 _x = self
01297 buff.write(_struct_2I.pack(_x.action_goal.goal_id.stamp.secs, _x.action_goal.goal_id.stamp.nsecs))
01298 _x = self.action_goal.goal_id.id
01299 length = len(_x)
01300 if python3 or type(_x) == unicode:
01301 _x = _x.encode('utf-8')
01302 length = len(_x)
01303 buff.write(struct.pack('<I%ss'%length, length, _x))
01304 _x = self.action_goal.goal.group_name
01305 length = len(_x)
01306 if python3 or type(_x) == unicode:
01307 _x = _x.encode('utf-8')
01308 length = len(_x)
01309 buff.write(struct.pack('<I%ss'%length, length, _x))
01310 _x = self
01311 buff.write(_struct_3I.pack(_x.action_goal.goal.joint_trajectory.header.seq, _x.action_goal.goal.joint_trajectory.header.stamp.secs, _x.action_goal.goal.joint_trajectory.header.stamp.nsecs))
01312 _x = self.action_goal.goal.joint_trajectory.header.frame_id
01313 length = len(_x)
01314 if python3 or type(_x) == unicode:
01315 _x = _x.encode('utf-8')
01316 length = len(_x)
01317 buff.write(struct.pack('<I%ss'%length, length, _x))
01318 length = len(self.action_goal.goal.joint_trajectory.joint_names)
01319 buff.write(_struct_I.pack(length))
01320 for val1 in self.action_goal.goal.joint_trajectory.joint_names:
01321 length = len(val1)
01322 if python3 or type(val1) == unicode:
01323 val1 = val1.encode('utf-8')
01324 length = len(val1)
01325 buff.write(struct.pack('<I%ss'%length, length, val1))
01326 length = len(self.action_goal.goal.joint_trajectory.points)
01327 buff.write(_struct_I.pack(length))
01328 for val1 in self.action_goal.goal.joint_trajectory.points:
01329 length = len(val1.positions)
01330 buff.write(_struct_I.pack(length))
01331 pattern = '<%sd'%length
01332 buff.write(val1.positions.tostring())
01333 length = len(val1.velocities)
01334 buff.write(_struct_I.pack(length))
01335 pattern = '<%sd'%length
01336 buff.write(val1.velocities.tostring())
01337 length = len(val1.accelerations)
01338 buff.write(_struct_I.pack(length))
01339 pattern = '<%sd'%length
01340 buff.write(val1.accelerations.tostring())
01341 _v17 = val1.time_from_start
01342 _x = _v17
01343 buff.write(_struct_2i.pack(_x.secs, _x.nsecs))
01344 _x = self
01345 buff.write(_struct_2i.pack(_x.action_goal.goal.time_offset.secs, _x.action_goal.goal.time_offset.nsecs))
01346 _x = self.action_goal.goal.target_link
01347 length = len(_x)
01348 if python3 or type(_x) == unicode:
01349 _x = _x.encode('utf-8')
01350 length = len(_x)
01351 buff.write(struct.pack('<I%ss'%length, length, _x))
01352 _x = self
01353 buff.write(_struct_3I.pack(_x.action_result.header.seq, _x.action_result.header.stamp.secs, _x.action_result.header.stamp.nsecs))
01354 _x = self.action_result.header.frame_id
01355 length = len(_x)
01356 if python3 or type(_x) == unicode:
01357 _x = _x.encode('utf-8')
01358 length = len(_x)
01359 buff.write(struct.pack('<I%ss'%length, length, _x))
01360 _x = self
01361 buff.write(_struct_2I.pack(_x.action_result.status.goal_id.stamp.secs, _x.action_result.status.goal_id.stamp.nsecs))
01362 _x = self.action_result.status.goal_id.id
01363 length = len(_x)
01364 if python3 or type(_x) == unicode:
01365 _x = _x.encode('utf-8')
01366 length = len(_x)
01367 buff.write(struct.pack('<I%ss'%length, length, _x))
01368 buff.write(_struct_B.pack(self.action_result.status.status))
01369 _x = self.action_result.status.text
01370 length = len(_x)
01371 if python3 or type(_x) == unicode:
01372 _x = _x.encode('utf-8')
01373 length = len(_x)
01374 buff.write(struct.pack('<I%ss'%length, length, _x))
01375 _x = self
01376 buff.write(_struct_3I.pack(_x.action_result.result.actual_trajectory.header.seq, _x.action_result.result.actual_trajectory.header.stamp.secs, _x.action_result.result.actual_trajectory.header.stamp.nsecs))
01377 _x = self.action_result.result.actual_trajectory.header.frame_id
01378 length = len(_x)
01379 if python3 or type(_x) == unicode:
01380 _x = _x.encode('utf-8')
01381 length = len(_x)
01382 buff.write(struct.pack('<I%ss'%length, length, _x))
01383 length = len(self.action_result.result.actual_trajectory.joint_names)
01384 buff.write(_struct_I.pack(length))
01385 for val1 in self.action_result.result.actual_trajectory.joint_names:
01386 length = len(val1)
01387 if python3 or type(val1) == unicode:
01388 val1 = val1.encode('utf-8')
01389 length = len(val1)
01390 buff.write(struct.pack('<I%ss'%length, length, val1))
01391 length = len(self.action_result.result.actual_trajectory.points)
01392 buff.write(_struct_I.pack(length))
01393 for val1 in self.action_result.result.actual_trajectory.points:
01394 length = len(val1.positions)
01395 buff.write(_struct_I.pack(length))
01396 pattern = '<%sd'%length
01397 buff.write(val1.positions.tostring())
01398 length = len(val1.velocities)
01399 buff.write(_struct_I.pack(length))
01400 pattern = '<%sd'%length
01401 buff.write(val1.velocities.tostring())
01402 length = len(val1.accelerations)
01403 buff.write(_struct_I.pack(length))
01404 pattern = '<%sd'%length
01405 buff.write(val1.accelerations.tostring())
01406 _v18 = val1.time_from_start
01407 _x = _v18
01408 buff.write(_struct_2i.pack(_x.secs, _x.nsecs))
01409 _x = self
01410 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))
01411 _x = self.action_feedback.header.frame_id
01412 length = len(_x)
01413 if python3 or type(_x) == unicode:
01414 _x = _x.encode('utf-8')
01415 length = len(_x)
01416 buff.write(struct.pack('<I%ss'%length, length, _x))
01417 _x = self
01418 buff.write(_struct_2I.pack(_x.action_feedback.status.goal_id.stamp.secs, _x.action_feedback.status.goal_id.stamp.nsecs))
01419 _x = self.action_feedback.status.goal_id.id
01420 length = len(_x)
01421 if python3 or type(_x) == unicode:
01422 _x = _x.encode('utf-8')
01423 length = len(_x)
01424 buff.write(struct.pack('<I%ss'%length, length, _x))
01425 buff.write(_struct_B.pack(self.action_feedback.status.status))
01426 _x = self.action_feedback.status.text
01427 length = len(_x)
01428 if python3 or type(_x) == unicode:
01429 _x = _x.encode('utf-8')
01430 length = len(_x)
01431 buff.write(struct.pack('<I%ss'%length, length, _x))
01432 _x = self
01433 buff.write(_struct_3I.pack(_x.action_feedback.feedback.current_state.joint_state.header.seq, _x.action_feedback.feedback.current_state.joint_state.header.stamp.secs, _x.action_feedback.feedback.current_state.joint_state.header.stamp.nsecs))
01434 _x = self.action_feedback.feedback.current_state.joint_state.header.frame_id
01435 length = len(_x)
01436 if python3 or type(_x) == unicode:
01437 _x = _x.encode('utf-8')
01438 length = len(_x)
01439 buff.write(struct.pack('<I%ss'%length, length, _x))
01440 length = len(self.action_feedback.feedback.current_state.joint_state.name)
01441 buff.write(_struct_I.pack(length))
01442 for val1 in self.action_feedback.feedback.current_state.joint_state.name:
01443 length = len(val1)
01444 if python3 or type(val1) == unicode:
01445 val1 = val1.encode('utf-8')
01446 length = len(val1)
01447 buff.write(struct.pack('<I%ss'%length, length, val1))
01448 length = len(self.action_feedback.feedback.current_state.joint_state.position)
01449 buff.write(_struct_I.pack(length))
01450 pattern = '<%sd'%length
01451 buff.write(self.action_feedback.feedback.current_state.joint_state.position.tostring())
01452 length = len(self.action_feedback.feedback.current_state.joint_state.velocity)
01453 buff.write(_struct_I.pack(length))
01454 pattern = '<%sd'%length
01455 buff.write(self.action_feedback.feedback.current_state.joint_state.velocity.tostring())
01456 length = len(self.action_feedback.feedback.current_state.joint_state.effort)
01457 buff.write(_struct_I.pack(length))
01458 pattern = '<%sd'%length
01459 buff.write(self.action_feedback.feedback.current_state.joint_state.effort.tostring())
01460 _x = self
01461 buff.write(_struct_2I.pack(_x.action_feedback.feedback.current_state.multi_dof_joint_state.stamp.secs, _x.action_feedback.feedback.current_state.multi_dof_joint_state.stamp.nsecs))
01462 length = len(self.action_feedback.feedback.current_state.multi_dof_joint_state.joint_names)
01463 buff.write(_struct_I.pack(length))
01464 for val1 in self.action_feedback.feedback.current_state.multi_dof_joint_state.joint_names:
01465 length = len(val1)
01466 if python3 or type(val1) == unicode:
01467 val1 = val1.encode('utf-8')
01468 length = len(val1)
01469 buff.write(struct.pack('<I%ss'%length, length, val1))
01470 length = len(self.action_feedback.feedback.current_state.multi_dof_joint_state.frame_ids)
01471 buff.write(_struct_I.pack(length))
01472 for val1 in self.action_feedback.feedback.current_state.multi_dof_joint_state.frame_ids:
01473 length = len(val1)
01474 if python3 or type(val1) == unicode:
01475 val1 = val1.encode('utf-8')
01476 length = len(val1)
01477 buff.write(struct.pack('<I%ss'%length, length, val1))
01478 length = len(self.action_feedback.feedback.current_state.multi_dof_joint_state.child_frame_ids)
01479 buff.write(_struct_I.pack(length))
01480 for val1 in self.action_feedback.feedback.current_state.multi_dof_joint_state.child_frame_ids:
01481 length = len(val1)
01482 if python3 or type(val1) == unicode:
01483 val1 = val1.encode('utf-8')
01484 length = len(val1)
01485 buff.write(struct.pack('<I%ss'%length, length, val1))
01486 length = len(self.action_feedback.feedback.current_state.multi_dof_joint_state.poses)
01487 buff.write(_struct_I.pack(length))
01488 for val1 in self.action_feedback.feedback.current_state.multi_dof_joint_state.poses:
01489 _v19 = val1.position
01490 _x = _v19
01491 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01492 _v20 = val1.orientation
01493 _x = _v20
01494 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01495 _x = self
01496 buff.write(_struct_3I.pack(_x.action_feedback.feedback.paused_trajectory_state.joint_state.header.seq, _x.action_feedback.feedback.paused_trajectory_state.joint_state.header.stamp.secs, _x.action_feedback.feedback.paused_trajectory_state.joint_state.header.stamp.nsecs))
01497 _x = self.action_feedback.feedback.paused_trajectory_state.joint_state.header.frame_id
01498 length = len(_x)
01499 if python3 or type(_x) == unicode:
01500 _x = _x.encode('utf-8')
01501 length = len(_x)
01502 buff.write(struct.pack('<I%ss'%length, length, _x))
01503 length = len(self.action_feedback.feedback.paused_trajectory_state.joint_state.name)
01504 buff.write(_struct_I.pack(length))
01505 for val1 in self.action_feedback.feedback.paused_trajectory_state.joint_state.name:
01506 length = len(val1)
01507 if python3 or type(val1) == unicode:
01508 val1 = val1.encode('utf-8')
01509 length = len(val1)
01510 buff.write(struct.pack('<I%ss'%length, length, val1))
01511 length = len(self.action_feedback.feedback.paused_trajectory_state.joint_state.position)
01512 buff.write(_struct_I.pack(length))
01513 pattern = '<%sd'%length
01514 buff.write(self.action_feedback.feedback.paused_trajectory_state.joint_state.position.tostring())
01515 length = len(self.action_feedback.feedback.paused_trajectory_state.joint_state.velocity)
01516 buff.write(_struct_I.pack(length))
01517 pattern = '<%sd'%length
01518 buff.write(self.action_feedback.feedback.paused_trajectory_state.joint_state.velocity.tostring())
01519 length = len(self.action_feedback.feedback.paused_trajectory_state.joint_state.effort)
01520 buff.write(_struct_I.pack(length))
01521 pattern = '<%sd'%length
01522 buff.write(self.action_feedback.feedback.paused_trajectory_state.joint_state.effort.tostring())
01523 _x = self
01524 buff.write(_struct_2I.pack(_x.action_feedback.feedback.paused_trajectory_state.multi_dof_joint_state.stamp.secs, _x.action_feedback.feedback.paused_trajectory_state.multi_dof_joint_state.stamp.nsecs))
01525 length = len(self.action_feedback.feedback.paused_trajectory_state.multi_dof_joint_state.joint_names)
01526 buff.write(_struct_I.pack(length))
01527 for val1 in self.action_feedback.feedback.paused_trajectory_state.multi_dof_joint_state.joint_names:
01528 length = len(val1)
01529 if python3 or type(val1) == unicode:
01530 val1 = val1.encode('utf-8')
01531 length = len(val1)
01532 buff.write(struct.pack('<I%ss'%length, length, val1))
01533 length = len(self.action_feedback.feedback.paused_trajectory_state.multi_dof_joint_state.frame_ids)
01534 buff.write(_struct_I.pack(length))
01535 for val1 in self.action_feedback.feedback.paused_trajectory_state.multi_dof_joint_state.frame_ids:
01536 length = len(val1)
01537 if python3 or type(val1) == unicode:
01538 val1 = val1.encode('utf-8')
01539 length = len(val1)
01540 buff.write(struct.pack('<I%ss'%length, length, val1))
01541 length = len(self.action_feedback.feedback.paused_trajectory_state.multi_dof_joint_state.child_frame_ids)
01542 buff.write(_struct_I.pack(length))
01543 for val1 in self.action_feedback.feedback.paused_trajectory_state.multi_dof_joint_state.child_frame_ids:
01544 length = len(val1)
01545 if python3 or type(val1) == unicode:
01546 val1 = val1.encode('utf-8')
01547 length = len(val1)
01548 buff.write(struct.pack('<I%ss'%length, length, val1))
01549 length = len(self.action_feedback.feedback.paused_trajectory_state.multi_dof_joint_state.poses)
01550 buff.write(_struct_I.pack(length))
01551 for val1 in self.action_feedback.feedback.paused_trajectory_state.multi_dof_joint_state.poses:
01552 _v21 = val1.position
01553 _x = _v21
01554 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01555 _v22 = val1.orientation
01556 _x = _v22
01557 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01558 _x = self
01559 buff.write(_struct_3I.pack(_x.action_feedback.feedback.paused_collision_map.header.seq, _x.action_feedback.feedback.paused_collision_map.header.stamp.secs, _x.action_feedback.feedback.paused_collision_map.header.stamp.nsecs))
01560 _x = self.action_feedback.feedback.paused_collision_map.header.frame_id
01561 length = len(_x)
01562 if python3 or type(_x) == unicode:
01563 _x = _x.encode('utf-8')
01564 length = len(_x)
01565 buff.write(struct.pack('<I%ss'%length, length, _x))
01566 _x = self.action_feedback.feedback.paused_collision_map.id
01567 length = len(_x)
01568 if python3 or type(_x) == unicode:
01569 _x = _x.encode('utf-8')
01570 length = len(_x)
01571 buff.write(struct.pack('<I%ss'%length, length, _x))
01572 _x = self
01573 buff.write(_struct_fb.pack(_x.action_feedback.feedback.paused_collision_map.padding, _x.action_feedback.feedback.paused_collision_map.operation.operation))
01574 length = len(self.action_feedback.feedback.paused_collision_map.shapes)
01575 buff.write(_struct_I.pack(length))
01576 for val1 in self.action_feedback.feedback.paused_collision_map.shapes:
01577 buff.write(_struct_b.pack(val1.type))
01578 length = len(val1.dimensions)
01579 buff.write(_struct_I.pack(length))
01580 pattern = '<%sd'%length
01581 buff.write(val1.dimensions.tostring())
01582 length = len(val1.triangles)
01583 buff.write(_struct_I.pack(length))
01584 pattern = '<%si'%length
01585 buff.write(val1.triangles.tostring())
01586 length = len(val1.vertices)
01587 buff.write(_struct_I.pack(length))
01588 for val2 in val1.vertices:
01589 _x = val2
01590 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01591 length = len(self.action_feedback.feedback.paused_collision_map.poses)
01592 buff.write(_struct_I.pack(length))
01593 for val1 in self.action_feedback.feedback.paused_collision_map.poses:
01594 _v23 = val1.position
01595 _x = _v23
01596 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01597 _v24 = val1.orientation
01598 _x = _v24
01599 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01600 except struct.error as se: self._check_types(se)
01601 except TypeError as te: self._check_types(te)
01602
01603 def deserialize_numpy(self, str, numpy):
01604 """
01605 unpack serialized message in str into this message instance using numpy for array types
01606 :param str: byte array of serialized message, ``str``
01607 :param numpy: numpy python module
01608 """
01609 try:
01610 if self.action_goal is None:
01611 self.action_goal = head_monitor_msgs.msg.HeadMonitorActionGoal()
01612 if self.action_result is None:
01613 self.action_result = head_monitor_msgs.msg.HeadMonitorActionResult()
01614 if self.action_feedback is None:
01615 self.action_feedback = head_monitor_msgs.msg.HeadMonitorActionFeedback()
01616 end = 0
01617 _x = self
01618 start = end
01619 end += 12
01620 (_x.action_goal.header.seq, _x.action_goal.header.stamp.secs, _x.action_goal.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01621 start = end
01622 end += 4
01623 (length,) = _struct_I.unpack(str[start:end])
01624 start = end
01625 end += length
01626 if python3:
01627 self.action_goal.header.frame_id = str[start:end].decode('utf-8')
01628 else:
01629 self.action_goal.header.frame_id = str[start:end]
01630 _x = self
01631 start = end
01632 end += 8
01633 (_x.action_goal.goal_id.stamp.secs, _x.action_goal.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
01634 start = end
01635 end += 4
01636 (length,) = _struct_I.unpack(str[start:end])
01637 start = end
01638 end += length
01639 if python3:
01640 self.action_goal.goal_id.id = str[start:end].decode('utf-8')
01641 else:
01642 self.action_goal.goal_id.id = str[start:end]
01643 start = end
01644 end += 4
01645 (length,) = _struct_I.unpack(str[start:end])
01646 start = end
01647 end += length
01648 if python3:
01649 self.action_goal.goal.group_name = str[start:end].decode('utf-8')
01650 else:
01651 self.action_goal.goal.group_name = str[start:end]
01652 _x = self
01653 start = end
01654 end += 12
01655 (_x.action_goal.goal.joint_trajectory.header.seq, _x.action_goal.goal.joint_trajectory.header.stamp.secs, _x.action_goal.goal.joint_trajectory.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01656 start = end
01657 end += 4
01658 (length,) = _struct_I.unpack(str[start:end])
01659 start = end
01660 end += length
01661 if python3:
01662 self.action_goal.goal.joint_trajectory.header.frame_id = str[start:end].decode('utf-8')
01663 else:
01664 self.action_goal.goal.joint_trajectory.header.frame_id = str[start:end]
01665 start = end
01666 end += 4
01667 (length,) = _struct_I.unpack(str[start:end])
01668 self.action_goal.goal.joint_trajectory.joint_names = []
01669 for i in range(0, length):
01670 start = end
01671 end += 4
01672 (length,) = _struct_I.unpack(str[start:end])
01673 start = end
01674 end += length
01675 if python3:
01676 val1 = str[start:end].decode('utf-8')
01677 else:
01678 val1 = str[start:end]
01679 self.action_goal.goal.joint_trajectory.joint_names.append(val1)
01680 start = end
01681 end += 4
01682 (length,) = _struct_I.unpack(str[start:end])
01683 self.action_goal.goal.joint_trajectory.points = []
01684 for i in range(0, length):
01685 val1 = trajectory_msgs.msg.JointTrajectoryPoint()
01686 start = end
01687 end += 4
01688 (length,) = _struct_I.unpack(str[start:end])
01689 pattern = '<%sd'%length
01690 start = end
01691 end += struct.calcsize(pattern)
01692 val1.positions = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01693 start = end
01694 end += 4
01695 (length,) = _struct_I.unpack(str[start:end])
01696 pattern = '<%sd'%length
01697 start = end
01698 end += struct.calcsize(pattern)
01699 val1.velocities = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01700 start = end
01701 end += 4
01702 (length,) = _struct_I.unpack(str[start:end])
01703 pattern = '<%sd'%length
01704 start = end
01705 end += struct.calcsize(pattern)
01706 val1.accelerations = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01707 _v25 = val1.time_from_start
01708 _x = _v25
01709 start = end
01710 end += 8
01711 (_x.secs, _x.nsecs,) = _struct_2i.unpack(str[start:end])
01712 self.action_goal.goal.joint_trajectory.points.append(val1)
01713 _x = self
01714 start = end
01715 end += 8
01716 (_x.action_goal.goal.time_offset.secs, _x.action_goal.goal.time_offset.nsecs,) = _struct_2i.unpack(str[start:end])
01717 start = end
01718 end += 4
01719 (length,) = _struct_I.unpack(str[start:end])
01720 start = end
01721 end += length
01722 if python3:
01723 self.action_goal.goal.target_link = str[start:end].decode('utf-8')
01724 else:
01725 self.action_goal.goal.target_link = str[start:end]
01726 _x = self
01727 start = end
01728 end += 12
01729 (_x.action_result.header.seq, _x.action_result.header.stamp.secs, _x.action_result.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01730 start = end
01731 end += 4
01732 (length,) = _struct_I.unpack(str[start:end])
01733 start = end
01734 end += length
01735 if python3:
01736 self.action_result.header.frame_id = str[start:end].decode('utf-8')
01737 else:
01738 self.action_result.header.frame_id = str[start:end]
01739 _x = self
01740 start = end
01741 end += 8
01742 (_x.action_result.status.goal_id.stamp.secs, _x.action_result.status.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
01743 start = end
01744 end += 4
01745 (length,) = _struct_I.unpack(str[start:end])
01746 start = end
01747 end += length
01748 if python3:
01749 self.action_result.status.goal_id.id = str[start:end].decode('utf-8')
01750 else:
01751 self.action_result.status.goal_id.id = str[start:end]
01752 start = end
01753 end += 1
01754 (self.action_result.status.status,) = _struct_B.unpack(str[start:end])
01755 start = end
01756 end += 4
01757 (length,) = _struct_I.unpack(str[start:end])
01758 start = end
01759 end += length
01760 if python3:
01761 self.action_result.status.text = str[start:end].decode('utf-8')
01762 else:
01763 self.action_result.status.text = str[start:end]
01764 _x = self
01765 start = end
01766 end += 12
01767 (_x.action_result.result.actual_trajectory.header.seq, _x.action_result.result.actual_trajectory.header.stamp.secs, _x.action_result.result.actual_trajectory.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01768 start = end
01769 end += 4
01770 (length,) = _struct_I.unpack(str[start:end])
01771 start = end
01772 end += length
01773 if python3:
01774 self.action_result.result.actual_trajectory.header.frame_id = str[start:end].decode('utf-8')
01775 else:
01776 self.action_result.result.actual_trajectory.header.frame_id = str[start:end]
01777 start = end
01778 end += 4
01779 (length,) = _struct_I.unpack(str[start:end])
01780 self.action_result.result.actual_trajectory.joint_names = []
01781 for i in range(0, length):
01782 start = end
01783 end += 4
01784 (length,) = _struct_I.unpack(str[start:end])
01785 start = end
01786 end += length
01787 if python3:
01788 val1 = str[start:end].decode('utf-8')
01789 else:
01790 val1 = str[start:end]
01791 self.action_result.result.actual_trajectory.joint_names.append(val1)
01792 start = end
01793 end += 4
01794 (length,) = _struct_I.unpack(str[start:end])
01795 self.action_result.result.actual_trajectory.points = []
01796 for i in range(0, length):
01797 val1 = trajectory_msgs.msg.JointTrajectoryPoint()
01798 start = end
01799 end += 4
01800 (length,) = _struct_I.unpack(str[start:end])
01801 pattern = '<%sd'%length
01802 start = end
01803 end += struct.calcsize(pattern)
01804 val1.positions = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01805 start = end
01806 end += 4
01807 (length,) = _struct_I.unpack(str[start:end])
01808 pattern = '<%sd'%length
01809 start = end
01810 end += struct.calcsize(pattern)
01811 val1.velocities = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01812 start = end
01813 end += 4
01814 (length,) = _struct_I.unpack(str[start:end])
01815 pattern = '<%sd'%length
01816 start = end
01817 end += struct.calcsize(pattern)
01818 val1.accelerations = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01819 _v26 = val1.time_from_start
01820 _x = _v26
01821 start = end
01822 end += 8
01823 (_x.secs, _x.nsecs,) = _struct_2i.unpack(str[start:end])
01824 self.action_result.result.actual_trajectory.points.append(val1)
01825 _x = self
01826 start = end
01827 end += 16
01828 (_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])
01829 start = end
01830 end += 4
01831 (length,) = _struct_I.unpack(str[start:end])
01832 start = end
01833 end += length
01834 if python3:
01835 self.action_feedback.header.frame_id = str[start:end].decode('utf-8')
01836 else:
01837 self.action_feedback.header.frame_id = str[start:end]
01838 _x = self
01839 start = end
01840 end += 8
01841 (_x.action_feedback.status.goal_id.stamp.secs, _x.action_feedback.status.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
01842 start = end
01843 end += 4
01844 (length,) = _struct_I.unpack(str[start:end])
01845 start = end
01846 end += length
01847 if python3:
01848 self.action_feedback.status.goal_id.id = str[start:end].decode('utf-8')
01849 else:
01850 self.action_feedback.status.goal_id.id = str[start:end]
01851 start = end
01852 end += 1
01853 (self.action_feedback.status.status,) = _struct_B.unpack(str[start:end])
01854 start = end
01855 end += 4
01856 (length,) = _struct_I.unpack(str[start:end])
01857 start = end
01858 end += length
01859 if python3:
01860 self.action_feedback.status.text = str[start:end].decode('utf-8')
01861 else:
01862 self.action_feedback.status.text = str[start:end]
01863 _x = self
01864 start = end
01865 end += 12
01866 (_x.action_feedback.feedback.current_state.joint_state.header.seq, _x.action_feedback.feedback.current_state.joint_state.header.stamp.secs, _x.action_feedback.feedback.current_state.joint_state.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01867 start = end
01868 end += 4
01869 (length,) = _struct_I.unpack(str[start:end])
01870 start = end
01871 end += length
01872 if python3:
01873 self.action_feedback.feedback.current_state.joint_state.header.frame_id = str[start:end].decode('utf-8')
01874 else:
01875 self.action_feedback.feedback.current_state.joint_state.header.frame_id = str[start:end]
01876 start = end
01877 end += 4
01878 (length,) = _struct_I.unpack(str[start:end])
01879 self.action_feedback.feedback.current_state.joint_state.name = []
01880 for i in range(0, length):
01881 start = end
01882 end += 4
01883 (length,) = _struct_I.unpack(str[start:end])
01884 start = end
01885 end += length
01886 if python3:
01887 val1 = str[start:end].decode('utf-8')
01888 else:
01889 val1 = str[start:end]
01890 self.action_feedback.feedback.current_state.joint_state.name.append(val1)
01891 start = end
01892 end += 4
01893 (length,) = _struct_I.unpack(str[start:end])
01894 pattern = '<%sd'%length
01895 start = end
01896 end += struct.calcsize(pattern)
01897 self.action_feedback.feedback.current_state.joint_state.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01898 start = end
01899 end += 4
01900 (length,) = _struct_I.unpack(str[start:end])
01901 pattern = '<%sd'%length
01902 start = end
01903 end += struct.calcsize(pattern)
01904 self.action_feedback.feedback.current_state.joint_state.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01905 start = end
01906 end += 4
01907 (length,) = _struct_I.unpack(str[start:end])
01908 pattern = '<%sd'%length
01909 start = end
01910 end += struct.calcsize(pattern)
01911 self.action_feedback.feedback.current_state.joint_state.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01912 _x = self
01913 start = end
01914 end += 8
01915 (_x.action_feedback.feedback.current_state.multi_dof_joint_state.stamp.secs, _x.action_feedback.feedback.current_state.multi_dof_joint_state.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
01916 start = end
01917 end += 4
01918 (length,) = _struct_I.unpack(str[start:end])
01919 self.action_feedback.feedback.current_state.multi_dof_joint_state.joint_names = []
01920 for i in range(0, length):
01921 start = end
01922 end += 4
01923 (length,) = _struct_I.unpack(str[start:end])
01924 start = end
01925 end += length
01926 if python3:
01927 val1 = str[start:end].decode('utf-8')
01928 else:
01929 val1 = str[start:end]
01930 self.action_feedback.feedback.current_state.multi_dof_joint_state.joint_names.append(val1)
01931 start = end
01932 end += 4
01933 (length,) = _struct_I.unpack(str[start:end])
01934 self.action_feedback.feedback.current_state.multi_dof_joint_state.frame_ids = []
01935 for i in range(0, length):
01936 start = end
01937 end += 4
01938 (length,) = _struct_I.unpack(str[start:end])
01939 start = end
01940 end += length
01941 if python3:
01942 val1 = str[start:end].decode('utf-8')
01943 else:
01944 val1 = str[start:end]
01945 self.action_feedback.feedback.current_state.multi_dof_joint_state.frame_ids.append(val1)
01946 start = end
01947 end += 4
01948 (length,) = _struct_I.unpack(str[start:end])
01949 self.action_feedback.feedback.current_state.multi_dof_joint_state.child_frame_ids = []
01950 for i in range(0, length):
01951 start = end
01952 end += 4
01953 (length,) = _struct_I.unpack(str[start:end])
01954 start = end
01955 end += length
01956 if python3:
01957 val1 = str[start:end].decode('utf-8')
01958 else:
01959 val1 = str[start:end]
01960 self.action_feedback.feedback.current_state.multi_dof_joint_state.child_frame_ids.append(val1)
01961 start = end
01962 end += 4
01963 (length,) = _struct_I.unpack(str[start:end])
01964 self.action_feedback.feedback.current_state.multi_dof_joint_state.poses = []
01965 for i in range(0, length):
01966 val1 = geometry_msgs.msg.Pose()
01967 _v27 = val1.position
01968 _x = _v27
01969 start = end
01970 end += 24
01971 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01972 _v28 = val1.orientation
01973 _x = _v28
01974 start = end
01975 end += 32
01976 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01977 self.action_feedback.feedback.current_state.multi_dof_joint_state.poses.append(val1)
01978 _x = self
01979 start = end
01980 end += 12
01981 (_x.action_feedback.feedback.paused_trajectory_state.joint_state.header.seq, _x.action_feedback.feedback.paused_trajectory_state.joint_state.header.stamp.secs, _x.action_feedback.feedback.paused_trajectory_state.joint_state.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01982 start = end
01983 end += 4
01984 (length,) = _struct_I.unpack(str[start:end])
01985 start = end
01986 end += length
01987 if python3:
01988 self.action_feedback.feedback.paused_trajectory_state.joint_state.header.frame_id = str[start:end].decode('utf-8')
01989 else:
01990 self.action_feedback.feedback.paused_trajectory_state.joint_state.header.frame_id = str[start:end]
01991 start = end
01992 end += 4
01993 (length,) = _struct_I.unpack(str[start:end])
01994 self.action_feedback.feedback.paused_trajectory_state.joint_state.name = []
01995 for i in range(0, length):
01996 start = end
01997 end += 4
01998 (length,) = _struct_I.unpack(str[start:end])
01999 start = end
02000 end += length
02001 if python3:
02002 val1 = str[start:end].decode('utf-8')
02003 else:
02004 val1 = str[start:end]
02005 self.action_feedback.feedback.paused_trajectory_state.joint_state.name.append(val1)
02006 start = end
02007 end += 4
02008 (length,) = _struct_I.unpack(str[start:end])
02009 pattern = '<%sd'%length
02010 start = end
02011 end += struct.calcsize(pattern)
02012 self.action_feedback.feedback.paused_trajectory_state.joint_state.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
02013 start = end
02014 end += 4
02015 (length,) = _struct_I.unpack(str[start:end])
02016 pattern = '<%sd'%length
02017 start = end
02018 end += struct.calcsize(pattern)
02019 self.action_feedback.feedback.paused_trajectory_state.joint_state.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
02020 start = end
02021 end += 4
02022 (length,) = _struct_I.unpack(str[start:end])
02023 pattern = '<%sd'%length
02024 start = end
02025 end += struct.calcsize(pattern)
02026 self.action_feedback.feedback.paused_trajectory_state.joint_state.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
02027 _x = self
02028 start = end
02029 end += 8
02030 (_x.action_feedback.feedback.paused_trajectory_state.multi_dof_joint_state.stamp.secs, _x.action_feedback.feedback.paused_trajectory_state.multi_dof_joint_state.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
02031 start = end
02032 end += 4
02033 (length,) = _struct_I.unpack(str[start:end])
02034 self.action_feedback.feedback.paused_trajectory_state.multi_dof_joint_state.joint_names = []
02035 for i in range(0, length):
02036 start = end
02037 end += 4
02038 (length,) = _struct_I.unpack(str[start:end])
02039 start = end
02040 end += length
02041 if python3:
02042 val1 = str[start:end].decode('utf-8')
02043 else:
02044 val1 = str[start:end]
02045 self.action_feedback.feedback.paused_trajectory_state.multi_dof_joint_state.joint_names.append(val1)
02046 start = end
02047 end += 4
02048 (length,) = _struct_I.unpack(str[start:end])
02049 self.action_feedback.feedback.paused_trajectory_state.multi_dof_joint_state.frame_ids = []
02050 for i in range(0, length):
02051 start = end
02052 end += 4
02053 (length,) = _struct_I.unpack(str[start:end])
02054 start = end
02055 end += length
02056 if python3:
02057 val1 = str[start:end].decode('utf-8')
02058 else:
02059 val1 = str[start:end]
02060 self.action_feedback.feedback.paused_trajectory_state.multi_dof_joint_state.frame_ids.append(val1)
02061 start = end
02062 end += 4
02063 (length,) = _struct_I.unpack(str[start:end])
02064 self.action_feedback.feedback.paused_trajectory_state.multi_dof_joint_state.child_frame_ids = []
02065 for i in range(0, length):
02066 start = end
02067 end += 4
02068 (length,) = _struct_I.unpack(str[start:end])
02069 start = end
02070 end += length
02071 if python3:
02072 val1 = str[start:end].decode('utf-8')
02073 else:
02074 val1 = str[start:end]
02075 self.action_feedback.feedback.paused_trajectory_state.multi_dof_joint_state.child_frame_ids.append(val1)
02076 start = end
02077 end += 4
02078 (length,) = _struct_I.unpack(str[start:end])
02079 self.action_feedback.feedback.paused_trajectory_state.multi_dof_joint_state.poses = []
02080 for i in range(0, length):
02081 val1 = geometry_msgs.msg.Pose()
02082 _v29 = val1.position
02083 _x = _v29
02084 start = end
02085 end += 24
02086 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02087 _v30 = val1.orientation
02088 _x = _v30
02089 start = end
02090 end += 32
02091 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
02092 self.action_feedback.feedback.paused_trajectory_state.multi_dof_joint_state.poses.append(val1)
02093 _x = self
02094 start = end
02095 end += 12
02096 (_x.action_feedback.feedback.paused_collision_map.header.seq, _x.action_feedback.feedback.paused_collision_map.header.stamp.secs, _x.action_feedback.feedback.paused_collision_map.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
02097 start = end
02098 end += 4
02099 (length,) = _struct_I.unpack(str[start:end])
02100 start = end
02101 end += length
02102 if python3:
02103 self.action_feedback.feedback.paused_collision_map.header.frame_id = str[start:end].decode('utf-8')
02104 else:
02105 self.action_feedback.feedback.paused_collision_map.header.frame_id = str[start:end]
02106 start = end
02107 end += 4
02108 (length,) = _struct_I.unpack(str[start:end])
02109 start = end
02110 end += length
02111 if python3:
02112 self.action_feedback.feedback.paused_collision_map.id = str[start:end].decode('utf-8')
02113 else:
02114 self.action_feedback.feedback.paused_collision_map.id = str[start:end]
02115 _x = self
02116 start = end
02117 end += 5
02118 (_x.action_feedback.feedback.paused_collision_map.padding, _x.action_feedback.feedback.paused_collision_map.operation.operation,) = _struct_fb.unpack(str[start:end])
02119 start = end
02120 end += 4
02121 (length,) = _struct_I.unpack(str[start:end])
02122 self.action_feedback.feedback.paused_collision_map.shapes = []
02123 for i in range(0, length):
02124 val1 = arm_navigation_msgs.msg.Shape()
02125 start = end
02126 end += 1
02127 (val1.type,) = _struct_b.unpack(str[start:end])
02128 start = end
02129 end += 4
02130 (length,) = _struct_I.unpack(str[start:end])
02131 pattern = '<%sd'%length
02132 start = end
02133 end += struct.calcsize(pattern)
02134 val1.dimensions = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
02135 start = end
02136 end += 4
02137 (length,) = _struct_I.unpack(str[start:end])
02138 pattern = '<%si'%length
02139 start = end
02140 end += struct.calcsize(pattern)
02141 val1.triangles = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
02142 start = end
02143 end += 4
02144 (length,) = _struct_I.unpack(str[start:end])
02145 val1.vertices = []
02146 for i in range(0, length):
02147 val2 = geometry_msgs.msg.Point()
02148 _x = val2
02149 start = end
02150 end += 24
02151 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02152 val1.vertices.append(val2)
02153 self.action_feedback.feedback.paused_collision_map.shapes.append(val1)
02154 start = end
02155 end += 4
02156 (length,) = _struct_I.unpack(str[start:end])
02157 self.action_feedback.feedback.paused_collision_map.poses = []
02158 for i in range(0, length):
02159 val1 = geometry_msgs.msg.Pose()
02160 _v31 = val1.position
02161 _x = _v31
02162 start = end
02163 end += 24
02164 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02165 _v32 = val1.orientation
02166 _x = _v32
02167 start = end
02168 end += 32
02169 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
02170 self.action_feedback.feedback.paused_collision_map.poses.append(val1)
02171 return self
02172 except struct.error as e:
02173 raise genpy.DeserializationError(e)
02174
02175 _struct_I = genpy.struct_I
02176 _struct_B = struct.Struct("<B")
02177 _struct_i3I = struct.Struct("<i3I")
02178 _struct_2i = struct.Struct("<2i")
02179 _struct_fb = struct.Struct("<fb")
02180 _struct_3I = struct.Struct("<3I")
02181 _struct_b = struct.Struct("<b")
02182 _struct_4d = struct.Struct("<4d")
02183 _struct_2I = struct.Struct("<2I")
02184 _struct_3d = struct.Struct("<3d")