def execute(self): cur = self._current_value if cur == self.States.IDLE: pass elif cur == self.States.WAITING_TO_ARRIVE: if self.mav1.has_arrived(): self.set_current_state(self._next_value) elif cur == self.States.TAKE_OFF: yaw = -1.57079633 self.mav1.set_target_pose( create_setpoint_message_xyz_yaw(0, 0, 1, yaw)) self.set_current_state(self.States.WAITING_TO_ARRIVE) self.set_next_state(self.States.BEAM) elif cur == self.States.BEAM: target = Point(4.6, 1, 1.0) self.mav1.set_target_pos(target) self.set_current_state(self.States.WAITING_TO_ARRIVE) self.set_next_state(self.States.BACK) self.pose_error = None elif cur == self.States.BACK: target = Point(-4.6, 1, 1.0) self.mav1.set_target_pos(target) self.set_current_state(self.States.WAITING_TO_ARRIVE) self.set_next_state(self.States.BEAM) self.pose_error = None elif cur == self.States.ALIGN_YAW: if self.pose_error == None: return if not self.mav1.has_arrived(): return angle_error = orientation_to_yaw(self.pose_error.orientation) self.pose_error = None rospy.loginfo("angle error " + str(angle_error)) if abs(angle_error) < self.max_align_error: self.set_current_state(self.States.ALIGN_POS) rospy.loginfo("YAW ALIGNED") self.max_align_error -= 0.01 return pid_out = self.yaw_pid(angle_error) current_yaw = orientation_to_yaw( self.mav1.current_pose.pose.orientation) target_yaw = current_yaw + pid_out self.mav1.set_target_yaw(target_yaw) elif cur == self.States.ALIGN_POS: if self.pose_error == None: return if not self.mav1.has_arrived(): return err = point_to_arr(self.pose_error.position) self.pose_error = None magnitude = np.linalg.norm(err) rospy.loginfo("pos error " + str(magnitude)) if magnitude < self.max_align_error * 20: self.set_current_state(self.States.ALIGN_YAW) rospy.loginfo("POS ALIGNED") self.max_align_error -= 0.01 return current_pos = point_to_arr(self.mav1.current_pose.pose.position) yaw = orientation_to_yaw(self.mav1.current_pose.pose.orientation) err[0] = cos(yaw) * err[0] + sin(yaw) * err[1] err[1] = -sin(yaw) * err[0] + cos(yaw) * err[1] pid_out = self.pos_pid(magnitude) ratio = abs(pid_out / magnitude) target = current_pos + ratio * err target_point = arr_to_point(target) self.mav1.set_target_pos(target_point)
def set_target_pos(self, pos=Point()): yaw = orientation_to_yaw(self.target_pose.pose.orientation) pose = create_setpoint_message_pos_yaw(pos, yaw) self.set_target_pose(pose)
def execute(self): cur = self._current_value if cur == self.States.IDLE: pass elif cur == self.States.WAITING_TO_ARRIVE: if self._mav1.has_arrived() and self._mav2.has_arrived(): self.set_current_state(self._next_value) elif cur == self.States.TAKE_OFF: target = Point(0, 0, 10) self._mav1.set_target_pos(target) self._mav2.set_target_pos(target) self.set_current_state(self.States.WAITING_TO_ARRIVE) self.set_next_state(self.States.UNDER_WIRE) elif cur == self.States.UNDER_WIRE: target = Point(-35.2, 30, 11) self._mav1.set_target_pos(target) self._mav2.set_target_pos(target) self.set_current_state(self.States.WAITING_TO_ARRIVE) self.set_next_state(self.States.ALIGN_YAW) self.pose_error = None elif cur == self.States.ALIGN_YAW: if self.pose_error == None: return if not self._mav1.has_arrived(): return angle_error = orientation_to_yaw(self.pose_error.orientation) self.pose_error = None print("angle error " + str(angle_error)) if abs(angle_error) < self.max_align_error: self.set_current_state(self.States.ALIGN_POS) print("YAW ALIGNED") self.max_align_error -= 0.01 return pid_out = self.yaw_pid(angle_error) current_yaw = orientation_to_yaw( self._mav1.current_pose.pose.orientation) target_yaw = current_yaw + pid_out self._mav1.set_target_yaw(target_yaw) self._mav2.set_target_yaw(target_yaw) elif cur == self.States.ALIGN_POS: if self.pose_error == None: return if not self._mav1.has_arrived(): return err = point_to_arr(self.pose_error.position) self.pose_error = None magnitude = np.linalg.norm(err) print("pos error " + str(magnitude)) if magnitude < self.max_align_error * 20: self.set_current_state(self.States.ALIGN_YAW) print("POS ALIGNED") self.max_align_error -= 0.01 return current_pos = point_to_arr(self._mav1.current_pose.pose.position) yaw = orientation_to_yaw(self._mav1.current_pose.pose.orientation) err[0] = cos(yaw) * err[0] + sin(yaw) * err[1] err[1] = -sin(yaw) * err[0] + cos(yaw) * err[1] pid_out = self.pos_pid(magnitude) ratio = abs(pid_out / magnitude) target = current_pos + ratio * err target_point = arr_to_point(target) self._mav1.set_target_pos(target_point) self._mav2.set_target_pos(target_point)
def get_yaw_error(self): sy = orientation_to_yaw(self.target_pose.pose.orientation) cy = orientation_to_yaw(self.current_pose.pose.orientation) yaw = abs(sy - cy) return yaw