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