rate = rospy.Rate(30)
while not rospy.is_shutdown():
try:
(trans, rot) = tf_listener.lookupTransform('/map', '/quad',
rospy.Time(0))
except tf.Exception as ex:
rospy.logerr(ex.message)
rate.sleep()
continue
# Exit immediately if fatal
if safety_client.is_fatal_active():
break
# Land if put into safety mode
if safety_client.is_safety_active():
target = (trans[0], trans[1], 0, 0)
velocity = TwistStamped()
velocity.header.frame_id = 'level_quad'
velocity.header.stamp = rospy.Time.now()
if abs(target[0] - trans[0]) >= 0.02:
velocity.twist.linear.x = constrain((target[0] - trans[0]) * kP,
-max_vel, max_vel)
if abs(target[1] - trans[1]) >= 0.02:
velocity.twist.linear.y = constrain((target[1] - trans[1]) * kP,
-max_vel, max_vel)
if abs(target[2] - trans[2]) >= 0.02:
velocity.twist.linear.z = target[2] - trans[2]
# Get the yaw (z axis) rotation from the quanternion
# Wait for valid transform
while not rospy.is_shutdown() and not tf_buffer.can_transform(
'map',
'base_footprint',
rospy.Time(0)):
# Publish the ground state message assuming we are on the ground
state_msg = BoolStamped()
state_msg.header.stamp = rospy.Time.now()
state_msg.data = True
landing_detected_pub.publish(state_msg)
rate.sleep()
while not rospy.is_shutdown():
if (safety_client.is_fatal_active()
or safety_client.is_safety_active()):
# We don't have a safety response for this node so just exit
break
# Make sure we are within the target update rate
if use_switches:
if ((rospy.Time.now() - last_switch_message.header.stamp)
> rospy.Duration(1.0/(switch_expected_update_rate
-switch_update_lag_tolerance))):
rospy.logwarn_throttle(1.0,
'Landing Detector is not receiving switch messages fast enough')
# If this fails something is seriously wrong, so we're ok with crashing
last_tf = tf_buffer.lookup_transform('map',
'base_footprint',
rospy.Time(0))
class VelocityFilter(object):
def __init__(self):
rospy.init_node('velocity_filter')
self._lock = threading.Lock()
self._optical_flow_sub = rospy.Subscriber(
'/optical_flow_estimator/twist', TwistWithCovarianceStamped,
lambda msg: self._callback('optical', msg))
# This topic is assumed to have rotation around +z
# (angle from +x with +y being +pi/2)
self._kalman_filter_sub = rospy.Subscriber(
'/odometry/filtered', Odometry,
lambda msg: self._callback('kalman', msg))
self._kalman_weight = rospy.get_param('~kalman_weight')
self._message_queue_length = rospy.get_param('~message_queue_length')
self._position_passthrough = rospy.get_param('~position_passthrough')
initial_msg = TwistWithCovarianceStamped()
initial_msg.header.stamp = rospy.Time()
# This queue contains 3-tuples, sorted by timestamp, oldest at index 0
#
# First item of each 3-tuple is a list with 3 items containing the x,
# y, and z velocities. The x and y values are the current values of the
# filter at that point, the z is too except that it's simply propogated
# forward from the last 'kalman' message.
#
# Second item of each 3-tuple is the message itself
#
# Third item of each 3-tuple is a string indicating the type of the
# message, either 'optical' or 'kalman'
self._queue = [([0.0, 0.0, 0.0], initial_msg, 'optical')]
self._vel_pub = rospy.Publisher('double_filtered_vel',
Odometry,
queue_size=10)
self._last_published_stamp = rospy.Time(0)
def _publish_odom(self, x, y, z, vx, vy, vz, time):
msg = Odometry()
msg.header.stamp = time
msg.header.frame_id = 'map'
msg.child_frame_id = 'level_quad'
assert not math.isnan(x)
assert not math.isnan(y)
assert not math.isnan(z)
assert not math.isinf(x)
assert not math.isinf(y)
assert not math.isinf(z)
assert not math.isnan(vx)
assert not math.isnan(vy)
assert not math.isnan(vz)
assert not math.isinf(vx)
assert not math.isinf(vy)
assert not math.isinf(vz)
msg.twist.twist.linear.x = vx
msg.twist.twist.linear.y = vy
msg.twist.twist.linear.z = vz
msg.pose.pose.position.x = x
msg.pose.pose.position.y = y
msg.pose.pose.position.z = z
self._vel_pub.publish(msg)
def _get_last_index(self, klass, time=None):
'''
Return the index of the last message of type `klass` (or the last
message of type `klass` before `time` if `time` is specified)
Returns `-1` if there are no messages of type `klass` in the queue
'''
for i in xrange(len(self._queue) - 1, -1, -1):
if (self._queue[i][2] == klass and
(time is None or self._queue[i][1].header.stamp < time)):
return i
return -1
def _callback(self, klass, msg):
with self._lock:
new_vel_vector = msg.twist.twist.linear
last_i = self._get_last_index(klass)
if (last_i != -1 and
msg.header.stamp <= self._queue[last_i][1].header.stamp):
# This message isn't newer than the last one of type klass
rospy.logerr((
'Ignoring message of type {} with timestamp {} older than '
+ 'previous message with timestamp {}').format(
klass, msg.header.stamp,
self._queue[last_i][1].header.stamp))
return
if (math.isnan(new_vel_vector.x) or math.isnan(new_vel_vector.y)
or math.isinf(new_vel_vector.x)
or math.isinf(new_vel_vector.y)):
rospy.logerr(
'Velocity filter rejecting bad message: {}'.format(msg))
return
for i in xrange(len(self._queue) - 1, -1, -1):
if self._queue[i][1].header.stamp < msg.header.stamp:
index = i + 1
break
else:
rospy.logerr(
'Skipping message older than everything in the queue')
return
z_vel = float('NaN') if klass == 'optical' else new_vel_vector.z
self._queue.insert(
index, ([float('NaN'), float('NaN'), z_vel], msg, klass))
self._reprocess(index)
# Make sure new timestamp is at least 0.1ms newer than the last one
new_stamp = max(
self._last_published_stamp + rospy.Duration(0, 100),
self._queue[-1][1].header.stamp)
last_kalman_i = self._get_last_index('kalman')
if last_kalman_i == -1 or not self._position_passthrough:
self._publish_odom(0.0,
0.0,
0.0,
*self._queue[-1][0],
time=new_stamp)
else:
last_kalman_msg = self._queue[last_kalman_i][1]
self._publish_odom(last_kalman_msg.pose.pose.position.x,
last_kalman_msg.pose.pose.position.y,
last_kalman_msg.pose.pose.position.z,
*self._queue[-1][0],
time=new_stamp)
self._last_published_stamp = new_stamp
while len(self._queue) > self._message_queue_length:
self._queue.pop(0)
def _reprocess(self, index):
for i in xrange(index, len(self._queue)):
msg = self._queue[i][1]
klass = self._queue[i][2]
# Get index of last differential measurement in the queue
last_differential_i = self._get_last_index('kalman',
msg.header.stamp)
assert last_differential_i < i and last_differential_i >= -1
if klass == 'kalman':
if last_differential_i == -1:
# No other differential measurements in the queue, so
# assume our velocity hasn't changed since the previous
dx = 0.0
dy = 0.0
else:
last_diff_vector = \
self._queue[last_differential_i][1].twist.twist.linear
dx = msg.twist.twist.linear.x - last_diff_vector.x
dy = msg.twist.twist.linear.y - last_diff_vector.y
self._queue[i][0][0] = self._queue[i - 1][0][0] + dx
self._queue[i][0][1] = self._queue[i - 1][0][1] + dy
elif klass == 'optical':
self._queue[i][0][0] = (
(1 - self._kalman_weight) * msg.twist.twist.linear.x +
self._kalman_weight * self._queue[i - 1][0][0])
self._queue[i][0][1] = (
(1 - self._kalman_weight) * msg.twist.twist.linear.y +
self._kalman_weight * self._queue[i - 1][0][1])
self._queue[i][0][2] = self._queue[i - 1][0][2]
else:
assert False
def run(self):
self._safety_client = SafetyClient('velocity_filter')
assert self._safety_client.form_bond()
rate = rospy.Rate(15)
while not rospy.is_shutdown():
if (self._safety_client.is_fatal_active()
or self._safety_client.is_safety_active()):
break
rate.sleep()
class MotionPlanner:
def __init__(self, _action_server, update_rate):
self._action_server = _action_server
self._update_rate = update_rate
self._task = None
self._velocity_pub = rospy.Publisher('movement_velocity_targets',
TwistStampedArray,
queue_size=0)
self._in_passthrough = False
self._passthrough_pub = rospy.Publisher('passthrough_command',
OrientationThrottleStamped,
queue_size=10)
self._arm_service = rospy.ServiceProxy('uav_arm', Arm)
self._safety_client = SafetyClient('motion_planner')
self._safety_land_complete = False
self._safety_land_requested = False
self._timeout_vel_sent = False
# Create action client to request a landing
self._action_client = actionlib.SimpleActionClient(
"motion_planner_server",
QuadMoveAction)
# Creates the SimpleActionClient for requesting ground interaction
self._ground_interaction_client = actionlib.SimpleActionClient(
'ground_interaction_action',
GroundInteractionAction)
self._ground_interaction_task_callback = None
self._command_implementations = {
task_commands.VelocityCommand: self._handle_velocity_command,
task_commands.ArmCommand: self._handle_arm_command,
task_commands.NopCommand: self._handle_nop_command,
task_commands.GroundInteractionCommand: self._handle_ground_interaction_command,
task_commands.ConfigurePassthroughMode: self._handle_passthrough_mode_command,
task_commands.AngleThrottleCommand: self._handle_passthrough_command
}
self._time_of_last_task = None
self._last_twist = None
try:
self._task_timeout = rospy.Duration(rospy.get_param('~task_timeout'))
self._task_timeout_deceleration_time = rospy.Duration(rospy.get_param('~task_timeout_deceleration_time'))
except KeyError as e:
rospy.logerr('Could not lookup a parameter for motion_planner')
raise
def run(self):
rate = rospy.Rate(self._update_rate)
rospy.logwarn('trying to form bond')
if not self._safety_client.form_bond():
rospy.logerr('Motion planner could not form bond with safety client')
return
rospy.logwarn('done forming bond')
rospy.wait_for_service('uav_arm')
self._action_client.wait_for_server()
self._ground_interaction_client.wait_for_server()
while not rospy.is_shutdown():
# Exit immediately if fatal
if self._safety_client.is_fatal_active() or 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 planner attempting to execute safety land')
self._safety_land_requested = True
task_commands = self._get_task_command()
for task_command in task_commands:
try:
self._command_implementations[type(task_command)](task_command)
except (KeyError, TypeError) as e:
rospy.logerr("Task requested unimplemented command, noping: %s", type(task_command))
self._handle_nop_command(None)
rate.sleep()
def handle_ground_interaction_done(self, status, result):
if self._ground_interaction_task_callback is not None:
try:
self._ground_interaction_task_callback(status, result)
except Exception as e:
rospy.logerr('Exception sending result using ground interaction done cb')
rospy.logerr(str(e))
rospy.logerr(traceback.format_exc())
rospy.logwarn('Motion planner aborted task')
self._action_server.set_aborted()
self._task = None
self._ground_interaction_task_callback = None
else:
rospy.logerr('Ground interaction done callback received with no task callback available')
def _handle_ground_interaction_command(self, ground_interaction_command):
if self._ground_interaction_task_callback is not None:
rospy.logerr('Task requested another ground interaction action before the last completed')
rospy.logwarn('Motion planner aborted task')
self._action_server.set_aborted()
self._task = None
self._ground_interaction_task_callback = None
self._ground_interaction_client.cancel_goal()
self._ground_interaction_client.stop_tracking_goal()
return
self._ground_interaction_task_callback = ground_interaction_command.completion_callback
# Request ground interaction of llm
goal = GroundInteractionGoal(interaction_type=ground_interaction_command.interaction_type)
# Sends the goal to the action server.
self._ground_interaction_client.send_goal(goal, done_cb=self.handle_ground_interaction_done)
def _handle_nop_command(self, nop_command):
pass
def _handle_velocity_command(self, velocity_command):
self._publish_twist(velocity_command.target_twist)
def _handle_arm_command(self, arm_command):
arm_result = self._use_arm_service(arm_command.arm_state, arm_command.set_mode, arm_command.angle)
self._call_tasks_arm_service_callback(arm_command.completion_callback, arm_result)
def _call_tasks_arm_service_callback(self, callback, arm_result):
try:
callback(arm_result)
except Exception as e:
rospy.logerr('Exception setting result using an arm callback')
rospy.logerr(str(e))
rospy.logerr(traceback.format_exc())
rospy.logwarn('Motion planner aborted task')
self._action_server.set_aborted()
self._task = None
def _use_arm_service(self, arm, set_mode, angle):
try:
armed = self._arm_service(arm, set_mode, angle)
except rospy.ServiceException as exc:
rospy.logerr("Could not arm: " + str(exc))
armed = False
return armed
def _handle_passthrough_mode_command(self, passthrough_mode_command):
if passthrough_mode_command.enable:
if self._ground_interaction_task_callback is not None:
rospy.logerr('Task requested a passthrough action before the last was completed')
rospy.logwarn('Motion planner aborted task')
self._action_server.set_aborted()
self._task = None
self._ground_interaction_task_callback = None
self._ground_interaction_client.cancel_goal()
self._ground_interaction_client.stop_tracking_goal()
return
self._ground_interaction_task_callback = passthrough_mode_command.completion_callback
goal = GroundInteractionGoal(interaction_type='passthrough')
self._ground_interaction_client.send_goal(goal, done_cb=self.handle_ground_interaction_done)
self._in_passthrough = True
else:
self._ground_interaction_client.cancel_goal()
self._in_passthrough = False
def _handle_passthrough_command(self, passthrough_command):
if self._in_passthrough:
msg = OrientationThrottleStamped()
msg.header.stamp = rospy.Time.now()
msg.data.pitch = passthrough_command.pitch
msg.data.roll = passthrough_command.roll
msg.data.yaw = passthrough_command.vyaw
msg.throttle = passthrough_command.vz
self._passthrough_pub.publish(msg)
else:
rospy.logerr('Task requested a passthrough command when not in passthrough mode')
def _publish_twist(self, twist):
if type(twist) is TwistStampedArray:
self._last_twist = twist.twists[-1]
self._velocity_pub.publish(twist)
elif type(twist) is TwistStamped:
self._last_twist = twist
velocity_msg = TwistStampedArray()
velocity_msg.twists = [twist]
self._velocity_pub.publish(velocity_msg)
else:
raise TypeError('Illegal type provided in Motion Planner')
def _safety_task_complete_callback(self, status, response):
if response.success:
rospy.logwarn('Motion planner supposedly safely landed the aircraft')
else:
rospy.logerr('Motion planner did not safely land aircraft')
self._safety_land_complete = True
def _get_task_command(self):
if (self._task is None) and self._action_server.has_new_task():
self._task = self._action_server.get_new_task()
if self._task:
self._time_of_last_task = rospy.Time.now()
self._timeout_vel_sent = False
if self._action_server.is_canceled():
try:
self._task.cancel()
except Exception as e:
rospy.logerr('Exception canceling task')
rospy.logerr(str(e))
rospy.logerr(traceback.format_exc())
rospy.logwarn('Motion planner aborted task')
self._action_server.set_aborted()
self._task = None
return (task_commands.NopCommand(),)
try:
task_request = self._task.get_desired_command()
except Exception as e:
rospy.logerr('Exception getting tasks preferred velocity')
rospy.logerr(str(e))
rospy.logerr(traceback.format_exc())
rospy.logwarn('Motion planner aborted task')
self._action_server.set_aborted()
self._task = None
return (task_commands.NopCommand(),)
task_state = task_request[0]
if isinstance(task_state, task_states.TaskCanceled):
self._action_server.set_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
return task_request[1:]
else:
assert isinstance(task_state, task_states.TaskRunning)
return task_request[1:]
else:
# There is no current task running
if self._time_of_last_task is None:
self._time_of_last_task = rospy.Time.now()
if ((rospy.Time.now() - self._time_of_last_task) > self._task_timeout):
if not self._timeout_vel_sent:
commands = TwistStampedArray()
if self._last_twist is None:
self._last_twist = TwistStamped()
self._last_twist.header.stamp = rospy.Time.now()
future_twist = TwistStamped()
future_twist.header.stamp = rospy.Time.now() + self._task_timeout_deceleration_time
commands.twists = [self._last_twist, future_twist]
# If past the timeout send a zero velocity command
self._publish_twist(commands)
self._timeout_vel_sent = True
rospy.logwarn('Task running timeout. Setting zero velocity')
else:
self._timeout_vel_sent = False
# No action to take return a nop
return (task_commands.NopCommand(),)
class TaskManager(object):
def __init__(self, action_server):
self._action_server = action_server
self._lock = threading.RLock()
self._task = None
# construct provided command handler
self._handler = CommandHandler()
# construct safety responder
self._safety_responder = SafetyResponder()
# safety client, used if Safety Enabled
self._safety_client = SafetyClient('task_manager')
try:
# load params
self._safety_enabled = rospy.get_param('~safety_enabled')
self._update_rate = rospy.Rate(rospy.get_param('~update_rate'))
self._timeout = rospy.Duration(rospy.get_param('~startup_timeout'))
self._force_cancel = rospy.get_param('~force_cancel')
except KeyError:
rospy.logfatal('TaskManager: Error getting params')
raise
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, timeout):
# wait until dependencies are ready
try:
self._handler.wait_until_ready(timeout)
self._safety_responder.wait_until_ready(timeout)
except Exception:
rospy.logfatal('TaskManager: Error waiting for dependencies')
raise
return True
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
class OrientationFilter(object):
def __init__(self):
rospy.init_node('orientation_filter')
self._lock = threading.Lock()
with self._lock:
self._orientation_sub = rospy.Subscriber(
'fc_orientation', OrientationAnglesStamped,
lambda msg: self._callback(OrientationAnglesStamped, msg))
# This topic is assumed to have rotation around +z
# (angle from +x with +y being +pi/2)
self._line_yaw_sub = rospy.Subscriber(
'line_yaw', Float64Stamped,
lambda msg: self._callback(Float64Stamped, msg))
self._debug_orientation_pub = rospy.Publisher(
'orientation_filter/debug_orientation',
OrientationAnglesStamped,
queue_size=10)
self._line_weight = rospy.get_param('~line_weight')
self._message_queue_length = rospy.get_param(
'~message_queue_length')
initial_msg = Float64Stamped()
initial_msg.header.stamp = rospy.Time()
initial_msg.data = rospy.get_param('~initial_yaw', float('NaN'))
# This queue contains pairs, sorted by timestamp, oldest at index 0
#
# First item of each pair is a 3-tuple containing:
# yaw (rotation around +z)
# pitch (rotation around +y')
# roll (rotation around +x'')
# Second item of each pair is the message itself
#
# The yaw in the 3-tuple is the current value of the filter at that
# point, the pitch and roll are too except that they're simply
# propogated forward from the last OrientationAnglesStamped message
self._queue = [([initial_msg.data,
float('NaN'),
float('NaN')], initial_msg)]
self._transform_broadcaster = tf2_ros.TransformBroadcaster()
self._last_published_stamp = rospy.Time(0)
def _publish_transform(self, r, p, y, time):
transform_msg = TransformStamped()
transform_msg.header.stamp = time
transform_msg.header.frame_id = 'level_quad'
transform_msg.child_frame_id = 'quad'
quaternion = tf.transformations.quaternion_from_euler(r, p, y)
transform_msg.transform.rotation.x = quaternion[0]
transform_msg.transform.rotation.y = quaternion[1]
transform_msg.transform.rotation.z = quaternion[2]
transform_msg.transform.rotation.w = quaternion[3]
self._transform_broadcaster.sendTransform(transform_msg)
transform_msg.header.stamp = time
transform_msg.header.frame_id = 'level_quad'
transform_msg.child_frame_id = 'heading_quad'
quaternion = tf.transformations.quaternion_from_euler(0, 0, y)
transform_msg.transform.rotation.x = quaternion[0]
transform_msg.transform.rotation.y = quaternion[1]
transform_msg.transform.rotation.z = quaternion[2]
transform_msg.transform.rotation.w = quaternion[3]
self._transform_broadcaster.sendTransform(transform_msg)
orientation_message = OrientationAnglesStamped()
orientation_message.header.stamp = time
orientation_message.data.pitch = p
orientation_message.data.roll = r
orientation_message.data.yaw = y
self._debug_orientation_pub.publish(orientation_message)
def _get_last_index(self, klass, time=None):
'''
Return the index of the last message of type `klass` (or the
last message of type `klass` before `time` if `time` is
specified)
Returns `-1` if there are no messages of type `klass` in the
queue
'''
for i in xrange(len(self._queue) - 1, -1, -1):
if (type(self._queue[i][1]) == klass and
(time is None or self._queue[i][1].header.stamp < time)):
return i
return -1
def _callback(self, klass, msg):
with self._lock:
last_i = self._get_last_index(klass)
if (last_i != -1 and
msg.header.stamp <= self._queue[last_i][1].header.stamp):
# This message isn't newer than the last one of type klass
rospy.logwarn((
'Ignoring message of type {} with timestamp {} older than '
+ 'previous message with timestamp {}').format(
klass, msg.header.stamp,
self._queue[last_i][1].header.stamp))
return
for i in xrange(len(self._queue) - 1, -1, -1):
if self._queue[i][1].header.stamp < msg.header.stamp:
index = i + 1
break
else:
index = 0
data = [float('NaN')] * 3
try:
data[1] = -msg.data.pitch
data[2] = msg.data.roll
except AttributeError:
# If this message type doesn't have pitch and roll, skip it
pass
self._queue.insert(index, (data, msg))
self._reprocess(index)
if (not math.isnan(self._queue[-1][0][0])
and not math.isnan(self._queue[-1][0][1])
and not math.isnan(self._queue[-1][0][2])):
# Make sure new timestamp is at least 1us newer than the
# last one. Using 1ns was dangerous and caused the tf library
# to create transforms with NaN values
new_stamp = max(
self._last_published_stamp + rospy.Duration(0, 1000),
self._queue[-1][1].header.stamp)
self._publish_transform(*reversed(self._queue[-1][0]),
time=new_stamp)
self._last_published_stamp = new_stamp
while len(self._queue) > self._message_queue_length:
self._queue.pop(0)
def _reprocess(self, index):
for i in xrange(index, len(self._queue)):
last_yaw = self._queue[i - 1][0][0]
msg = self._queue[i][1]
last_orientation_i = self._get_last_index(OrientationAnglesStamped,
msg.header.stamp)
if type(msg) == OrientationAnglesStamped:
if last_orientation_i == -1:
dyaw = 0
else:
dyaw = -(msg.data.yaw -
self._queue[last_orientation_i][1].data.yaw)
if dyaw > math.pi:
dyaw -= 2 * math.pi
elif dyaw < -math.pi:
dyaw += 2 * math.pi
self._queue[i][0][0] = last_yaw + dyaw
elif type(msg) == Float64Stamped:
new_yaw = msg.data
if new_yaw - last_yaw > math.pi:
new_yaw -= 2 * math.pi
elif new_yaw - last_yaw < -math.pi:
new_yaw += 2 * math.pi
# Skip the filtering step if we don't already have a valid
# measurement
if math.isnan(last_yaw):
self._queue[i][0][0] = new_yaw
else:
self._queue[i][0][0] = (self._line_weight * new_yaw +
(1 - self._line_weight) * last_yaw)
if last_orientation_i != -1:
self._queue[i][0][1] = self._queue[last_orientation_i][0][
1]
self._queue[i][0][2] = self._queue[last_orientation_i][0][
2]
else:
assert False
self._queue[i][0][0] += 2 * math.pi
self._queue[i][0][0] %= 2 * math.pi
def run(self):
self._safety_client = SafetyClient('orientation_filter')
assert self._safety_client.form_bond()
rate = rospy.Rate(15)
while not rospy.is_shutdown():
if (self._safety_client.is_fatal_active()
or self._safety_client.is_safety_active()):
break
rate.sleep()
class CrazyflieFcComms:
def __init__(self):
self._uav_command = None
self._connected = False
self._timestamp_offset = None
self._armed = False
self._cf = None
self._commands_allowed = True
self._uav_command = None
self._vbat = None
self._crash_detected = False
self._crash_landed = False
# safety
self._safety_client = SafetyClient('fc_comms_crazyflie')
# Publishers for FC sensors
self._battery_publisher = rospy.Publisher('fc_battery',
Float64Stamped,
queue_size=5)
self._status_publisher = rospy.Publisher('fc_status',
FlightControllerStatus,
queue_size=5)
self._imu_publisher = rospy.Publisher('fc_imu', Imu, queue_size=5)
self._orientation_pub = rospy.Publisher('fc_orientation',
OrientationAnglesStamped,
queue_size=5)
self._velocity_pub = rospy.Publisher('optical_flow_estimator/twist',
TwistWithCovarianceStamped,
queue_size=5)
self._range_pub = rospy.Publisher('short_distance_lidar',
Range,
queue_size=5)
self._switch_pub = rospy.Publisher('landing_gear_contact_switches',
LandingGearContactsStamped,
queue_size=5)
self._mag_pub = rospy.Publisher('fc_mag', MagneticField, queue_size=5)
# Subscriber for uav_angle values
self._uav_angle_subscriber = rospy.Subscriber(
'uav_direction_command', OrientationThrottleStamped,
self._uav_command_handler)
# Service to arm copter
self._uav_arm_service = rospy.Service('uav_arm', Arm,
self._arm_service_handler)
self._current_height = 0.0
self._over_height_counts = 0
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()
# Always return success because making sure the crazyflie is
# armed correctly isn't critical
def _arm_service_handler(self, arm_request):
if self._armed and not arm_request.data:
self._cf.commander.send_setpoint(0, 0, 0, 0)
self._armed = False
return ArmResponse(success=True)
elif not self._armed and arm_request.data:
if self._vbat < 3.55 or self._vbat is None:
rospy.logerr(
'Crazyflie FC Comms refusing to arm, battery to low or not received'
)
return ArmResponse(success=False)
else:
self._cf.commander.send_setpoint(0, 0, 0, 0)
self._armed = True
return ArmResponse(success=True)
# State is the same as it was before. So simply return true.
return ArmResponse(success=True)
def _uav_command_handler(self, data):
self._uav_command = data
def _receive_crazyflie_data(self, timestamp, data, logconf):
# Catch all exceptions because otherwise the crazyflie
# library catches them all silently
try:
stamp = self._calculate_timestamp(timestamp)
if logconf.name == 'high_update_rate':
imu = Imu()
imu.header.stamp = stamp
imu.header.frame_id = 'quad'
imu.linear_acceleration.x = data['acc.x'] * 9.81
imu.linear_acceleration.y = data['acc.y'] * 9.81
imu.linear_acceleration.z = data['acc.z'] * 9.81
imu.orientation_covariance[0] = -1
imu.angular_velocity_covariance[0] = -1
variance = 6.0
imu.linear_acceleration_covariance[0] = variance
imu.linear_acceleration_covariance[4] = variance
imu.linear_acceleration_covariance[8] = variance
self._imu_publisher.publish(imu)
orientation = OrientationAnglesStamped()
orientation.header.stamp = stamp
orientation.data.roll = data[
'stabilizer.roll'] * math.pi / 180.0
orientation.data.pitch = data[
'stabilizer.pitch'] * math.pi / 180.0
# Zero out the yaw for now. Crazyflie has a problem with yaw calculation
# and having correct yaw is not critical for this target yet.
orientation.data.yaw = -0.0 * data[
'stabilizer.yaw'] * math.pi / 180.0
self._orientation_pub.publish(orientation)
if abs(orientation.data.roll) > 1.5 or abs(
orientation.data.pitch) > 1.5:
rospy.logwarn(
'Crazyflie FC Comms maximimum attitude angle exceeded')
self._crash_detected = True
elif logconf.name == 'medium_update_rate':
twist = TwistWithCovarianceStamped()
twist.header.stamp = stamp - rospy.Duration.from_sec(0.020)
twist.header.frame_id = 'heading_quad'
twist.twist.twist.linear.x = data['kalman_states.vx']
twist.twist.twist.linear.y = data['kalman_states.vy']
twist.twist.twist.linear.z = 0.0
variance = 0.0001
twist.twist.covariance[0] = variance
twist.twist.covariance[7] = variance
self._velocity_pub.publish(twist)
range_msg = Range()
range_msg.header.stamp = stamp - rospy.Duration.from_sec(0.050)
range_msg.radiation_type = Range.INFRARED
range_msg.header.frame_id = '/short_distance_lidar'
range_msg.field_of_view = 0.3
range_msg.min_range = 0.01
range_msg.max_range = 1.6
self._current_height = data['range.zrange'] / 1000.0
range_msg.range = self._current_height
if range_msg.range > 2.0:
self._over_height_counts = self._over_height_counts + 1
else:
self._over_height_counts = 0
if self._over_height_counts > 10:
rospy.logwarn(
'Crazyflie FC Comms maximimum height exceeded')
self._crash_detected = True
self._range_pub.publish(range_msg)
switch_msg = LandingGearContactsStamped()
switch_msg.header.stamp = stamp
pressed = data['range.zrange'] < 50
switch_msg.front = pressed
switch_msg.back = pressed
switch_msg.left = pressed
switch_msg.right = pressed
self._switch_pub.publish(switch_msg)
elif logconf.name == 'slow_update_rate':
battery = Float64Stamped()
battery.header.stamp = stamp
battery.data = data['pm.vbat']
self._vbat = data['pm.vbat']
self._battery_publisher.publish(battery)
mag = MagneticField()
mag.header.stamp = stamp
mag.magnetic_field.x = data['mag.x']
mag.magnetic_field.y = data['mag.y']
mag.magnetic_field.z = data['mag.z']
self._mag_pub.publish(mag)
else:
rospy.logerr(
'Crazyflie FC Comms received message block with unkown name'
)
except Exception as e:
rospy.logerr("Crazyflie FC Comms error receiving data: {}".format(
str(e)))
rospy.logerr(traceback.format_exc())
def _calculate_timestamp(self, timestamp):
if self._timestamp_offset is None:
self._timestamp_offset = (rospy.Time.now(), timestamp)
stamp = self._timestamp_offset[0] + rospy.Duration.from_sec(
(float(timestamp - self._timestamp_offset[1]) / 1000.0) - 0.025)
return stamp
def _receive_crazyflie_error(self, logconf, msg):
rospy.logerr('Crazyflie FC Comms error: {}, {}'.format(
logconf.name, msg))
def _connection_made(self, uri):
rospy.loginfo('Crazyflie FC Comms connected to {}'.format(uri))
self._connected = True
def _connection_failed(self, uri, msg):
rospy.logerr('Crazyflie FC Comms connection {} failed: {}'.format(
uri, msg))
self._connected = False
def _connection_lost(self, uri, msg):
rospy.logerr('Crazyflie FC Comms connection to {} lost: {}'.format(
uri, msg))
self._connected = False
def _disconnected(self, uri):
rospy.loginfo('Crazyflie FC Comms disconnected from {}'.format(uri))
self._connected = False
