$search
00001 """autogenerated by genmsg_py from EECartImpedAction.msg. Do not edit.""" 00002 import roslib.message 00003 import struct 00004 00005 import actionlib_msgs.msg 00006 import geometry_msgs.msg 00007 import roslib.rostime 00008 import ee_cart_imped_msgs.msg 00009 import std_msgs.msg 00010 00011 class EECartImpedAction(roslib.message.Message): 00012 _md5sum = "363b9ba3af4663dbf9394e1ef0b84435" 00013 _type = "ee_cart_imped_msgs/EECartImpedAction" 00014 _has_header = False #flag to mark the presence of a Header object 00015 _full_text = """# ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ====== 00016 00017 EECartImpedActionGoal action_goal 00018 EECartImpedActionResult action_result 00019 EECartImpedActionFeedback action_feedback 00020 00021 ================================================================================ 00022 MSG: ee_cart_imped_msgs/EECartImpedActionGoal 00023 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ====== 00024 00025 Header header 00026 actionlib_msgs/GoalID goal_id 00027 EECartImpedGoal goal 00028 00029 ================================================================================ 00030 MSG: std_msgs/Header 00031 # Standard metadata for higher-level stamped data types. 00032 # This is generally used to communicate timestamped data 00033 # in a particular coordinate frame. 00034 # 00035 # sequence ID: consecutively increasing ID 00036 uint32 seq 00037 #Two-integer timestamp that is expressed as: 00038 # * stamp.secs: seconds (stamp_secs) since epoch 00039 # * stamp.nsecs: nanoseconds since stamp_secs 00040 # time-handling sugar is provided by the client library 00041 time stamp 00042 #Frame this data is associated with 00043 # 0: no frame 00044 # 1: global frame 00045 string frame_id 00046 00047 ================================================================================ 00048 MSG: actionlib_msgs/GoalID 00049 # The stamp should store the time at which this goal was requested. 00050 # It is used by an action server when it tries to preempt all 00051 # goals that were requested before a certain time 00052 time stamp 00053 00054 # The id provides a way to associate feedback and 00055 # result message with specific goal requests. The id 00056 # specified must be unique. 00057 string id 00058 00059 00060 ================================================================================ 00061 MSG: ee_cart_imped_msgs/EECartImpedGoal 00062 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ====== 00063 #goal definition 00064 Header header 00065 ee_cart_imped_msgs/StiffPoint[] trajectory 00066 00067 ================================================================================ 00068 MSG: ee_cart_imped_msgs/StiffPoint 00069 Header header 00070 #The pose to achieve in the stiffness directions 00071 geometry_msgs/Pose pose 00072 #Wrench or stiffness for each dimension 00073 geometry_msgs/Wrench wrench_or_stiffness 00074 #The following are True if a force/torque should 00075 #be exerted and False if a stiffness should be used. 00076 bool isForceX 00077 bool isForceY 00078 bool isForceZ 00079 bool isTorqueX 00080 bool isTorqueY 00081 bool isTorqueZ 00082 #The time from the start of the trajectory that this 00083 #point should be achieved. 00084 duration time_from_start 00085 ================================================================================ 00086 MSG: geometry_msgs/Pose 00087 # A representation of pose in free space, composed of postion and orientation. 00088 Point position 00089 Quaternion orientation 00090 00091 ================================================================================ 00092 MSG: geometry_msgs/Point 00093 # This contains the position of a point in free space 00094 float64 x 00095 float64 y 00096 float64 z 00097 00098 ================================================================================ 00099 MSG: geometry_msgs/Quaternion 00100 # This represents an orientation in free space in quaternion form. 00101 00102 float64 x 00103 float64 y 00104 float64 z 00105 float64 w 00106 00107 ================================================================================ 00108 MSG: geometry_msgs/Wrench 00109 # This represents force in free space, seperated into 00110 # it's linear and angular parts. 00111 Vector3 force 00112 Vector3 torque 00113 00114 ================================================================================ 00115 MSG: geometry_msgs/Vector3 00116 # This represents a vector in free space. 00117 00118 float64 x 00119 float64 y 00120 float64 z 00121 ================================================================================ 00122 MSG: ee_cart_imped_msgs/EECartImpedActionResult 00123 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ====== 00124 00125 Header header 00126 actionlib_msgs/GoalStatus status 00127 EECartImpedResult result 00128 00129 ================================================================================ 00130 MSG: actionlib_msgs/GoalStatus 00131 GoalID goal_id 00132 uint8 status 00133 uint8 PENDING = 0 # The goal has yet to be processed by the action server 00134 uint8 ACTIVE = 1 # The goal is currently being processed by the action server 00135 uint8 PREEMPTED = 2 # The goal received a cancel request after it started executing 00136 # and has since completed its execution (Terminal State) 00137 uint8 SUCCEEDED = 3 # The goal was achieved successfully by the action server (Terminal State) 00138 uint8 ABORTED = 4 # The goal was aborted during execution by the action server due 00139 # to some failure (Terminal State) 00140 uint8 REJECTED = 5 # The goal was rejected by the action server without being processed, 00141 # because the goal was unattainable or invalid (Terminal State) 00142 uint8 PREEMPTING = 6 # The goal received a cancel request after it started executing 00143 # and has not yet completed execution 00144 uint8 RECALLING = 7 # The goal received a cancel request before it started executing, 00145 # but the action server has not yet confirmed that the goal is canceled 00146 uint8 RECALLED = 8 # The goal received a cancel request before it started executing 00147 # and was successfully cancelled (Terminal State) 00148 uint8 LOST = 9 # An action client can determine that a goal is LOST. This should not be 00149 # sent over the wire by an action server 00150 00151 #Allow for the user to associate a string with GoalStatus for debugging 00152 string text 00153 00154 00155 ================================================================================ 00156 MSG: ee_cart_imped_msgs/EECartImpedResult 00157 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ====== 00158 #result definition 00159 #whether it was successful 00160 #the pose and force we ended with 00161 Header header 00162 bool success 00163 ee_cart_imped_msgs/StiffPoint desired 00164 geometry_msgs/Pose actual_pose 00165 float64 effort_sq_error 00166 00167 ================================================================================ 00168 MSG: ee_cart_imped_msgs/EECartImpedActionFeedback 00169 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ====== 00170 00171 Header header 00172 actionlib_msgs/GoalStatus status 00173 EECartImpedFeedback feedback 00174 00175 ================================================================================ 00176 MSG: ee_cart_imped_msgs/EECartImpedFeedback 00177 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ====== 00178 #feedback 00179 #current pose and squared error in force 00180 #and the running time of this goal so far 00181 Header header 00182 ee_cart_imped_msgs/StiffPoint[] goal 00183 ee_cart_imped_msgs/StiffPoint initial_point 00184 ee_cart_imped_msgs/StiffPoint desired 00185 ee_cart_imped_msgs/StiffPoint actual_pose 00186 float64 effort_sq_error 00187 float64[] requested_joint_efforts 00188 float64[] actual_joint_efforts 00189 duration running_time 00190 00191 00192 00193 """ 00194 __slots__ = ['action_goal','action_result','action_feedback'] 00195 _slot_types = ['ee_cart_imped_msgs/EECartImpedActionGoal','ee_cart_imped_msgs/EECartImpedActionResult','ee_cart_imped_msgs/EECartImpedActionFeedback'] 00196 00197 def __init__(self, *args, **kwds): 00198 """ 00199 Constructor. Any message fields that are implicitly/explicitly 00200 set to None will be assigned a default value. The recommend 00201 use is keyword arguments as this is more robust to future message 00202 changes. You cannot mix in-order arguments and keyword arguments. 00203 00204 The available fields are: 00205 action_goal,action_result,action_feedback 00206 00207 @param args: complete set of field values, in .msg order 00208 @param kwds: use keyword arguments corresponding to message field names 00209 to set specific fields. 00210 """ 00211 if args or kwds: 00212 super(EECartImpedAction, self).__init__(*args, **kwds) 00213 #message fields cannot be None, assign default values for those that are 00214 if self.action_goal is None: 00215 self.action_goal = ee_cart_imped_msgs.msg.EECartImpedActionGoal() 00216 if self.action_result is None: 00217 self.action_result = ee_cart_imped_msgs.msg.EECartImpedActionResult() 00218 if self.action_feedback is None: 00219 self.action_feedback = ee_cart_imped_msgs.msg.EECartImpedActionFeedback() 00220 else: 00221 self.action_goal = ee_cart_imped_msgs.msg.EECartImpedActionGoal() 00222 self.action_result = ee_cart_imped_msgs.msg.EECartImpedActionResult() 00223 self.action_feedback = ee_cart_imped_msgs.msg.EECartImpedActionFeedback() 00224 00225 def _get_types(self): 00226 """ 00227 internal API method 00228 """ 00229 return self._slot_types 00230 00231 def serialize(self, buff): 00232 """ 00233 serialize message into buffer 00234 @param buff: buffer 00235 @type buff: StringIO 00236 """ 00237 try: 00238 _x = self 00239 buff.write(_struct_3I.pack(_x.action_goal.header.seq, _x.action_goal.header.stamp.secs, _x.action_goal.header.stamp.nsecs)) 00240 _x = self.action_goal.header.frame_id 00241 length = len(_x) 00242 buff.write(struct.pack('<I%ss'%length, length, _x)) 00243 _x = self 00244 buff.write(_struct_2I.pack(_x.action_goal.goal_id.stamp.secs, _x.action_goal.goal_id.stamp.nsecs)) 00245 _x = self.action_goal.goal_id.id 00246 length = len(_x) 00247 buff.write(struct.pack('<I%ss'%length, length, _x)) 00248 _x = self 00249 buff.write(_struct_3I.pack(_x.action_goal.goal.header.seq, _x.action_goal.goal.header.stamp.secs, _x.action_goal.goal.header.stamp.nsecs)) 00250 _x = self.action_goal.goal.header.frame_id 00251 length = len(_x) 00252 buff.write(struct.pack('<I%ss'%length, length, _x)) 00253 length = len(self.action_goal.goal.trajectory) 00254 buff.write(_struct_I.pack(length)) 00255 for val1 in self.action_goal.goal.trajectory: 00256 _v1 = val1.header 00257 buff.write(_struct_I.pack(_v1.seq)) 00258 _v2 = _v1.stamp 00259 _x = _v2 00260 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00261 _x = _v1.frame_id 00262 length = len(_x) 00263 buff.write(struct.pack('<I%ss'%length, length, _x)) 00264 _v3 = val1.pose 00265 _v4 = _v3.position 00266 _x = _v4 00267 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00268 _v5 = _v3.orientation 00269 _x = _v5 00270 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 00271 _v6 = val1.wrench_or_stiffness 00272 _v7 = _v6.force 00273 _x = _v7 00274 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00275 _v8 = _v6.torque 00276 _x = _v8 00277 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00278 _x = val1 00279 buff.write(_struct_6B.pack(_x.isForceX, _x.isForceY, _x.isForceZ, _x.isTorqueX, _x.isTorqueY, _x.isTorqueZ)) 00280 _v9 = val1.time_from_start 00281 _x = _v9 00282 buff.write(_struct_2i.pack(_x.secs, _x.nsecs)) 00283 _x = self 00284 buff.write(_struct_3I.pack(_x.action_result.header.seq, _x.action_result.header.stamp.secs, _x.action_result.header.stamp.nsecs)) 00285 _x = self.action_result.header.frame_id 00286 length = len(_x) 00287 buff.write(struct.pack('<I%ss'%length, length, _x)) 00288 _x = self 00289 buff.write(_struct_2I.pack(_x.action_result.status.goal_id.stamp.secs, _x.action_result.status.goal_id.stamp.nsecs)) 00290 _x = self.action_result.status.goal_id.id 00291 length = len(_x) 00292 buff.write(struct.pack('<I%ss'%length, length, _x)) 00293 buff.write(_struct_B.pack(self.action_result.status.status)) 00294 _x = self.action_result.status.text 00295 length = len(_x) 00296 buff.write(struct.pack('<I%ss'%length, length, _x)) 00297 _x = self 00298 buff.write(_struct_3I.pack(_x.action_result.result.header.seq, _x.action_result.result.header.stamp.secs, _x.action_result.result.header.stamp.nsecs)) 00299 _x = self.action_result.result.header.frame_id 00300 length = len(_x) 00301 buff.write(struct.pack('<I%ss'%length, length, _x)) 00302 _x = self 00303 buff.write(_struct_B3I.pack(_x.action_result.result.success, _x.action_result.result.desired.header.seq, _x.action_result.result.desired.header.stamp.secs, _x.action_result.result.desired.header.stamp.nsecs)) 00304 _x = self.action_result.result.desired.header.frame_id 00305 length = len(_x) 00306 buff.write(struct.pack('<I%ss'%length, length, _x)) 00307 _x = self 00308 buff.write(_struct_13d6B2i8d3I.pack(_x.action_result.result.desired.pose.position.x, _x.action_result.result.desired.pose.position.y, _x.action_result.result.desired.pose.position.z, _x.action_result.result.desired.pose.orientation.x, _x.action_result.result.desired.pose.orientation.y, _x.action_result.result.desired.pose.orientation.z, _x.action_result.result.desired.pose.orientation.w, _x.action_result.result.desired.wrench_or_stiffness.force.x, _x.action_result.result.desired.wrench_or_stiffness.force.y, _x.action_result.result.desired.wrench_or_stiffness.force.z, _x.action_result.result.desired.wrench_or_stiffness.torque.x, _x.action_result.result.desired.wrench_or_stiffness.torque.y, _x.action_result.result.desired.wrench_or_stiffness.torque.z, _x.action_result.result.desired.isForceX, _x.action_result.result.desired.isForceY, _x.action_result.result.desired.isForceZ, _x.action_result.result.desired.isTorqueX, _x.action_result.result.desired.isTorqueY, _x.action_result.result.desired.isTorqueZ, _x.action_result.result.desired.time_from_start.secs, _x.action_result.result.desired.time_from_start.nsecs, _x.action_result.result.actual_pose.position.x, _x.action_result.result.actual_pose.position.y, _x.action_result.result.actual_pose.position.z, _x.action_result.result.actual_pose.orientation.x, _x.action_result.result.actual_pose.orientation.y, _x.action_result.result.actual_pose.orientation.z, _x.action_result.result.actual_pose.orientation.w, _x.action_result.result.effort_sq_error, _x.action_feedback.header.seq, _x.action_feedback.header.stamp.secs, _x.action_feedback.header.stamp.nsecs)) 00309 _x = self.action_feedback.header.frame_id 00310 length = len(_x) 00311 buff.write(struct.pack('<I%ss'%length, length, _x)) 00312 _x = self 00313 buff.write(_struct_2I.pack(_x.action_feedback.status.goal_id.stamp.secs, _x.action_feedback.status.goal_id.stamp.nsecs)) 00314 _x = self.action_feedback.status.goal_id.id 00315 length = len(_x) 00316 buff.write(struct.pack('<I%ss'%length, length, _x)) 00317 buff.write(_struct_B.pack(self.action_feedback.status.status)) 00318 _x = self.action_feedback.status.text 00319 length = len(_x) 00320 buff.write(struct.pack('<I%ss'%length, length, _x)) 00321 _x = self 00322 buff.write(_struct_3I.pack(_x.action_feedback.feedback.header.seq, _x.action_feedback.feedback.header.stamp.secs, _x.action_feedback.feedback.header.stamp.nsecs)) 00323 _x = self.action_feedback.feedback.header.frame_id 00324 length = len(_x) 00325 buff.write(struct.pack('<I%ss'%length, length, _x)) 00326 length = len(self.action_feedback.feedback.goal) 00327 buff.write(_struct_I.pack(length)) 00328 for val1 in self.action_feedback.feedback.goal: 00329 _v10 = val1.header 00330 buff.write(_struct_I.pack(_v10.seq)) 00331 _v11 = _v10.stamp 00332 _x = _v11 00333 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00334 _x = _v10.frame_id 00335 length = len(_x) 00336 buff.write(struct.pack('<I%ss'%length, length, _x)) 00337 _v12 = val1.pose 00338 _v13 = _v12.position 00339 _x = _v13 00340 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00341 _v14 = _v12.orientation 00342 _x = _v14 00343 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 00344 _v15 = val1.wrench_or_stiffness 00345 _v16 = _v15.force 00346 _x = _v16 00347 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00348 _v17 = _v15.torque 00349 _x = _v17 00350 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00351 _x = val1 00352 buff.write(_struct_6B.pack(_x.isForceX, _x.isForceY, _x.isForceZ, _x.isTorqueX, _x.isTorqueY, _x.isTorqueZ)) 00353 _v18 = val1.time_from_start 00354 _x = _v18 00355 buff.write(_struct_2i.pack(_x.secs, _x.nsecs)) 00356 _x = self 00357 buff.write(_struct_3I.pack(_x.action_feedback.feedback.initial_point.header.seq, _x.action_feedback.feedback.initial_point.header.stamp.secs, _x.action_feedback.feedback.initial_point.header.stamp.nsecs)) 00358 _x = self.action_feedback.feedback.initial_point.header.frame_id 00359 length = len(_x) 00360 buff.write(struct.pack('<I%ss'%length, length, _x)) 00361 _x = self 00362 buff.write(_struct_13d6B2i3I.pack(_x.action_feedback.feedback.initial_point.pose.position.x, _x.action_feedback.feedback.initial_point.pose.position.y, _x.action_feedback.feedback.initial_point.pose.position.z, _x.action_feedback.feedback.initial_point.pose.orientation.x, _x.action_feedback.feedback.initial_point.pose.orientation.y, _x.action_feedback.feedback.initial_point.pose.orientation.z, _x.action_feedback.feedback.initial_point.pose.orientation.w, _x.action_feedback.feedback.initial_point.wrench_or_stiffness.force.x, _x.action_feedback.feedback.initial_point.wrench_or_stiffness.force.y, _x.action_feedback.feedback.initial_point.wrench_or_stiffness.force.z, _x.action_feedback.feedback.initial_point.wrench_or_stiffness.torque.x, _x.action_feedback.feedback.initial_point.wrench_or_stiffness.torque.y, _x.action_feedback.feedback.initial_point.wrench_or_stiffness.torque.z, _x.action_feedback.feedback.initial_point.isForceX, _x.action_feedback.feedback.initial_point.isForceY, _x.action_feedback.feedback.initial_point.isForceZ, _x.action_feedback.feedback.initial_point.isTorqueX, _x.action_feedback.feedback.initial_point.isTorqueY, _x.action_feedback.feedback.initial_point.isTorqueZ, _x.action_feedback.feedback.initial_point.time_from_start.secs, _x.action_feedback.feedback.initial_point.time_from_start.nsecs, _x.action_feedback.feedback.desired.header.seq, _x.action_feedback.feedback.desired.header.stamp.secs, _x.action_feedback.feedback.desired.header.stamp.nsecs)) 00363 _x = self.action_feedback.feedback.desired.header.frame_id 00364 length = len(_x) 00365 buff.write(struct.pack('<I%ss'%length, length, _x)) 00366 _x = self 00367 buff.write(_struct_13d6B2i3I.pack(_x.action_feedback.feedback.desired.pose.position.x, _x.action_feedback.feedback.desired.pose.position.y, _x.action_feedback.feedback.desired.pose.position.z, _x.action_feedback.feedback.desired.pose.orientation.x, _x.action_feedback.feedback.desired.pose.orientation.y, _x.action_feedback.feedback.desired.pose.orientation.z, _x.action_feedback.feedback.desired.pose.orientation.w, _x.action_feedback.feedback.desired.wrench_or_stiffness.force.x, _x.action_feedback.feedback.desired.wrench_or_stiffness.force.y, _x.action_feedback.feedback.desired.wrench_or_stiffness.force.z, _x.action_feedback.feedback.desired.wrench_or_stiffness.torque.x, _x.action_feedback.feedback.desired.wrench_or_stiffness.torque.y, _x.action_feedback.feedback.desired.wrench_or_stiffness.torque.z, _x.action_feedback.feedback.desired.isForceX, _x.action_feedback.feedback.desired.isForceY, _x.action_feedback.feedback.desired.isForceZ, _x.action_feedback.feedback.desired.isTorqueX, _x.action_feedback.feedback.desired.isTorqueY, _x.action_feedback.feedback.desired.isTorqueZ, _x.action_feedback.feedback.desired.time_from_start.secs, _x.action_feedback.feedback.desired.time_from_start.nsecs, _x.action_feedback.feedback.actual_pose.header.seq, _x.action_feedback.feedback.actual_pose.header.stamp.secs, _x.action_feedback.feedback.actual_pose.header.stamp.nsecs)) 00368 _x = self.action_feedback.feedback.actual_pose.header.frame_id 00369 length = len(_x) 00370 buff.write(struct.pack('<I%ss'%length, length, _x)) 00371 _x = self 00372 buff.write(_struct_13d6B2id.pack(_x.action_feedback.feedback.actual_pose.pose.position.x, _x.action_feedback.feedback.actual_pose.pose.position.y, _x.action_feedback.feedback.actual_pose.pose.position.z, _x.action_feedback.feedback.actual_pose.pose.orientation.x, _x.action_feedback.feedback.actual_pose.pose.orientation.y, _x.action_feedback.feedback.actual_pose.pose.orientation.z, _x.action_feedback.feedback.actual_pose.pose.orientation.w, _x.action_feedback.feedback.actual_pose.wrench_or_stiffness.force.x, _x.action_feedback.feedback.actual_pose.wrench_or_stiffness.force.y, _x.action_feedback.feedback.actual_pose.wrench_or_stiffness.force.z, _x.action_feedback.feedback.actual_pose.wrench_or_stiffness.torque.x, _x.action_feedback.feedback.actual_pose.wrench_or_stiffness.torque.y, _x.action_feedback.feedback.actual_pose.wrench_or_stiffness.torque.z, _x.action_feedback.feedback.actual_pose.isForceX, _x.action_feedback.feedback.actual_pose.isForceY, _x.action_feedback.feedback.actual_pose.isForceZ, _x.action_feedback.feedback.actual_pose.isTorqueX, _x.action_feedback.feedback.actual_pose.isTorqueY, _x.action_feedback.feedback.actual_pose.isTorqueZ, _x.action_feedback.feedback.actual_pose.time_from_start.secs, _x.action_feedback.feedback.actual_pose.time_from_start.nsecs, _x.action_feedback.feedback.effort_sq_error)) 00373 length = len(self.action_feedback.feedback.requested_joint_efforts) 00374 buff.write(_struct_I.pack(length)) 00375 pattern = '<%sd'%length 00376 buff.write(struct.pack(pattern, *self.action_feedback.feedback.requested_joint_efforts)) 00377 length = len(self.action_feedback.feedback.actual_joint_efforts) 00378 buff.write(_struct_I.pack(length)) 00379 pattern = '<%sd'%length 00380 buff.write(struct.pack(pattern, *self.action_feedback.feedback.actual_joint_efforts)) 00381 _x = self 00382 buff.write(_struct_2i.pack(_x.action_feedback.feedback.running_time.secs, _x.action_feedback.feedback.running_time.nsecs)) 00383 except struct.error as se: self._check_types(se) 00384 except TypeError as te: self._check_types(te) 00385 00386 def deserialize(self, str): 00387 """ 00388 unpack serialized message in str into this message instance 00389 @param str: byte array of serialized message 00390 @type str: str 00391 """ 00392 try: 00393 if self.action_goal is None: 00394 self.action_goal = ee_cart_imped_msgs.msg.EECartImpedActionGoal() 00395 if self.action_result is None: 00396 self.action_result = ee_cart_imped_msgs.msg.EECartImpedActionResult() 00397 if self.action_feedback is None: 00398 self.action_feedback = ee_cart_imped_msgs.msg.EECartImpedActionFeedback() 00399 end = 0 00400 _x = self 00401 start = end 00402 end += 12 00403 (_x.action_goal.header.seq, _x.action_goal.header.stamp.secs, _x.action_goal.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 00404 start = end 00405 end += 4 00406 (length,) = _struct_I.unpack(str[start:end]) 00407 start = end 00408 end += length 00409 self.action_goal.header.frame_id = str[start:end] 00410 _x = self 00411 start = end 00412 end += 8 00413 (_x.action_goal.goal_id.stamp.secs, _x.action_goal.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end]) 00414 start = end 00415 end += 4 00416 (length,) = _struct_I.unpack(str[start:end]) 00417 start = end 00418 end += length 00419 self.action_goal.goal_id.id = str[start:end] 00420 _x = self 00421 start = end 00422 end += 12 00423 (_x.action_goal.goal.header.seq, _x.action_goal.goal.header.stamp.secs, _x.action_goal.goal.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 00424 start = end 00425 end += 4 00426 (length,) = _struct_I.unpack(str[start:end]) 00427 start = end 00428 end += length 00429 self.action_goal.goal.header.frame_id = str[start:end] 00430 start = end 00431 end += 4 00432 (length,) = _struct_I.unpack(str[start:end]) 00433 self.action_goal.goal.trajectory = [] 00434 for i in range(0, length): 00435 val1 = ee_cart_imped_msgs.msg.StiffPoint() 00436 _v19 = val1.header 00437 start = end 00438 end += 4 00439 (_v19.seq,) = _struct_I.unpack(str[start:end]) 00440 _v20 = _v19.stamp 00441 _x = _v20 00442 start = end 00443 end += 8 00444 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 00445 start = end 00446 end += 4 00447 (length,) = _struct_I.unpack(str[start:end]) 00448 start = end 00449 end += length 00450 _v19.frame_id = str[start:end] 00451 _v21 = val1.pose 00452 _v22 = _v21.position 00453 _x = _v22 00454 start = end 00455 end += 24 00456 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 00457 _v23 = _v21.orientation 00458 _x = _v23 00459 start = end 00460 end += 32 00461 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 00462 _v24 = val1.wrench_or_stiffness 00463 _v25 = _v24.force 00464 _x = _v25 00465 start = end 00466 end += 24 00467 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 00468 _v26 = _v24.torque 00469 _x = _v26 00470 start = end 00471 end += 24 00472 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 00473 _x = val1 00474 start = end 00475 end += 6 00476 (_x.isForceX, _x.isForceY, _x.isForceZ, _x.isTorqueX, _x.isTorqueY, _x.isTorqueZ,) = _struct_6B.unpack(str[start:end]) 00477 val1.isForceX = bool(val1.isForceX) 00478 val1.isForceY = bool(val1.isForceY) 00479 val1.isForceZ = bool(val1.isForceZ) 00480 val1.isTorqueX = bool(val1.isTorqueX) 00481 val1.isTorqueY = bool(val1.isTorqueY) 00482 val1.isTorqueZ = bool(val1.isTorqueZ) 00483 _v27 = val1.time_from_start 00484 _x = _v27 00485 start = end 00486 end += 8 00487 (_x.secs, _x.nsecs,) = _struct_2i.unpack(str[start:end]) 00488 self.action_goal.goal.trajectory.append(val1) 00489 _x = self 00490 start = end 00491 end += 12 00492 (_x.action_result.header.seq, _x.action_result.header.stamp.secs, _x.action_result.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 00493 start = end 00494 end += 4 00495 (length,) = _struct_I.unpack(str[start:end]) 00496 start = end 00497 end += length 00498 self.action_result.header.frame_id = str[start:end] 00499 _x = self 00500 start = end 00501 end += 8 00502 (_x.action_result.status.goal_id.stamp.secs, _x.action_result.status.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end]) 00503 start = end 00504 end += 4 00505 (length,) = _struct_I.unpack(str[start:end]) 00506 start = end 00507 end += length 00508 self.action_result.status.goal_id.id = str[start:end] 00509 start = end 00510 end += 1 00511 (self.action_result.status.status,) = _struct_B.unpack(str[start:end]) 00512 start = end 00513 end += 4 00514 (length,) = _struct_I.unpack(str[start:end]) 00515 start = end 00516 end += length 00517 self.action_result.status.text = str[start:end] 00518 _x = self 00519 start = end 00520 end += 12 00521 (_x.action_result.result.header.seq, _x.action_result.result.header.stamp.secs, _x.action_result.result.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 00522 start = end 00523 end += 4 00524 (length,) = _struct_I.unpack(str[start:end]) 00525 start = end 00526 end += length 00527 self.action_result.result.header.frame_id = str[start:end] 00528 _x = self 00529 start = end 00530 end += 13 00531 (_x.action_result.result.success, _x.action_result.result.desired.header.seq, _x.action_result.result.desired.header.stamp.secs, _x.action_result.result.desired.header.stamp.nsecs,) = _struct_B3I.unpack(str[start:end]) 00532 self.action_result.result.success = bool(self.action_result.result.success) 00533 start = end 00534 end += 4 00535 (length,) = _struct_I.unpack(str[start:end]) 00536 start = end 00537 end += length 00538 self.action_result.result.desired.header.frame_id = str[start:end] 00539 _x = self 00540 start = end 00541 end += 194 00542 (_x.action_result.result.desired.pose.position.x, _x.action_result.result.desired.pose.position.y, _x.action_result.result.desired.pose.position.z, _x.action_result.result.desired.pose.orientation.x, _x.action_result.result.desired.pose.orientation.y, _x.action_result.result.desired.pose.orientation.z, _x.action_result.result.desired.pose.orientation.w, _x.action_result.result.desired.wrench_or_stiffness.force.x, _x.action_result.result.desired.wrench_or_stiffness.force.y, _x.action_result.result.desired.wrench_or_stiffness.force.z, _x.action_result.result.desired.wrench_or_stiffness.torque.x, _x.action_result.result.desired.wrench_or_stiffness.torque.y, _x.action_result.result.desired.wrench_or_stiffness.torque.z, _x.action_result.result.desired.isForceX, _x.action_result.result.desired.isForceY, _x.action_result.result.desired.isForceZ, _x.action_result.result.desired.isTorqueX, _x.action_result.result.desired.isTorqueY, _x.action_result.result.desired.isTorqueZ, _x.action_result.result.desired.time_from_start.secs, _x.action_result.result.desired.time_from_start.nsecs, _x.action_result.result.actual_pose.position.x, _x.action_result.result.actual_pose.position.y, _x.action_result.result.actual_pose.position.z, _x.action_result.result.actual_pose.orientation.x, _x.action_result.result.actual_pose.orientation.y, _x.action_result.result.actual_pose.orientation.z, _x.action_result.result.actual_pose.orientation.w, _x.action_result.result.effort_sq_error, _x.action_feedback.header.seq, _x.action_feedback.header.stamp.secs, _x.action_feedback.header.stamp.nsecs,) = _struct_13d6B2i8d3I.unpack(str[start:end]) 00543 self.action_result.result.desired.isForceX = bool(self.action_result.result.desired.isForceX) 00544 self.action_result.result.desired.isForceY = bool(self.action_result.result.desired.isForceY) 00545 self.action_result.result.desired.isForceZ = bool(self.action_result.result.desired.isForceZ) 00546 self.action_result.result.desired.isTorqueX = bool(self.action_result.result.desired.isTorqueX) 00547 self.action_result.result.desired.isTorqueY = bool(self.action_result.result.desired.isTorqueY) 00548 self.action_result.result.desired.isTorqueZ = bool(self.action_result.result.desired.isTorqueZ) 00549 start = end 00550 end += 4 00551 (length,) = _struct_I.unpack(str[start:end]) 00552 start = end 00553 end += length 00554 self.action_feedback.header.frame_id = str[start:end] 00555 _x = self 00556 start = end 00557 end += 8 00558 (_x.action_feedback.status.goal_id.stamp.secs, _x.action_feedback.status.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end]) 00559 start = end 00560 end += 4 00561 (length,) = _struct_I.unpack(str[start:end]) 00562 start = end 00563 end += length 00564 self.action_feedback.status.goal_id.id = str[start:end] 00565 start = end 00566 end += 1 00567 (self.action_feedback.status.status,) = _struct_B.unpack(str[start:end]) 00568 start = end 00569 end += 4 00570 (length,) = _struct_I.unpack(str[start:end]) 00571 start = end 00572 end += length 00573 self.action_feedback.status.text = str[start:end] 00574 _x = self 00575 start = end 00576 end += 12 00577 (_x.action_feedback.feedback.header.seq, _x.action_feedback.feedback.header.stamp.secs, _x.action_feedback.feedback.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 00578 start = end 00579 end += 4 00580 (length,) = _struct_I.unpack(str[start:end]) 00581 start = end 00582 end += length 00583 self.action_feedback.feedback.header.frame_id = str[start:end] 00584 start = end 00585 end += 4 00586 (length,) = _struct_I.unpack(str[start:end]) 00587 self.action_feedback.feedback.goal = [] 00588 for i in range(0, length): 00589 val1 = ee_cart_imped_msgs.msg.StiffPoint() 00590 _v28 = val1.header 00591 start = end 00592 end += 4 00593 (_v28.seq,) = _struct_I.unpack(str[start:end]) 00594 _v29 = _v28.stamp 00595 _x = _v29 00596 start = end 00597 end += 8 00598 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 00599 start = end 00600 end += 4 00601 (length,) = _struct_I.unpack(str[start:end]) 00602 start = end 00603 end += length 00604 _v28.frame_id = str[start:end] 00605 _v30 = val1.pose 00606 _v31 = _v30.position 00607 _x = _v31 00608 start = end 00609 end += 24 00610 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 00611 _v32 = _v30.orientation 00612 _x = _v32 00613 start = end 00614 end += 32 00615 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 00616 _v33 = val1.wrench_or_stiffness 00617 _v34 = _v33.force 00618 _x = _v34 00619 start = end 00620 end += 24 00621 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 00622 _v35 = _v33.torque 00623 _x = _v35 00624 start = end 00625 end += 24 00626 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 00627 _x = val1 00628 start = end 00629 end += 6 00630 (_x.isForceX, _x.isForceY, _x.isForceZ, _x.isTorqueX, _x.isTorqueY, _x.isTorqueZ,) = _struct_6B.unpack(str[start:end]) 00631 val1.isForceX = bool(val1.isForceX) 00632 val1.isForceY = bool(val1.isForceY) 00633 val1.isForceZ = bool(val1.isForceZ) 00634 val1.isTorqueX = bool(val1.isTorqueX) 00635 val1.isTorqueY = bool(val1.isTorqueY) 00636 val1.isTorqueZ = bool(val1.isTorqueZ) 00637 _v36 = val1.time_from_start 00638 _x = _v36 00639 start = end 00640 end += 8 00641 (_x.secs, _x.nsecs,) = _struct_2i.unpack(str[start:end]) 00642 self.action_feedback.feedback.goal.append(val1) 00643 _x = self 00644 start = end 00645 end += 12 00646 (_x.action_feedback.feedback.initial_point.header.seq, _x.action_feedback.feedback.initial_point.header.stamp.secs, _x.action_feedback.feedback.initial_point.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 00647 start = end 00648 end += 4 00649 (length,) = _struct_I.unpack(str[start:end]) 00650 start = end 00651 end += length 00652 self.action_feedback.feedback.initial_point.header.frame_id = str[start:end] 00653 _x = self 00654 start = end 00655 end += 130 00656 (_x.action_feedback.feedback.initial_point.pose.position.x, _x.action_feedback.feedback.initial_point.pose.position.y, _x.action_feedback.feedback.initial_point.pose.position.z, _x.action_feedback.feedback.initial_point.pose.orientation.x, _x.action_feedback.feedback.initial_point.pose.orientation.y, _x.action_feedback.feedback.initial_point.pose.orientation.z, _x.action_feedback.feedback.initial_point.pose.orientation.w, _x.action_feedback.feedback.initial_point.wrench_or_stiffness.force.x, _x.action_feedback.feedback.initial_point.wrench_or_stiffness.force.y, _x.action_feedback.feedback.initial_point.wrench_or_stiffness.force.z, _x.action_feedback.feedback.initial_point.wrench_or_stiffness.torque.x, _x.action_feedback.feedback.initial_point.wrench_or_stiffness.torque.y, _x.action_feedback.feedback.initial_point.wrench_or_stiffness.torque.z, _x.action_feedback.feedback.initial_point.isForceX, _x.action_feedback.feedback.initial_point.isForceY, _x.action_feedback.feedback.initial_point.isForceZ, _x.action_feedback.feedback.initial_point.isTorqueX, _x.action_feedback.feedback.initial_point.isTorqueY, _x.action_feedback.feedback.initial_point.isTorqueZ, _x.action_feedback.feedback.initial_point.time_from_start.secs, _x.action_feedback.feedback.initial_point.time_from_start.nsecs, _x.action_feedback.feedback.desired.header.seq, _x.action_feedback.feedback.desired.header.stamp.secs, _x.action_feedback.feedback.desired.header.stamp.nsecs,) = _struct_13d6B2i3I.unpack(str[start:end]) 00657 self.action_feedback.feedback.initial_point.isForceX = bool(self.action_feedback.feedback.initial_point.isForceX) 00658 self.action_feedback.feedback.initial_point.isForceY = bool(self.action_feedback.feedback.initial_point.isForceY) 00659 self.action_feedback.feedback.initial_point.isForceZ = bool(self.action_feedback.feedback.initial_point.isForceZ) 00660 self.action_feedback.feedback.initial_point.isTorqueX = bool(self.action_feedback.feedback.initial_point.isTorqueX) 00661 self.action_feedback.feedback.initial_point.isTorqueY = bool(self.action_feedback.feedback.initial_point.isTorqueY) 00662 self.action_feedback.feedback.initial_point.isTorqueZ = bool(self.action_feedback.feedback.initial_point.isTorqueZ) 00663 start = end 00664 end += 4 00665 (length,) = _struct_I.unpack(str[start:end]) 00666 start = end 00667 end += length 00668 self.action_feedback.feedback.desired.header.frame_id = str[start:end] 00669 _x = self 00670 start = end 00671 end += 130 00672 (_x.action_feedback.feedback.desired.pose.position.x, _x.action_feedback.feedback.desired.pose.position.y, _x.action_feedback.feedback.desired.pose.position.z, _x.action_feedback.feedback.desired.pose.orientation.x, _x.action_feedback.feedback.desired.pose.orientation.y, _x.action_feedback.feedback.desired.pose.orientation.z, _x.action_feedback.feedback.desired.pose.orientation.w, _x.action_feedback.feedback.desired.wrench_or_stiffness.force.x, _x.action_feedback.feedback.desired.wrench_or_stiffness.force.y, _x.action_feedback.feedback.desired.wrench_or_stiffness.force.z, _x.action_feedback.feedback.desired.wrench_or_stiffness.torque.x, _x.action_feedback.feedback.desired.wrench_or_stiffness.torque.y, _x.action_feedback.feedback.desired.wrench_or_stiffness.torque.z, _x.action_feedback.feedback.desired.isForceX, _x.action_feedback.feedback.desired.isForceY, _x.action_feedback.feedback.desired.isForceZ, _x.action_feedback.feedback.desired.isTorqueX, _x.action_feedback.feedback.desired.isTorqueY, _x.action_feedback.feedback.desired.isTorqueZ, _x.action_feedback.feedback.desired.time_from_start.secs, _x.action_feedback.feedback.desired.time_from_start.nsecs, _x.action_feedback.feedback.actual_pose.header.seq, _x.action_feedback.feedback.actual_pose.header.stamp.secs, _x.action_feedback.feedback.actual_pose.header.stamp.nsecs,) = _struct_13d6B2i3I.unpack(str[start:end]) 00673 self.action_feedback.feedback.desired.isForceX = bool(self.action_feedback.feedback.desired.isForceX) 00674 self.action_feedback.feedback.desired.isForceY = bool(self.action_feedback.feedback.desired.isForceY) 00675 self.action_feedback.feedback.desired.isForceZ = bool(self.action_feedback.feedback.desired.isForceZ) 00676 self.action_feedback.feedback.desired.isTorqueX = bool(self.action_feedback.feedback.desired.isTorqueX) 00677 self.action_feedback.feedback.desired.isTorqueY = bool(self.action_feedback.feedback.desired.isTorqueY) 00678 self.action_feedback.feedback.desired.isTorqueZ = bool(self.action_feedback.feedback.desired.isTorqueZ) 00679 start = end 00680 end += 4 00681 (length,) = _struct_I.unpack(str[start:end]) 00682 start = end 00683 end += length 00684 self.action_feedback.feedback.actual_pose.header.frame_id = str[start:end] 00685 _x = self 00686 start = end 00687 end += 126 00688 (_x.action_feedback.feedback.actual_pose.pose.position.x, _x.action_feedback.feedback.actual_pose.pose.position.y, _x.action_feedback.feedback.actual_pose.pose.position.z, _x.action_feedback.feedback.actual_pose.pose.orientation.x, _x.action_feedback.feedback.actual_pose.pose.orientation.y, _x.action_feedback.feedback.actual_pose.pose.orientation.z, _x.action_feedback.feedback.actual_pose.pose.orientation.w, _x.action_feedback.feedback.actual_pose.wrench_or_stiffness.force.x, _x.action_feedback.feedback.actual_pose.wrench_or_stiffness.force.y, _x.action_feedback.feedback.actual_pose.wrench_or_stiffness.force.z, _x.action_feedback.feedback.actual_pose.wrench_or_stiffness.torque.x, _x.action_feedback.feedback.actual_pose.wrench_or_stiffness.torque.y, _x.action_feedback.feedback.actual_pose.wrench_or_stiffness.torque.z, _x.action_feedback.feedback.actual_pose.isForceX, _x.action_feedback.feedback.actual_pose.isForceY, _x.action_feedback.feedback.actual_pose.isForceZ, _x.action_feedback.feedback.actual_pose.isTorqueX, _x.action_feedback.feedback.actual_pose.isTorqueY, _x.action_feedback.feedback.actual_pose.isTorqueZ, _x.action_feedback.feedback.actual_pose.time_from_start.secs, _x.action_feedback.feedback.actual_pose.time_from_start.nsecs, _x.action_feedback.feedback.effort_sq_error,) = _struct_13d6B2id.unpack(str[start:end]) 00689 self.action_feedback.feedback.actual_pose.isForceX = bool(self.action_feedback.feedback.actual_pose.isForceX) 00690 self.action_feedback.feedback.actual_pose.isForceY = bool(self.action_feedback.feedback.actual_pose.isForceY) 00691 self.action_feedback.feedback.actual_pose.isForceZ = bool(self.action_feedback.feedback.actual_pose.isForceZ) 00692 self.action_feedback.feedback.actual_pose.isTorqueX = bool(self.action_feedback.feedback.actual_pose.isTorqueX) 00693 self.action_feedback.feedback.actual_pose.isTorqueY = bool(self.action_feedback.feedback.actual_pose.isTorqueY) 00694 self.action_feedback.feedback.actual_pose.isTorqueZ = bool(self.action_feedback.feedback.actual_pose.isTorqueZ) 00695 start = end 00696 end += 4 00697 (length,) = _struct_I.unpack(str[start:end]) 00698 pattern = '<%sd'%length 00699 start = end 00700 end += struct.calcsize(pattern) 00701 self.action_feedback.feedback.requested_joint_efforts = struct.unpack(pattern, str[start:end]) 00702 start = end 00703 end += 4 00704 (length,) = _struct_I.unpack(str[start:end]) 00705 pattern = '<%sd'%length 00706 start = end 00707 end += struct.calcsize(pattern) 00708 self.action_feedback.feedback.actual_joint_efforts = struct.unpack(pattern, str[start:end]) 00709 _x = self 00710 start = end 00711 end += 8 00712 (_x.action_feedback.feedback.running_time.secs, _x.action_feedback.feedback.running_time.nsecs,) = _struct_2i.unpack(str[start:end]) 00713 return self 00714 except struct.error as e: 00715 raise roslib.message.DeserializationError(e) #most likely buffer underfill 00716 00717 00718 def serialize_numpy(self, buff, numpy): 00719 """ 00720 serialize message with numpy array types into buffer 00721 @param buff: buffer 00722 @type buff: StringIO 00723 @param numpy: numpy python module 00724 @type numpy module 00725 """ 00726 try: 00727 _x = self 00728 buff.write(_struct_3I.pack(_x.action_goal.header.seq, _x.action_goal.header.stamp.secs, _x.action_goal.header.stamp.nsecs)) 00729 _x = self.action_goal.header.frame_id 00730 length = len(_x) 00731 buff.write(struct.pack('<I%ss'%length, length, _x)) 00732 _x = self 00733 buff.write(_struct_2I.pack(_x.action_goal.goal_id.stamp.secs, _x.action_goal.goal_id.stamp.nsecs)) 00734 _x = self.action_goal.goal_id.id 00735 length = len(_x) 00736 buff.write(struct.pack('<I%ss'%length, length, _x)) 00737 _x = self 00738 buff.write(_struct_3I.pack(_x.action_goal.goal.header.seq, _x.action_goal.goal.header.stamp.secs, _x.action_goal.goal.header.stamp.nsecs)) 00739 _x = self.action_goal.goal.header.frame_id 00740 length = len(_x) 00741 buff.write(struct.pack('<I%ss'%length, length, _x)) 00742 length = len(self.action_goal.goal.trajectory) 00743 buff.write(_struct_I.pack(length)) 00744 for val1 in self.action_goal.goal.trajectory: 00745 _v37 = val1.header 00746 buff.write(_struct_I.pack(_v37.seq)) 00747 _v38 = _v37.stamp 00748 _x = _v38 00749 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00750 _x = _v37.frame_id 00751 length = len(_x) 00752 buff.write(struct.pack('<I%ss'%length, length, _x)) 00753 _v39 = val1.pose 00754 _v40 = _v39.position 00755 _x = _v40 00756 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00757 _v41 = _v39.orientation 00758 _x = _v41 00759 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 00760 _v42 = val1.wrench_or_stiffness 00761 _v43 = _v42.force 00762 _x = _v43 00763 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00764 _v44 = _v42.torque 00765 _x = _v44 00766 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00767 _x = val1 00768 buff.write(_struct_6B.pack(_x.isForceX, _x.isForceY, _x.isForceZ, _x.isTorqueX, _x.isTorqueY, _x.isTorqueZ)) 00769 _v45 = val1.time_from_start 00770 _x = _v45 00771 buff.write(_struct_2i.pack(_x.secs, _x.nsecs)) 00772 _x = self 00773 buff.write(_struct_3I.pack(_x.action_result.header.seq, _x.action_result.header.stamp.secs, _x.action_result.header.stamp.nsecs)) 00774 _x = self.action_result.header.frame_id 00775 length = len(_x) 00776 buff.write(struct.pack('<I%ss'%length, length, _x)) 00777 _x = self 00778 buff.write(_struct_2I.pack(_x.action_result.status.goal_id.stamp.secs, _x.action_result.status.goal_id.stamp.nsecs)) 00779 _x = self.action_result.status.goal_id.id 00780 length = len(_x) 00781 buff.write(struct.pack('<I%ss'%length, length, _x)) 00782 buff.write(_struct_B.pack(self.action_result.status.status)) 00783 _x = self.action_result.status.text 00784 length = len(_x) 00785 buff.write(struct.pack('<I%ss'%length, length, _x)) 00786 _x = self 00787 buff.write(_struct_3I.pack(_x.action_result.result.header.seq, _x.action_result.result.header.stamp.secs, _x.action_result.result.header.stamp.nsecs)) 00788 _x = self.action_result.result.header.frame_id 00789 length = len(_x) 00790 buff.write(struct.pack('<I%ss'%length, length, _x)) 00791 _x = self 00792 buff.write(_struct_B3I.pack(_x.action_result.result.success, _x.action_result.result.desired.header.seq, _x.action_result.result.desired.header.stamp.secs, _x.action_result.result.desired.header.stamp.nsecs)) 00793 _x = self.action_result.result.desired.header.frame_id 00794 length = len(_x) 00795 buff.write(struct.pack('<I%ss'%length, length, _x)) 00796 _x = self 00797 buff.write(_struct_13d6B2i8d3I.pack(_x.action_result.result.desired.pose.position.x, _x.action_result.result.desired.pose.position.y, _x.action_result.result.desired.pose.position.z, _x.action_result.result.desired.pose.orientation.x, _x.action_result.result.desired.pose.orientation.y, _x.action_result.result.desired.pose.orientation.z, _x.action_result.result.desired.pose.orientation.w, _x.action_result.result.desired.wrench_or_stiffness.force.x, _x.action_result.result.desired.wrench_or_stiffness.force.y, _x.action_result.result.desired.wrench_or_stiffness.force.z, _x.action_result.result.desired.wrench_or_stiffness.torque.x, _x.action_result.result.desired.wrench_or_stiffness.torque.y, _x.action_result.result.desired.wrench_or_stiffness.torque.z, _x.action_result.result.desired.isForceX, _x.action_result.result.desired.isForceY, _x.action_result.result.desired.isForceZ, _x.action_result.result.desired.isTorqueX, _x.action_result.result.desired.isTorqueY, _x.action_result.result.desired.isTorqueZ, _x.action_result.result.desired.time_from_start.secs, _x.action_result.result.desired.time_from_start.nsecs, _x.action_result.result.actual_pose.position.x, _x.action_result.result.actual_pose.position.y, _x.action_result.result.actual_pose.position.z, _x.action_result.result.actual_pose.orientation.x, _x.action_result.result.actual_pose.orientation.y, _x.action_result.result.actual_pose.orientation.z, _x.action_result.result.actual_pose.orientation.w, _x.action_result.result.effort_sq_error, _x.action_feedback.header.seq, _x.action_feedback.header.stamp.secs, _x.action_feedback.header.stamp.nsecs)) 00798 _x = self.action_feedback.header.frame_id 00799 length = len(_x) 00800 buff.write(struct.pack('<I%ss'%length, length, _x)) 00801 _x = self 00802 buff.write(_struct_2I.pack(_x.action_feedback.status.goal_id.stamp.secs, _x.action_feedback.status.goal_id.stamp.nsecs)) 00803 _x = self.action_feedback.status.goal_id.id 00804 length = len(_x) 00805 buff.write(struct.pack('<I%ss'%length, length, _x)) 00806 buff.write(_struct_B.pack(self.action_feedback.status.status)) 00807 _x = self.action_feedback.status.text 00808 length = len(_x) 00809 buff.write(struct.pack('<I%ss'%length, length, _x)) 00810 _x = self 00811 buff.write(_struct_3I.pack(_x.action_feedback.feedback.header.seq, _x.action_feedback.feedback.header.stamp.secs, _x.action_feedback.feedback.header.stamp.nsecs)) 00812 _x = self.action_feedback.feedback.header.frame_id 00813 length = len(_x) 00814 buff.write(struct.pack('<I%ss'%length, length, _x)) 00815 length = len(self.action_feedback.feedback.goal) 00816 buff.write(_struct_I.pack(length)) 00817 for val1 in self.action_feedback.feedback.goal: 00818 _v46 = val1.header 00819 buff.write(_struct_I.pack(_v46.seq)) 00820 _v47 = _v46.stamp 00821 _x = _v47 00822 buff.write(_struct_2I.pack(_x.secs, _x.nsecs)) 00823 _x = _v46.frame_id 00824 length = len(_x) 00825 buff.write(struct.pack('<I%ss'%length, length, _x)) 00826 _v48 = val1.pose 00827 _v49 = _v48.position 00828 _x = _v49 00829 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00830 _v50 = _v48.orientation 00831 _x = _v50 00832 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w)) 00833 _v51 = val1.wrench_or_stiffness 00834 _v52 = _v51.force 00835 _x = _v52 00836 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00837 _v53 = _v51.torque 00838 _x = _v53 00839 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z)) 00840 _x = val1 00841 buff.write(_struct_6B.pack(_x.isForceX, _x.isForceY, _x.isForceZ, _x.isTorqueX, _x.isTorqueY, _x.isTorqueZ)) 00842 _v54 = val1.time_from_start 00843 _x = _v54 00844 buff.write(_struct_2i.pack(_x.secs, _x.nsecs)) 00845 _x = self 00846 buff.write(_struct_3I.pack(_x.action_feedback.feedback.initial_point.header.seq, _x.action_feedback.feedback.initial_point.header.stamp.secs, _x.action_feedback.feedback.initial_point.header.stamp.nsecs)) 00847 _x = self.action_feedback.feedback.initial_point.header.frame_id 00848 length = len(_x) 00849 buff.write(struct.pack('<I%ss'%length, length, _x)) 00850 _x = self 00851 buff.write(_struct_13d6B2i3I.pack(_x.action_feedback.feedback.initial_point.pose.position.x, _x.action_feedback.feedback.initial_point.pose.position.y, _x.action_feedback.feedback.initial_point.pose.position.z, _x.action_feedback.feedback.initial_point.pose.orientation.x, _x.action_feedback.feedback.initial_point.pose.orientation.y, _x.action_feedback.feedback.initial_point.pose.orientation.z, _x.action_feedback.feedback.initial_point.pose.orientation.w, _x.action_feedback.feedback.initial_point.wrench_or_stiffness.force.x, _x.action_feedback.feedback.initial_point.wrench_or_stiffness.force.y, _x.action_feedback.feedback.initial_point.wrench_or_stiffness.force.z, _x.action_feedback.feedback.initial_point.wrench_or_stiffness.torque.x, _x.action_feedback.feedback.initial_point.wrench_or_stiffness.torque.y, _x.action_feedback.feedback.initial_point.wrench_or_stiffness.torque.z, _x.action_feedback.feedback.initial_point.isForceX, _x.action_feedback.feedback.initial_point.isForceY, _x.action_feedback.feedback.initial_point.isForceZ, _x.action_feedback.feedback.initial_point.isTorqueX, _x.action_feedback.feedback.initial_point.isTorqueY, _x.action_feedback.feedback.initial_point.isTorqueZ, _x.action_feedback.feedback.initial_point.time_from_start.secs, _x.action_feedback.feedback.initial_point.time_from_start.nsecs, _x.action_feedback.feedback.desired.header.seq, _x.action_feedback.feedback.desired.header.stamp.secs, _x.action_feedback.feedback.desired.header.stamp.nsecs)) 00852 _x = self.action_feedback.feedback.desired.header.frame_id 00853 length = len(_x) 00854 buff.write(struct.pack('<I%ss'%length, length, _x)) 00855 _x = self 00856 buff.write(_struct_13d6B2i3I.pack(_x.action_feedback.feedback.desired.pose.position.x, _x.action_feedback.feedback.desired.pose.position.y, _x.action_feedback.feedback.desired.pose.position.z, _x.action_feedback.feedback.desired.pose.orientation.x, _x.action_feedback.feedback.desired.pose.orientation.y, _x.action_feedback.feedback.desired.pose.orientation.z, _x.action_feedback.feedback.desired.pose.orientation.w, _x.action_feedback.feedback.desired.wrench_or_stiffness.force.x, _x.action_feedback.feedback.desired.wrench_or_stiffness.force.y, _x.action_feedback.feedback.desired.wrench_or_stiffness.force.z, _x.action_feedback.feedback.desired.wrench_or_stiffness.torque.x, _x.action_feedback.feedback.desired.wrench_or_stiffness.torque.y, _x.action_feedback.feedback.desired.wrench_or_stiffness.torque.z, _x.action_feedback.feedback.desired.isForceX, _x.action_feedback.feedback.desired.isForceY, _x.action_feedback.feedback.desired.isForceZ, _x.action_feedback.feedback.desired.isTorqueX, _x.action_feedback.feedback.desired.isTorqueY, _x.action_feedback.feedback.desired.isTorqueZ, _x.action_feedback.feedback.desired.time_from_start.secs, _x.action_feedback.feedback.desired.time_from_start.nsecs, _x.action_feedback.feedback.actual_pose.header.seq, _x.action_feedback.feedback.actual_pose.header.stamp.secs, _x.action_feedback.feedback.actual_pose.header.stamp.nsecs)) 00857 _x = self.action_feedback.feedback.actual_pose.header.frame_id 00858 length = len(_x) 00859 buff.write(struct.pack('<I%ss'%length, length, _x)) 00860 _x = self 00861 buff.write(_struct_13d6B2id.pack(_x.action_feedback.feedback.actual_pose.pose.position.x, _x.action_feedback.feedback.actual_pose.pose.position.y, _x.action_feedback.feedback.actual_pose.pose.position.z, _x.action_feedback.feedback.actual_pose.pose.orientation.x, _x.action_feedback.feedback.actual_pose.pose.orientation.y, _x.action_feedback.feedback.actual_pose.pose.orientation.z, _x.action_feedback.feedback.actual_pose.pose.orientation.w, _x.action_feedback.feedback.actual_pose.wrench_or_stiffness.force.x, _x.action_feedback.feedback.actual_pose.wrench_or_stiffness.force.y, _x.action_feedback.feedback.actual_pose.wrench_or_stiffness.force.z, _x.action_feedback.feedback.actual_pose.wrench_or_stiffness.torque.x, _x.action_feedback.feedback.actual_pose.wrench_or_stiffness.torque.y, _x.action_feedback.feedback.actual_pose.wrench_or_stiffness.torque.z, _x.action_feedback.feedback.actual_pose.isForceX, _x.action_feedback.feedback.actual_pose.isForceY, _x.action_feedback.feedback.actual_pose.isForceZ, _x.action_feedback.feedback.actual_pose.isTorqueX, _x.action_feedback.feedback.actual_pose.isTorqueY, _x.action_feedback.feedback.actual_pose.isTorqueZ, _x.action_feedback.feedback.actual_pose.time_from_start.secs, _x.action_feedback.feedback.actual_pose.time_from_start.nsecs, _x.action_feedback.feedback.effort_sq_error)) 00862 length = len(self.action_feedback.feedback.requested_joint_efforts) 00863 buff.write(_struct_I.pack(length)) 00864 pattern = '<%sd'%length 00865 buff.write(self.action_feedback.feedback.requested_joint_efforts.tostring()) 00866 length = len(self.action_feedback.feedback.actual_joint_efforts) 00867 buff.write(_struct_I.pack(length)) 00868 pattern = '<%sd'%length 00869 buff.write(self.action_feedback.feedback.actual_joint_efforts.tostring()) 00870 _x = self 00871 buff.write(_struct_2i.pack(_x.action_feedback.feedback.running_time.secs, _x.action_feedback.feedback.running_time.nsecs)) 00872 except struct.error as se: self._check_types(se) 00873 except TypeError as te: self._check_types(te) 00874 00875 def deserialize_numpy(self, str, numpy): 00876 """ 00877 unpack serialized message in str into this message instance using numpy for array types 00878 @param str: byte array of serialized message 00879 @type str: str 00880 @param numpy: numpy python module 00881 @type numpy: module 00882 """ 00883 try: 00884 if self.action_goal is None: 00885 self.action_goal = ee_cart_imped_msgs.msg.EECartImpedActionGoal() 00886 if self.action_result is None: 00887 self.action_result = ee_cart_imped_msgs.msg.EECartImpedActionResult() 00888 if self.action_feedback is None: 00889 self.action_feedback = ee_cart_imped_msgs.msg.EECartImpedActionFeedback() 00890 end = 0 00891 _x = self 00892 start = end 00893 end += 12 00894 (_x.action_goal.header.seq, _x.action_goal.header.stamp.secs, _x.action_goal.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 00895 start = end 00896 end += 4 00897 (length,) = _struct_I.unpack(str[start:end]) 00898 start = end 00899 end += length 00900 self.action_goal.header.frame_id = str[start:end] 00901 _x = self 00902 start = end 00903 end += 8 00904 (_x.action_goal.goal_id.stamp.secs, _x.action_goal.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end]) 00905 start = end 00906 end += 4 00907 (length,) = _struct_I.unpack(str[start:end]) 00908 start = end 00909 end += length 00910 self.action_goal.goal_id.id = str[start:end] 00911 _x = self 00912 start = end 00913 end += 12 00914 (_x.action_goal.goal.header.seq, _x.action_goal.goal.header.stamp.secs, _x.action_goal.goal.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 00915 start = end 00916 end += 4 00917 (length,) = _struct_I.unpack(str[start:end]) 00918 start = end 00919 end += length 00920 self.action_goal.goal.header.frame_id = str[start:end] 00921 start = end 00922 end += 4 00923 (length,) = _struct_I.unpack(str[start:end]) 00924 self.action_goal.goal.trajectory = [] 00925 for i in range(0, length): 00926 val1 = ee_cart_imped_msgs.msg.StiffPoint() 00927 _v55 = val1.header 00928 start = end 00929 end += 4 00930 (_v55.seq,) = _struct_I.unpack(str[start:end]) 00931 _v56 = _v55.stamp 00932 _x = _v56 00933 start = end 00934 end += 8 00935 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 00936 start = end 00937 end += 4 00938 (length,) = _struct_I.unpack(str[start:end]) 00939 start = end 00940 end += length 00941 _v55.frame_id = str[start:end] 00942 _v57 = val1.pose 00943 _v58 = _v57.position 00944 _x = _v58 00945 start = end 00946 end += 24 00947 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 00948 _v59 = _v57.orientation 00949 _x = _v59 00950 start = end 00951 end += 32 00952 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 00953 _v60 = val1.wrench_or_stiffness 00954 _v61 = _v60.force 00955 _x = _v61 00956 start = end 00957 end += 24 00958 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 00959 _v62 = _v60.torque 00960 _x = _v62 00961 start = end 00962 end += 24 00963 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 00964 _x = val1 00965 start = end 00966 end += 6 00967 (_x.isForceX, _x.isForceY, _x.isForceZ, _x.isTorqueX, _x.isTorqueY, _x.isTorqueZ,) = _struct_6B.unpack(str[start:end]) 00968 val1.isForceX = bool(val1.isForceX) 00969 val1.isForceY = bool(val1.isForceY) 00970 val1.isForceZ = bool(val1.isForceZ) 00971 val1.isTorqueX = bool(val1.isTorqueX) 00972 val1.isTorqueY = bool(val1.isTorqueY) 00973 val1.isTorqueZ = bool(val1.isTorqueZ) 00974 _v63 = val1.time_from_start 00975 _x = _v63 00976 start = end 00977 end += 8 00978 (_x.secs, _x.nsecs,) = _struct_2i.unpack(str[start:end]) 00979 self.action_goal.goal.trajectory.append(val1) 00980 _x = self 00981 start = end 00982 end += 12 00983 (_x.action_result.header.seq, _x.action_result.header.stamp.secs, _x.action_result.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 00984 start = end 00985 end += 4 00986 (length,) = _struct_I.unpack(str[start:end]) 00987 start = end 00988 end += length 00989 self.action_result.header.frame_id = str[start:end] 00990 _x = self 00991 start = end 00992 end += 8 00993 (_x.action_result.status.goal_id.stamp.secs, _x.action_result.status.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end]) 00994 start = end 00995 end += 4 00996 (length,) = _struct_I.unpack(str[start:end]) 00997 start = end 00998 end += length 00999 self.action_result.status.goal_id.id = str[start:end] 01000 start = end 01001 end += 1 01002 (self.action_result.status.status,) = _struct_B.unpack(str[start:end]) 01003 start = end 01004 end += 4 01005 (length,) = _struct_I.unpack(str[start:end]) 01006 start = end 01007 end += length 01008 self.action_result.status.text = str[start:end] 01009 _x = self 01010 start = end 01011 end += 12 01012 (_x.action_result.result.header.seq, _x.action_result.result.header.stamp.secs, _x.action_result.result.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 01013 start = end 01014 end += 4 01015 (length,) = _struct_I.unpack(str[start:end]) 01016 start = end 01017 end += length 01018 self.action_result.result.header.frame_id = str[start:end] 01019 _x = self 01020 start = end 01021 end += 13 01022 (_x.action_result.result.success, _x.action_result.result.desired.header.seq, _x.action_result.result.desired.header.stamp.secs, _x.action_result.result.desired.header.stamp.nsecs,) = _struct_B3I.unpack(str[start:end]) 01023 self.action_result.result.success = bool(self.action_result.result.success) 01024 start = end 01025 end += 4 01026 (length,) = _struct_I.unpack(str[start:end]) 01027 start = end 01028 end += length 01029 self.action_result.result.desired.header.frame_id = str[start:end] 01030 _x = self 01031 start = end 01032 end += 194 01033 (_x.action_result.result.desired.pose.position.x, _x.action_result.result.desired.pose.position.y, _x.action_result.result.desired.pose.position.z, _x.action_result.result.desired.pose.orientation.x, _x.action_result.result.desired.pose.orientation.y, _x.action_result.result.desired.pose.orientation.z, _x.action_result.result.desired.pose.orientation.w, _x.action_result.result.desired.wrench_or_stiffness.force.x, _x.action_result.result.desired.wrench_or_stiffness.force.y, _x.action_result.result.desired.wrench_or_stiffness.force.z, _x.action_result.result.desired.wrench_or_stiffness.torque.x, _x.action_result.result.desired.wrench_or_stiffness.torque.y, _x.action_result.result.desired.wrench_or_stiffness.torque.z, _x.action_result.result.desired.isForceX, _x.action_result.result.desired.isForceY, _x.action_result.result.desired.isForceZ, _x.action_result.result.desired.isTorqueX, _x.action_result.result.desired.isTorqueY, _x.action_result.result.desired.isTorqueZ, _x.action_result.result.desired.time_from_start.secs, _x.action_result.result.desired.time_from_start.nsecs, _x.action_result.result.actual_pose.position.x, _x.action_result.result.actual_pose.position.y, _x.action_result.result.actual_pose.position.z, _x.action_result.result.actual_pose.orientation.x, _x.action_result.result.actual_pose.orientation.y, _x.action_result.result.actual_pose.orientation.z, _x.action_result.result.actual_pose.orientation.w, _x.action_result.result.effort_sq_error, _x.action_feedback.header.seq, _x.action_feedback.header.stamp.secs, _x.action_feedback.header.stamp.nsecs,) = _struct_13d6B2i8d3I.unpack(str[start:end]) 01034 self.action_result.result.desired.isForceX = bool(self.action_result.result.desired.isForceX) 01035 self.action_result.result.desired.isForceY = bool(self.action_result.result.desired.isForceY) 01036 self.action_result.result.desired.isForceZ = bool(self.action_result.result.desired.isForceZ) 01037 self.action_result.result.desired.isTorqueX = bool(self.action_result.result.desired.isTorqueX) 01038 self.action_result.result.desired.isTorqueY = bool(self.action_result.result.desired.isTorqueY) 01039 self.action_result.result.desired.isTorqueZ = bool(self.action_result.result.desired.isTorqueZ) 01040 start = end 01041 end += 4 01042 (length,) = _struct_I.unpack(str[start:end]) 01043 start = end 01044 end += length 01045 self.action_feedback.header.frame_id = str[start:end] 01046 _x = self 01047 start = end 01048 end += 8 01049 (_x.action_feedback.status.goal_id.stamp.secs, _x.action_feedback.status.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end]) 01050 start = end 01051 end += 4 01052 (length,) = _struct_I.unpack(str[start:end]) 01053 start = end 01054 end += length 01055 self.action_feedback.status.goal_id.id = str[start:end] 01056 start = end 01057 end += 1 01058 (self.action_feedback.status.status,) = _struct_B.unpack(str[start:end]) 01059 start = end 01060 end += 4 01061 (length,) = _struct_I.unpack(str[start:end]) 01062 start = end 01063 end += length 01064 self.action_feedback.status.text = str[start:end] 01065 _x = self 01066 start = end 01067 end += 12 01068 (_x.action_feedback.feedback.header.seq, _x.action_feedback.feedback.header.stamp.secs, _x.action_feedback.feedback.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 01069 start = end 01070 end += 4 01071 (length,) = _struct_I.unpack(str[start:end]) 01072 start = end 01073 end += length 01074 self.action_feedback.feedback.header.frame_id = str[start:end] 01075 start = end 01076 end += 4 01077 (length,) = _struct_I.unpack(str[start:end]) 01078 self.action_feedback.feedback.goal = [] 01079 for i in range(0, length): 01080 val1 = ee_cart_imped_msgs.msg.StiffPoint() 01081 _v64 = val1.header 01082 start = end 01083 end += 4 01084 (_v64.seq,) = _struct_I.unpack(str[start:end]) 01085 _v65 = _v64.stamp 01086 _x = _v65 01087 start = end 01088 end += 8 01089 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end]) 01090 start = end 01091 end += 4 01092 (length,) = _struct_I.unpack(str[start:end]) 01093 start = end 01094 end += length 01095 _v64.frame_id = str[start:end] 01096 _v66 = val1.pose 01097 _v67 = _v66.position 01098 _x = _v67 01099 start = end 01100 end += 24 01101 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 01102 _v68 = _v66.orientation 01103 _x = _v68 01104 start = end 01105 end += 32 01106 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end]) 01107 _v69 = val1.wrench_or_stiffness 01108 _v70 = _v69.force 01109 _x = _v70 01110 start = end 01111 end += 24 01112 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 01113 _v71 = _v69.torque 01114 _x = _v71 01115 start = end 01116 end += 24 01117 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end]) 01118 _x = val1 01119 start = end 01120 end += 6 01121 (_x.isForceX, _x.isForceY, _x.isForceZ, _x.isTorqueX, _x.isTorqueY, _x.isTorqueZ,) = _struct_6B.unpack(str[start:end]) 01122 val1.isForceX = bool(val1.isForceX) 01123 val1.isForceY = bool(val1.isForceY) 01124 val1.isForceZ = bool(val1.isForceZ) 01125 val1.isTorqueX = bool(val1.isTorqueX) 01126 val1.isTorqueY = bool(val1.isTorqueY) 01127 val1.isTorqueZ = bool(val1.isTorqueZ) 01128 _v72 = val1.time_from_start 01129 _x = _v72 01130 start = end 01131 end += 8 01132 (_x.secs, _x.nsecs,) = _struct_2i.unpack(str[start:end]) 01133 self.action_feedback.feedback.goal.append(val1) 01134 _x = self 01135 start = end 01136 end += 12 01137 (_x.action_feedback.feedback.initial_point.header.seq, _x.action_feedback.feedback.initial_point.header.stamp.secs, _x.action_feedback.feedback.initial_point.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 01138 start = end 01139 end += 4 01140 (length,) = _struct_I.unpack(str[start:end]) 01141 start = end 01142 end += length 01143 self.action_feedback.feedback.initial_point.header.frame_id = str[start:end] 01144 _x = self 01145 start = end 01146 end += 130 01147 (_x.action_feedback.feedback.initial_point.pose.position.x, _x.action_feedback.feedback.initial_point.pose.position.y, _x.action_feedback.feedback.initial_point.pose.position.z, _x.action_feedback.feedback.initial_point.pose.orientation.x, _x.action_feedback.feedback.initial_point.pose.orientation.y, _x.action_feedback.feedback.initial_point.pose.orientation.z, _x.action_feedback.feedback.initial_point.pose.orientation.w, _x.action_feedback.feedback.initial_point.wrench_or_stiffness.force.x, _x.action_feedback.feedback.initial_point.wrench_or_stiffness.force.y, _x.action_feedback.feedback.initial_point.wrench_or_stiffness.force.z, _x.action_feedback.feedback.initial_point.wrench_or_stiffness.torque.x, _x.action_feedback.feedback.initial_point.wrench_or_stiffness.torque.y, _x.action_feedback.feedback.initial_point.wrench_or_stiffness.torque.z, _x.action_feedback.feedback.initial_point.isForceX, _x.action_feedback.feedback.initial_point.isForceY, _x.action_feedback.feedback.initial_point.isForceZ, _x.action_feedback.feedback.initial_point.isTorqueX, _x.action_feedback.feedback.initial_point.isTorqueY, _x.action_feedback.feedback.initial_point.isTorqueZ, _x.action_feedback.feedback.initial_point.time_from_start.secs, _x.action_feedback.feedback.initial_point.time_from_start.nsecs, _x.action_feedback.feedback.desired.header.seq, _x.action_feedback.feedback.desired.header.stamp.secs, _x.action_feedback.feedback.desired.header.stamp.nsecs,) = _struct_13d6B2i3I.unpack(str[start:end]) 01148 self.action_feedback.feedback.initial_point.isForceX = bool(self.action_feedback.feedback.initial_point.isForceX) 01149 self.action_feedback.feedback.initial_point.isForceY = bool(self.action_feedback.feedback.initial_point.isForceY) 01150 self.action_feedback.feedback.initial_point.isForceZ = bool(self.action_feedback.feedback.initial_point.isForceZ) 01151 self.action_feedback.feedback.initial_point.isTorqueX = bool(self.action_feedback.feedback.initial_point.isTorqueX) 01152 self.action_feedback.feedback.initial_point.isTorqueY = bool(self.action_feedback.feedback.initial_point.isTorqueY) 01153 self.action_feedback.feedback.initial_point.isTorqueZ = bool(self.action_feedback.feedback.initial_point.isTorqueZ) 01154 start = end 01155 end += 4 01156 (length,) = _struct_I.unpack(str[start:end]) 01157 start = end 01158 end += length 01159 self.action_feedback.feedback.desired.header.frame_id = str[start:end] 01160 _x = self 01161 start = end 01162 end += 130 01163 (_x.action_feedback.feedback.desired.pose.position.x, _x.action_feedback.feedback.desired.pose.position.y, _x.action_feedback.feedback.desired.pose.position.z, _x.action_feedback.feedback.desired.pose.orientation.x, _x.action_feedback.feedback.desired.pose.orientation.y, _x.action_feedback.feedback.desired.pose.orientation.z, _x.action_feedback.feedback.desired.pose.orientation.w, _x.action_feedback.feedback.desired.wrench_or_stiffness.force.x, _x.action_feedback.feedback.desired.wrench_or_stiffness.force.y, _x.action_feedback.feedback.desired.wrench_or_stiffness.force.z, _x.action_feedback.feedback.desired.wrench_or_stiffness.torque.x, _x.action_feedback.feedback.desired.wrench_or_stiffness.torque.y, _x.action_feedback.feedback.desired.wrench_or_stiffness.torque.z, _x.action_feedback.feedback.desired.isForceX, _x.action_feedback.feedback.desired.isForceY, _x.action_feedback.feedback.desired.isForceZ, _x.action_feedback.feedback.desired.isTorqueX, _x.action_feedback.feedback.desired.isTorqueY, _x.action_feedback.feedback.desired.isTorqueZ, _x.action_feedback.feedback.desired.time_from_start.secs, _x.action_feedback.feedback.desired.time_from_start.nsecs, _x.action_feedback.feedback.actual_pose.header.seq, _x.action_feedback.feedback.actual_pose.header.stamp.secs, _x.action_feedback.feedback.actual_pose.header.stamp.nsecs,) = _struct_13d6B2i3I.unpack(str[start:end]) 01164 self.action_feedback.feedback.desired.isForceX = bool(self.action_feedback.feedback.desired.isForceX) 01165 self.action_feedback.feedback.desired.isForceY = bool(self.action_feedback.feedback.desired.isForceY) 01166 self.action_feedback.feedback.desired.isForceZ = bool(self.action_feedback.feedback.desired.isForceZ) 01167 self.action_feedback.feedback.desired.isTorqueX = bool(self.action_feedback.feedback.desired.isTorqueX) 01168 self.action_feedback.feedback.desired.isTorqueY = bool(self.action_feedback.feedback.desired.isTorqueY) 01169 self.action_feedback.feedback.desired.isTorqueZ = bool(self.action_feedback.feedback.desired.isTorqueZ) 01170 start = end 01171 end += 4 01172 (length,) = _struct_I.unpack(str[start:end]) 01173 start = end 01174 end += length 01175 self.action_feedback.feedback.actual_pose.header.frame_id = str[start:end] 01176 _x = self 01177 start = end 01178 end += 126 01179 (_x.action_feedback.feedback.actual_pose.pose.position.x, _x.action_feedback.feedback.actual_pose.pose.position.y, _x.action_feedback.feedback.actual_pose.pose.position.z, _x.action_feedback.feedback.actual_pose.pose.orientation.x, _x.action_feedback.feedback.actual_pose.pose.orientation.y, _x.action_feedback.feedback.actual_pose.pose.orientation.z, _x.action_feedback.feedback.actual_pose.pose.orientation.w, _x.action_feedback.feedback.actual_pose.wrench_or_stiffness.force.x, _x.action_feedback.feedback.actual_pose.wrench_or_stiffness.force.y, _x.action_feedback.feedback.actual_pose.wrench_or_stiffness.force.z, _x.action_feedback.feedback.actual_pose.wrench_or_stiffness.torque.x, _x.action_feedback.feedback.actual_pose.wrench_or_stiffness.torque.y, _x.action_feedback.feedback.actual_pose.wrench_or_stiffness.torque.z, _x.action_feedback.feedback.actual_pose.isForceX, _x.action_feedback.feedback.actual_pose.isForceY, _x.action_feedback.feedback.actual_pose.isForceZ, _x.action_feedback.feedback.actual_pose.isTorqueX, _x.action_feedback.feedback.actual_pose.isTorqueY, _x.action_feedback.feedback.actual_pose.isTorqueZ, _x.action_feedback.feedback.actual_pose.time_from_start.secs, _x.action_feedback.feedback.actual_pose.time_from_start.nsecs, _x.action_feedback.feedback.effort_sq_error,) = _struct_13d6B2id.unpack(str[start:end]) 01180 self.action_feedback.feedback.actual_pose.isForceX = bool(self.action_feedback.feedback.actual_pose.isForceX) 01181 self.action_feedback.feedback.actual_pose.isForceY = bool(self.action_feedback.feedback.actual_pose.isForceY) 01182 self.action_feedback.feedback.actual_pose.isForceZ = bool(self.action_feedback.feedback.actual_pose.isForceZ) 01183 self.action_feedback.feedback.actual_pose.isTorqueX = bool(self.action_feedback.feedback.actual_pose.isTorqueX) 01184 self.action_feedback.feedback.actual_pose.isTorqueY = bool(self.action_feedback.feedback.actual_pose.isTorqueY) 01185 self.action_feedback.feedback.actual_pose.isTorqueZ = bool(self.action_feedback.feedback.actual_pose.isTorqueZ) 01186 start = end 01187 end += 4 01188 (length,) = _struct_I.unpack(str[start:end]) 01189 pattern = '<%sd'%length 01190 start = end 01191 end += struct.calcsize(pattern) 01192 self.action_feedback.feedback.requested_joint_efforts = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 01193 start = end 01194 end += 4 01195 (length,) = _struct_I.unpack(str[start:end]) 01196 pattern = '<%sd'%length 01197 start = end 01198 end += struct.calcsize(pattern) 01199 self.action_feedback.feedback.actual_joint_efforts = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 01200 _x = self 01201 start = end 01202 end += 8 01203 (_x.action_feedback.feedback.running_time.secs, _x.action_feedback.feedback.running_time.nsecs,) = _struct_2i.unpack(str[start:end]) 01204 return self 01205 except struct.error as e: 01206 raise roslib.message.DeserializationError(e) #most likely buffer underfill 01207 01208 _struct_I = roslib.message.struct_I 01209 _struct_6B = struct.Struct("<6B") 01210 _struct_B = struct.Struct("<B") 01211 _struct_13d6B2id = struct.Struct("<13d6B2id") 01212 _struct_2i = struct.Struct("<2i") 01213 _struct_3I = struct.Struct("<3I") 01214 _struct_B3I = struct.Struct("<B3I") 01215 _struct_13d6B2i3I = struct.Struct("<13d6B2i3I") 01216 _struct_13d6B2i8d3I = struct.Struct("<13d6B2i8d3I") 01217 _struct_4d = struct.Struct("<4d") 01218 _struct_2I = struct.Struct("<2I") 01219 _struct_3d = struct.Struct("<3d")