def check_transition(self, task):
with self._lock:
passed = True
if self._state == RobotStates.SAFETY_ACTIVE:
if not isinstance(task, LandTask):
raise IARCFatalSafetyException(
'StateMonitor has been told we are safety active and landing was not requested')
elif self._state == RobotStates.WAITING_ON_TAKEOFF:
if not isinstance(task, TakeoffTask):
passed = False
elif (self._state == RobotStates.TAKEOFF_FAILED or
self._state == RobotStates.LANDING_FAILED
or self._state == RobotStates.RECOVERY_FAILED):
raise IARCSafetyException('A critical task failed')
elif self._state == RobotStates.WAITING_ON_RECOVERY:
if not isinstance(task, HeightRecoveryTask):
passed = False
elif self._state == RobotStates.NORMAL:
pass
elif self._state == RobotStates.FATAL:
raise IARCFatalSafetyException('StateMonitor determined robot is in a fatal state')
else:
raise IARCFatalSafetyException('StateMonitor is in an invalid state')
if passed:
self._last_task = task
return passed
def abort_task(self, msg):
'''
Aborts task
Returns true if task is actually gone, false otherwise
'''
if self._task is None:
raise IARCFatalSafetyException('No task running to abort')
try:
ready = self._task.cancel()
if ready:
self._task = None
self._task_state = task_states.TaskAborted(msg)
return True
else:
rospy.logwarn('Task has not completed')
return False
except Exception as e:
rospy.logerr('Exception aborting task')
rospy.logerr(str(e))
rospy.logerr(traceback.format_exc())
rospy.logerr('Task Command Handler aborted task')
self._task_state = task_states.TaskAborted(
msg=msg + ', then error aborting task')
self._task = None
return True
assert False
def wait_until_ready(self, startup_timeout):
while (rospy.Time.now() == rospy.Time(0) and not rospy.is_shutdown()):
# Wait for time to be initialized
rospy.sleep(0.005)
if rospy.is_shutdown():
raise rospy.ROSInterruptException()
start_time = rospy.Time.now()
while ((self._obstacle_points is None)
and rospy.Time.now() < start_time + startup_timeout
and not rospy.is_shutdown()):
rospy.sleep(0.005)
if rospy.Time.now() >= start_time + startup_timeout:
raise IARCFatalSafetyException(
'SafetyMonitor timed out on startup')
if rospy.is_shutdown():
raise rospy.ROSInterruptException()
def wait_until_ready(self, startup_timeout):
while (rospy.Time.now() == rospy.Time(0) and not rospy.is_shutdown()):
# Wait for time to be initialized
rospy.sleep(0.005)
if rospy.is_shutdown():
raise rospy.ROSInterruptException()
start_time = rospy.Time.now()
while ((self._drone_odometry is None
or self._roombas is None
or self._obstacles is None
or self._arm_status is None)
and rospy.Time.now() < start_time + startup_timeout
and not rospy.is_shutdown()):
rospy.sleep(0.005)
if rospy.Time.now() >= start_time + startup_timeout:
rospy.logerr('Drone odometry: %s', self._drone_odometry is not None)
rospy.logerr('Roombas: %s', self._roombas is not None)
rospy.logerr('Obstacles: %s', self._obstacles is not None)
rospy.logerr('Arm status: %s', self._arm_status is not None)
raise IARCFatalSafetyException('SafetyMonitor timed out on startup')
if rospy.is_shutdown():
raise rospy.ROSInterruptException()
def run(self):
# this check is needed, as sometimes there are issues with simulated time
while rospy.Time.now() == rospy.Time(0) and not rospy.is_shutdown():
rospy.logwarn('TaskManager: waiting on time')
rospy.sleep(0.005)
if rospy.is_shutdown():
raise rospy.ROSInterruptException()
# check to make sure all dependencies are ready
if not self._wait_until_ready(self._timeout):
raise rospy.ROSInitException()
# forming bond with safety client, if enabled
if self._safety_enabled and not self._safety_client.form_bond():
raise IARCFatalSafetyException(
'TaskManager: could not form bond with safety client')
while not rospy.is_shutdown():
# main loop
with self._lock:
if self._safety_enabled and self._safety_client.is_fatal_active(
):
raise IARCFatalSafetyException('TaskManager: Safety Fatal')
if self._safety_enabled and self._safety_client.is_safety_active(
):
rospy.logerr('TaskManager: Activating safety response')
self._safety_responder.activate_safety_response()
return
if self._task is None and self._action_server.has_new_task():
# new task is available
try:
request = self._action_server.get_new_task()
# use provided handler to check that request is valid
new_task = self._handler.check_request(request)
if new_task is not None:
self._task = new_task
self._action_server.set_accepted()
else:
rospy.logerr('TaskManager: new task is invalid')
self._action_server.set_rejected()
except Exception as e:
rospy.logfatal(
'TaskManager: Exception getting new task')
raise
if self._task is not None and self._action_server.task_canceled(
):
# there is a task running, but it is canceled
try:
success = self._task.cancel()
if not success:
rospy.logwarn(
'TaskManager: task refusing to cancel')
if self._force_cancel:
rospy.logwarn(
'TaskManager: forcing task cancel')
self._action_server.set_canceled()
self._task = None
elif success:
self._action_server.set_canceled()
self._task = None
except Exception as e:
rospy.logfatal('TaskManager: Error canceling task')
raise
if self._task is not None:
# can get next command and task state
try:
state, command = self._task.get_desired_command()
except Exception as e:
rospy.logerr(
'TaskManager: Error getting command from task. Aborting task'
)
rospy.logerr(str(e))
self._action_server.set_aborted()
self._task = None
try:
task_state = self._handler.handle(command, state)
except Exception as e:
# this is a fatal error, as the handler should be able to
# handle commands of valid tasks without raising exceptions
rospy.logfatal(
'TaskManager: Error handling task command')
rospy.logfatal(str(e))
raise
if task_state == TaskHandledState.ABORTED:
rospy.logerr('TaskManager: aborting task')
self._action_server.set_aborted()
self._task = None
elif task_state == TaskHandledState.DONE:
rospy.loginfo(
'TaskManager: task has completed cleanly')
self._action_server.set_succeeded()
self._task = None
elif task_state == TaskHandledState.FAILED:
rospy.logerr('TaskManager: Task failed')
self._action_server.set_failed()
self._task = None
elif task_state == TaskHandledState.OKAY:
rospy.loginfo_throttle(1, 'TaskManager: task running')
else:
raise IARCFatalSafetyException(
'TaskManager: invalid task handled state returned from handler: '
+ str(task_state))
self._update_rate.sleep()
def wait_until_ready(self, startup_timeout):
if not self._ground_interaction_client.wait_for_server(
startup_timeout):
raise IARCFatalSafetyException(
'TaskCommandHandler could not initialize action client')
def _get_task_command(self):
if self._task is not None:
try:
task_request = self._task.get_desired_command()
except Exception as e:
rospy.logerr('Exception getting task command')
rospy.logerr(str(e))
rospy.logerr(traceback.format_exc())
rospy.logerr('Task Command Handler aborted task')
self._task = None
self._task_state = task_states.TaskAborted(
msg='Exception getting task command')
return (task_commands.NopCommand(), )
try:
_task_state = task_request[0]
except (TypeError, IndexError) as e:
rospy.logerr('Exception getting task state')
rospy.logerr(str(e))
rospy.logerr(traceback.format_exc())
rospy.logerr('Task Command Handler aborted task')
self._task = None
self._task_state = task_states.TaskAborted(
msg='Exception getting task state')
return (task_commands.NopCommand(), )
if issubclass(type(_task_state), task_states.TaskState):
self._task_state = _task_state
else:
rospy.logerr('Task provided unknown state')
rospy.logerr('Task Command Handler aborted task')
self._task = None
self._task_state = task_states.TaskAborted(
msg='Error getting task state')
return (task_commands.NopCommand(), )
try:
if isinstance(self._task_state, task_states.TaskDone):
self._task = None
return task_request[1:]
elif isinstance(self._task_state, task_states.TaskRunning):
return task_request[1:]
else:
self._task = None
except (TypeError, IndexError) as e:
rospy.logerr('Exception getting task request')
rospy.logerr(str(e))
rospy.logerr(traceback.format_exc())
rospy.logerr('Task Command Handler aborted task')
self._task = None
self._task_state = task_states.TaskAborted(
msg='Exception getting task request')
return (task_commands.NopCommand(), )
elif isinstance(self._task_state, task_states.TaskCanceled):
pass
else:
raise IARCFatalSafetyException(
'TaskCommandHandler ran with no task running.')
# no action to take, return a Nop
return (task_commands.NopCommand(), )
def set_last_task_end_state(self, state):
with self._lock:
self._last_task_end_state = state
if self._state == RobotStates.SAFETY_ACTIVE:
pass
# If the tasked completed succesfully
elif isinstance(state, task_states.TaskDone):
if isinstance(self._last_task, LandTask):
self._state = RobotStates.WAITING_ON_TAKEOFF
elif (isinstance(self._last_task, BlockRoombaTask)):
self._state = RobotStates.WAITING_ON_RECOVERY
else:
self._state = RobotStates.NORMAL
# If the task was canceled, there might be cleanup actions
# to perform
elif isinstance(state, task_states.TaskCanceled):
# assumes that canceling land results in velocity mode
if isinstance(self._last_task, LandTask):
self._state = RobotStates.NORMAL
# Takeoff needs to take the drone above the safe height
# Hit roomba if canceled might not have taken the drone back up
elif (isinstance(self._last_task, TakeoffTask)
or isinstance(self._last_task, HitRoombaTask)):
if not self._BELOW_MIN_MAN_HEIGHT:
self._state = RobotStates.NORMAL
else:
rospy.logerr('Takeoff did not finish when it was canceled')
self._state = RobotStates.FATAL
# Block roomba can never bring the drone back up
elif isinstance(self._last_task, BlockRoombaTask):
self._state = RobotStates.WAITING_ON_RECOVERY
else:
self._state = RobotStates.NORMAL
# If the task aborted there is probably a situation that
# needs to be responded to
elif (isinstance(state, task_states.TaskAborted)
or isinstance(state, task_states.TaskFailed)):
if isinstance(self._last_task, TakeoffTask):
self._state = RobotStates.TAKEOFF_FAILED
elif isinstance(self._last_task, LandTask):
self._state = RobotStates.LANDING_FAILED
elif isinstance(self._last_task, HeightRecoveryTask):
self._state = RobotStates.RECOVERY_FAILED
elif isinstance(self._last_task, BlockRoombaTask):
self._state = RobotStates.WAITING_ON_RECOVERY
# Hit handles its own abort due to the tricky situation
elif isinstance(self._last_task, HitRoombaTask):
self._state = RobotStates.NORMAL
# The task returned something that can't be interpreted
else:
rospy.logerr('Invalid ending task state provided in StateMonitor')
self._state = RobotStates.FATAL
rospy.loginfo('RobotState: ' + str(self._state))
if self._state == RobotStates.FATAL:
raise IARCFatalSafetyException('StateMonitor determined robot is in a fatal state')
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()
def run(self):
# Initialize the low-level drivers (don't list the debug drivers)
cflib.crtp.init_drivers(enable_debug_driver=False)
scan_rate = rospy.Rate(2)
while not rospy.is_shutdown():
# Use first Crazyflie found
rospy.loginfo('Crazyflie FC Comms scanning for Crazyflies')
crazyflies = cflib.crtp.scan_interfaces()
if len(crazyflies) != 0:
break
rospy.logerr(
'Crazyflie FC Comms could not find Crazyflie. Trying again.')
scan_rate.sleep()
uri = crazyflies[0][0]
self._cf = Crazyflie()
self._cf.connected.add_callback(self._connection_made)
self._cf.disconnected.add_callback(self._disconnected)
self._cf.connection_failed.add_callback(self._connection_failed)
self._cf.connection_lost.add_callback(self._connection_lost)
rospy.loginfo('Crazyflie FC Comms connecting to: {}'.format(uri))
self._cf.open_link(uri)
connected_check_rate = rospy.Rate(2)
while not self._connected and not rospy.is_shutdown():
connected_check_rate.sleep()
pass
rospy.loginfo('Crazyflie FC Comms reseting kalmann filter')
self._cf.param.set_value('kalman.resetEstimation', '1')
time.sleep(0.1)
self._cf.param.set_value('kalman.resetEstimation', '0')
time.sleep(2)
fast_log_stab = LogConfig(name='high_update_rate', period_in_ms=20)
medium_log_stab = LogConfig(name='medium_update_rate', period_in_ms=30)
slow_log_stab = LogConfig(name='slow_update_rate', period_in_ms=100)
fast_log_stab.add_variable('acc.x', 'float')
fast_log_stab.add_variable('acc.y', 'float')
fast_log_stab.add_variable('acc.z', 'float')
fast_log_stab.add_variable('stabilizer.roll', 'float')
fast_log_stab.add_variable('stabilizer.pitch', 'float')
fast_log_stab.add_variable('stabilizer.yaw', 'float')
# kalman_states.ox and kalman_states.oy are also available
# and provide position estimates
medium_log_stab.add_variable('kalman_states.vx', 'float')
medium_log_stab.add_variable('kalman_states.vy', 'float')
medium_log_stab.add_variable('range.zrange', 'uint16_t')
slow_log_stab.add_variable('pm.vbat', 'float')
slow_log_stab.add_variable('mag.x', 'float')
slow_log_stab.add_variable('mag.y', 'float')
slow_log_stab.add_variable('mag.z', 'float')
try:
self._cf.log.add_config(fast_log_stab)
self._cf.log.add_config(medium_log_stab)
self._cf.log.add_config(slow_log_stab)
fast_log_stab.data_received_cb.add_callback(
self._receive_crazyflie_data)
medium_log_stab.data_received_cb.add_callback(
self._receive_crazyflie_data)
slow_log_stab.data_received_cb.add_callback(
self._receive_crazyflie_data)
fast_log_stab.error_cb.add_callback(self._receive_crazyflie_error)
medium_log_stab.error_cb.add_callback(
self._receive_crazyflie_error)
slow_log_stab.error_cb.add_callback(self._receive_crazyflie_error)
fast_log_stab.start()
medium_log_stab.start()
slow_log_stab.start()
except KeyError as e:
rospy.logerr('Crazyflie FC Comms could not start logging,'
'{} not found'.format(str(e)))
except AttributeError as e:
rospy.logerr(
'Crazyflie FC Comms could not finishing configuring logging: {}'
.format(str(e)))
rospy.loginfo("Crazyflie FC Comms finished configured logs")
# Publish a landing gear contact message so that the landing detector can start up
switch_msg = LandingGearContactsStamped()
switch_msg.front = True
switch_msg.back = True
switch_msg.left = True
switch_msg.right = True
self._switch_pub.publish(switch_msg)
rospy.loginfo('Crazyflie FC Comms trying to form bond')
# forming bond with safety client
if not self._safety_client.form_bond():
raise IARCFatalSafetyException(
'Crazyflie FC Comms could not form bond with safety client')
rospy.loginfo('Crazyflie FC Comms done forming bond')
# Update at the same rate as low level motion
rate = rospy.Rate(60)
while not rospy.is_shutdown() and self._connected:
status = FlightControllerStatus()
status.header.stamp = rospy.Time.now()
status.armed = self._armed
status.auto_pilot = True
status.failsafe = False
self._status_publisher.publish(status)
if self._safety_client.is_fatal_active():
raise IARCFatalSafetyException('Safety Client is fatal active')
if self._safety_client.is_safety_active():
rospy.logwarn_throttle(
1.0, 'Crazyflie FC Comms activated safety response')
self._commands_allowed = False
self._cf.commander.send_setpoint(0, 0, 0, 0)
if self._crash_detected:
rospy.logwarn_throttle(1.0,
'Crazyflie FC Comms detected crash')
self._commands_allowed = False
if self._current_height > 0.15 and not self._crash_landed:
self._cf.commander.send_setpoint(
0, 0, 0, 0.47 * 65535) # Max throttle is 65535
else:
self._crash_landed = True
rospy.logwarn_throttle(
1.0, 'Crazyflie FC Comms shut down motors')
self._cf.commander.send_setpoint(0, 0, 0, 0)
if self._armed and self._commands_allowed:
if self._uav_command is None:
# Keep the connection alive
self._cf.commander.send_setpoint(0, 0, 0, 0)
else:
throttle = max(10000.0, self._uav_command.throttle *
65535.0) # Max throttle is 65535
self._cf.commander.send_setpoint(
self._uav_command.data.roll * 180.0 / math.pi,
self._uav_command.data.pitch * 180.0 / math.pi,
-1.0 * self._uav_command.data.yaw * 180.0 / math.pi,
throttle)
rate.sleep()
self._cf.commander.send_setpoint(0, 0, 0, 0)
# Give time for buffer to flush
time.sleep(0.1)
self._cf.close_link()