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