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()
ground_interaction_client.wait_for_result() rospy.logwarn("Takeoff success: {}".format( ground_interaction_client.get_result())) 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)
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