class MotionCommandCoordinator(object):
def __init__(self, action_server):
# action server for getting requests from AI
self._action_server = action_server
# current state of motion coordinator
self._task = None
self._first_task_seen = False
# used for timeouts & extrapolating
self._time_of_last_task = None
self._timeout_vel_sent = False
# to keep things thread safe
self._lock = threading.RLock()
# handles monitoring of state of drone
self._state_monitor = StateMonitor()
# handles communicating between tasks and LLM
self._task_command_handler = TaskCommandHandler()
self._idle_obstacle_avoider = IdleObstacleAvoider()
self._avoid_magnitude = rospy.get_param("~obst_avoid_magnitude")
self._kickout_distance = rospy.get_param('~kickout_distance')
self._new_task_distance = rospy.get_param('~new_task_distance')
self._safe_distance = rospy.get_param('~safe_distance')
# safety
self._safety_client = SafetyClient('motion_command_coordinator')
self._safety_land_complete = False
self._safety_land_requested = False
# Create action client to request a safety landing
self._action_client = actionlib.SimpleActionClient(
"motion_planner_server", QuadMoveAction)
try:
# update rate for motion coordinator
self._update_rate = rospy.get_param('~update_rate')
# task timeout values
self._task_timeout = rospy.Duration(
rospy.get_param('~task_timeout'))
# startup timeout
self._startup_timeout = rospy.Duration(
rospy.get_param('~startup_timeout'))
except KeyError as e:
rospy.logerr('Could not lookup a parameter for motion coordinator')
raise
def run(self):
# rate limiting of updates of motion coordinator
rate = rospy.Rate(self._update_rate)
# waiting for dependencies to be ready
while rospy.Time.now() == rospy.Time(0) and not rospy.is_shutdown():
rospy.sleep(0.005)
if rospy.is_shutdown():
raise rospy.ROSInterruptException()
start_time = rospy.Time.now()
if not self._action_client.wait_for_server(self._startup_timeout):
raise IARCFatalSafetyException(
'Motion Coordinator could not initialize action client')
self._state_monitor.wait_until_ready(self._startup_timeout -
(rospy.Time.now() - start_time))
self._task_command_handler.wait_until_ready(self._startup_timeout -
(rospy.Time.now() -
start_time))
self._idle_obstacle_avoider.wait_until_ready(self._startup_timeout -
(rospy.Time.now() -
start_time))
AbstractTask().topic_buffer.wait_until_ready(self._startup_timeout -
(rospy.Time.now() -
start_time))
# forming bond with safety client
if not self._safety_client.form_bond():
raise IARCFatalSafetyException(
'Motion Coordinator could not form bond with safety client')
while not rospy.is_shutdown():
with self._lock:
# Exit immediately if fatal
if self._safety_client.is_fatal_active():
raise IARCFatalSafetyException(
'Safety Client is fatal active')
elif self._safety_land_complete:
return
# Land if put into safety mode
if self._safety_client.is_safety_active(
) and not self._safety_land_requested:
# Request landing
goal = QuadMoveGoal(movement_type="land", preempt=True)
self._action_client.send_goal(
goal, done_cb=self._safety_task_complete_callback)
rospy.logwarn(
'motion coordinator attempting to execute safety land')
self._safety_land_requested = True
self._state_monitor.signal_safety_active()
# set the time of last task to now if we have not seen a task yet
if not self._first_task_seen:
self._time_of_last_task = rospy.Time.now()
self._first_task_seen = True
closest_obstacle_dist = self._idle_obstacle_avoider.get_distance_to_obstacle(
)
if self._task is None and self._action_server.has_new_task():
new_task = self._action_server.get_new_task()
if not self._state_monitor.check_transition(new_task):
rospy.logerr(
'Illegal task transition request requested in motion coordinator. Aborting requested task.'
)
self._action_server.set_aborted()
elif not closest_obstacle_dist >= self._new_task_distance:
rospy.logerr(
'Attempt to start task too close to obstacle.' +
' Aborting requested task.')
self._action_server.set_aborted()
else:
self._time_of_last_task = None
self._task = new_task
self._task_command_handler.new_task(
new_task, self._get_current_transition())
if self._task is not None:
task_canceled = False
if self._action_server.is_canceled():
task_canceled = self._task_command_handler.cancel_task(
)
if not task_canceled and closest_obstacle_dist < self._kickout_distance:
# Abort current task
task_canceled = self._task_command_handler.abort_task(
'Too close to obstacle')
# if canceling task did not result in an error
if not task_canceled:
self._task_command_handler.run()
task_state = self._task_command_handler.get_state()
# handles state of task, motion coordinator, and action server
if isinstance(task_state, task_states.TaskCanceled):
self._action_server.set_canceled()
rospy.logwarn('Task was canceled')
self._task = None
elif isinstance(task_state, task_states.TaskAborted):
rospy.logwarn('Task aborted with: %s', task_state.msg)
self._action_server.set_aborted()
self._task = None
elif isinstance(task_state, task_states.TaskFailed):
rospy.logwarn('Task failed with: %s', task_state.msg)
self._action_server.set_succeeded(False)
self._task = None
elif isinstance(task_state, task_states.TaskDone):
self._action_server.set_succeeded(True)
self._task = None
elif not isinstance(task_state, task_states.TaskRunning):
rospy.logerr(
"Invalid task state returned, aborting task")
self._action_server.set_aborted()
self._task = None
task_state = task_states.TaskAborted(
msg='Invalid task state returned')
# as soon as we set a task to None, start time
# and send ending state to State Monitor
if self._task is None:
self._time_of_last_task = rospy.Time.now()
self._timeout_vel_sent = False
self._state_monitor.set_last_task_end_state(task_state)
# No task is running, run obstacle avoider
else:
vel = AbstractTask.topic_buffer.get_linear_motion_profile_generator(
).expected_point_at_time(
rospy.Time.now()).motion_point.twist.linear
vel_vec_2d = np.array([vel.x, vel.y], dtype=np.float)
avoid_vector, acceleration = self._idle_obstacle_avoider.get_safest(
vel_vec_2d)
avoid_twist = TwistStamped()
avoid_twist.header.stamp = rospy.Time.now()
avoid_twist.twist.linear.x = avoid_vector[0]
avoid_twist.twist.linear.y = avoid_vector[1]
self._task_command_handler.send_timeout(
avoid_twist, acceleration=acceleration)
rospy.logwarn_throttle(
1.0, 'Task running timeout. Running obstacle avoider')
rate.sleep()
# fills out the Intermediary State for the task
def _get_current_transition(self):
state = TransitionData()
state.last_twist = self._task_command_handler.get_last_twist()
state.last_task_ending_state = self._task_command_handler.get_state()
state.timeout_sent = self._timeout_vel_sent
return self._state_monitor.fill_out_transition(state)
# callback for safety task completition
def _safety_task_complete_callback(self, status, response):
with self._lock:
if response.success:
rospy.logwarn(
'Motion Coordinator supposedly safely landed the aircraft')
else:
rospy.logerr('Motion Coordinator did not safely land aircraft')
self._safety_land_complete = True
