def takeoff_land(): safety_client = SafetyClient('takeoff_land_abstract') # Since this abstract is top level in the control chain there is no need to check # for a safety state. We can also get away with not checking for a fatal state since # all nodes below will shut down. assert (safety_client.form_bond()) # Creates the SimpleActionClient, passing the type of the action # (QuadMoveAction) to the constructor. (Look in the action folder) client = actionlib.SimpleActionClient("motion_planner_server", QuadMoveAction) # Waits until the action server has started up and started # listening for goals. client.wait_for_server() rospy.sleep(5.0) # Test takeoff goal = QuadMoveGoal(movement_type="takeoff") # Sends the goal to the action server. client.send_goal(goal) # Waits for the server to finish performing the action. client.wait_for_result() rospy.logwarn("Takeoff success: {}".format(client.get_result())) rospy.sleep(5.0) # Test land goal = QuadMoveGoal(movement_type="land") # Sends the goal to the action server. client.send_goal(goal) # Waits for the server to finish performing the action. client.wait_for_result() rospy.logwarn("Land success: {}".format(client.get_result()))
def hit_roomba_land(): safety_client = SafetyClient('hit_roomba_abstract') # Since this abstract is top level in the control chain there is no need to check # for a safety state. We can also get away with not checking for a fatal state since # all nodes below will shut down. assert (safety_client.form_bond()) # Creates the SimpleActionClient, passing the type of the action # (QuadMoveAction) to the constructor. (Look in the action folder) client = actionlib.SimpleActionClient("motion_planner_server", QuadMoveAction) # Waits until the action server has started up and started # listening for goals. client.wait_for_server() rospy.sleep(2.0) # Test takeoff goal = QuadMoveGoal(movement_type="takeoff") # Sends the goal to the action server. client.send_goal(goal) # Waits for the server to finish performing the action. client.wait_for_result() rospy.logwarn("Takeoff success: {}".format(client.get_result())) rospy.sleep(2.0) # change element in array to test diff roombas roomba_id = roomba_array.data[5].child_frame_id roomba_id = roomba_id[0:len(roomba_id) - 10] # Test tracking goal = QuadMoveGoal(movement_type="track_roomba", frame_id=roomba_id, tracking_mode=True) # Sends the goal to the action server. client.send_goal(goal) # Waits for the server to finish performing the action. client.wait_for_result() rospy.logwarn("Track Roomba success: {}".format(client.get_result())) # Test tracking goal = QuadMoveGoal(movement_type="hit_roomba", frame_id=roomba_id) # Sends the goal to the action server. client.send_goal(goal) # Waits for the server to finish performing the action. client.wait_for_result() rospy.logwarn("Hit Roomba success: {}".format(client.get_result())) goal = QuadMoveGoal(movement_type="height_recovery") # Sends the goal to the action server. client.send_goal(goal) # Waits for the server to finish performing the action. client.wait_for_result() rospy.logwarn("Height Recovery success: {}".format(client.get_result()))
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()
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 test_rre(): safety_client = SafetyClient('roomba_request_executer_test_abstract') # Since this abstract is top level in the control chain there is no need to # check for a safety state. We can also get away with not checking for a # fatal state since all nodes below will shut down. assert (safety_client.form_bond()) if rospy.is_shutdown(): return # Creates the SimpleActionClient, passing the type of the action # (QuadMoveAction) to the constructor. (Look in the action folder) client = actionlib.SimpleActionClient("motion_planner_server", QuadMoveAction) # Waits until the action server has started up and started # listening for goals. client.wait_for_server() if rospy.is_shutdown(): return rospy.sleep(2.0) # Test takeoff goal = QuadMoveGoal(movement_type="takeoff") # Sends the goal to the action server. client.send_goal(goal) # Waits for the server to finish performing the action. client.wait_for_result() if rospy.is_shutdown(): return rospy.logwarn("Takeoff success: {}".format(client.get_result())) rospy.sleep(2.0) while roomba_array is None and not rospy.is_shutdown(): rospy.sleep(2) # change element in array to test diff roombas roomba_id = roomba_array.data[-1].child_frame_id RoombaRequestExecuter.init('motion_planner_server') roomba_request = RoombaRequest(roomba_id, RoombaRequest.BUMP, 5) RoombaRequestExecuter.run(roomba_request, _receive_roomba_executer_status) while RoombaRequestExecuter.has_running_task(): rospy.logwarn('Current Roomba Request Executer state: ' + executer_state.name) rospy.sleep(2) if executer_state == RoombaRequestExecuterState.SUCCESS: rospy.logwarn("Roomba request executer completed successfully") else: rospy.logerr("Roomba request executer failed")
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 __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 hold_current_position_land(): safety_client = SafetyClient('hold_position_abstract') # Since this abstract is top level in the control chain there is no need to check # for a safety state. We can also get away with not checking for a fatal state since # all nodes below will shut down. assert (safety_client.form_bond()) # Creates the SimpleActionClient, passing the type of the action # (QuadMoveAction) to the constructor. (Look in the action folder) client = actionlib.SimpleActionClient("motion_planner_server", QuadMoveAction) # Waits until the action server has started up and started # listening for goals. client.wait_for_server() rospy.sleep(2.0) # Test takeoff goal = QuadMoveGoal(movement_type="takeoff") # Sends the goal to the action server. client.send_goal(goal) # Waits for the server to finish performing the action. client.wait_for_result() rospy.logwarn("Takeoff success: {}".format(client.get_result())) rospy.sleep(2.0) _x_position = _drone_odometry.pose.pose.position.x - .25 _y_position = _drone_odometry.pose.pose.position.y - .25 _z_position = _drone_odometry.pose.pose.position.z + .1 # Test tracking goal = QuadMoveGoal(movement_type="hold_position", hold_current_position=False, x_position=_x_position, y_position=_y_position, z_position=_z_position) # Sends the goal to the action server. client.send_goal(goal) rospy.sleep(5) client.cancel_goal() rospy.logwarn("Hold Position Task canceled")
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
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
def wall_bounce(): safety_client = SafetyClient('wall_bounce_abstract') # Since this abstract is top level in the control chain there is no need to check # for a safety state. We can also get away with not checking for a fatal state since # all nodes below will shut down. assert(safety_client.form_bond()) if rospy.is_shutdown(): return # Creates the SimpleActionClient, passing the type of the action # (QuadMoveAction) to the constructor. (Look in the action folder) client = actionlib.SimpleActionClient("motion_planner_server", QuadMoveAction) # Waits until the action server has started up and started # listening for goals. client.wait_for_server() if rospy.is_shutdown(): return VELOCITY = 0.5 STOP_DELAY = 2.0 # Start going forward angle = 0 distance_to_wall_threshold = 2.0 last_wall_hit = -1 # Test takeoff goal = QuadMoveGoal(movement_type="takeoff") # Sends the goal to the action server. client.send_goal(goal) # Waits for the server to finish performing the action. client.wait_for_result() if rospy.is_shutdown(): return rospy.logwarn("Takeoff success: {}".format(client.get_result())) rospy.sleep(0.5) for i in range(0, 100): # Go in a direction until the distance to any wall (that was not the last wall hit) is too small x_vel = math.cos(angle) * VELOCITY y_vel = math.sin(angle) * VELOCITY rospy.logwarn('SET A VELOCITY') rospy.logwarn('Changing direction, new angle: {} degrees'.format(angle * 180.0 / math.pi)) goal = QuadMoveGoal(movement_type="velocity_test", x_velocity=x_vel, y_velocity=y_vel, z_position=1.5) # Sends the goal to the action server. client.send_goal(goal) rospy.sleep(0.1) CONE_WIDTH_RAD = 30 * math.pi / 180.0 CONE_OFFSET_RAD = ((math.pi/2) - CONE_WIDTH_RAD) / 2.0 rate = rospy.Rate(30) while True: if (last_wall_hit != 0 and last_wall_hit != 3) and arena_position_estimator.distance_to_left() < distance_to_wall_threshold: last_wall_hit = 0 rospy.logerr('LEFT WALL HIT') # random angle pointed SE angle = -((random.random() * CONE_WIDTH_RAD) + CONE_OFFSET_RAD) - (math.pi/2) break if (last_wall_hit != 1 and last_wall_hit != 2) and arena_position_estimator.distance_to_right() < distance_to_wall_threshold: last_wall_hit = 1 rospy.logerr('RIGHT WALL HIT') # random angle pointed NW angle = ((random.random() * CONE_WIDTH_RAD) + CONE_OFFSET_RAD) break if (last_wall_hit != 2 and last_wall_hit != 0) and arena_position_estimator.distance_to_top() < distance_to_wall_threshold: last_wall_hit = 2 rospy.logerr('TOP WALL HIT') # random angle pointed SW angle = ((random.random() * CONE_WIDTH_RAD) + CONE_OFFSET_RAD) + (math.pi/2) break if (last_wall_hit != 3 and last_wall_hit != 1) and arena_position_estimator.distance_to_bottom() < distance_to_wall_threshold: last_wall_hit = 3 rospy.logerr('BOTTOM WALL HIT') # random angle pointed NE angle = -((random.random() * CONE_WIDTH_RAD) + CONE_OFFSET_RAD) break rate.sleep() client.cancel_goal() goal = QuadMoveGoal(movement_type="velocity_test", x_velocity=0.0, y_velocity=0.0, z_position=1.5) # Sends the goal to the action server. client.send_goal(goal) rospy.sleep(STOP_DELAY) client.cancel_goal() rospy.logwarn("Stopped") # Test land goal = QuadMoveGoal(movement_type="land") # Sends the goal to the action server. client.send_goal(goal) # Waits for the server to finish performing the action. client.wait_for_result() if rospy.is_shutdown(): return rospy.logwarn("Land success: {}".format(client.get_result()))
from actionlib_msgs.msg import GoalStatus from iarc7_motion.msg import GroundInteractionGoal, GroundInteractionAction from iarc7_msgs.msg import MotionPointStampedArray, MotionPointStamped from iarc7_msgs.srv import Arm from iarc7_safety.SafetyClient import SafetyClient def constrain(x, l, h): return min(h, max(x, l)) if __name__ == '__main__': rospy.init_node('waypoints_test') safety_client = SafetyClient('motion_command_coordinator') assert safety_client.form_bond() motion_point_pub = rospy.Publisher('motion_point_targets', MotionPointStampedArray, queue_size=0) tf_listener = tf.TransformListener() while not rospy.is_shutdown() and rospy.Time.now() == 0: pass start_time = rospy.Time.now() arm_service = rospy.ServiceProxy('uav_arm', Arm) arm_service.wait_for_service() armed = False while not rospy.is_shutdown() and not armed:
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()
#!/usr/bin/env python import rospy from iarc7_safety.SafetyClient import SafetyClient if __name__ == '__main__': rospy.init_node('test_bond') rospy.loginfo('Trying to form bond') safety_client = SafetyClient('test_bond') assert (safety_client.form_bond()) rospy.loginfo('Bond formed') rate = rospy.Rate(10) while not rospy.is_shutdown(): # Exit immediately if fatal if safety_client.is_fatal_active(): rospy.loginfo('Fatal active') break # Exit if safety state, this node can't do anything about it if safety_client.is_safety_active(): rospy.loginfo('Safety active') break rate.sleep()
def competition_demo(): safety_client = SafetyClient('competition_demo_abstract') # Since this abstract is top level in the control chain there is no need to check # for a safety state. We can also get away with not checking for a fatal state since # all nodes below will shut down. assert (safety_client.form_bond()) if rospy.is_shutdown(): return # Creates the SimpleActionClient, passing the type of the action # (QuadMoveAction) to the constructor. (Look in the action folder) client = actionlib.SimpleActionClient("motion_planner_server", QuadMoveAction) # Waits until the action server has started up and started # listening for goals. client.wait_for_server() if rospy.is_shutdown(): return rospy.sleep(2.0) # Test takeoff goal = QuadMoveGoal(movement_type="takeoff") # Sends the goal to the action server. client.send_goal(goal) # Waits for the server to finish performing the action. client.wait_for_result() if rospy.is_shutdown(): return rospy.logwarn("Takeoff success: {}".format(client.get_result())) rospy.sleep(2.0) goal = QuadMoveGoal(movement_type="velocity_test", x_velocity=0.2, y_velocity=-0.2) # Sends the goal to the action server. client.send_goal(goal) rospy.sleep(3.0) client.cancel_goal() rospy.logwarn("Translation 1 canceled") rospy.sleep(2.0) goal = QuadMoveGoal(movement_type="velocity_test", x_velocity=-0.2, y_velocity=0.2) # Sends the goal to the action server. client.send_goal(goal) rospy.sleep(0.5) client.cancel_goal() rospy.logwarn("Translation 2 canceled") rospy.sleep(2.0) goal = QuadMoveGoal(movement_type="velocity_test", x_velocity=0.3, y_velocity=0.0) # Sends the goal to the action server. client.send_goal(goal) rospy.sleep(1.0) client.cancel_goal() rospy.logwarn("Translation 3 canceled") rospy.sleep(2.0) goal = QuadMoveGoal(movement_type="velocity_test", x_velocity=-0.2, y_velocity=-0.2) # Sends the goal to the action server. client.send_goal(goal) rospy.sleep(0.5) client.cancel_goal() rospy.logwarn("Translation 4 canceled") rospy.sleep(2.0) goal = QuadMoveGoal(movement_type="velocity_test", x_velocity=0.0, y_velocity=0.3) # Sends the goal to the action server. client.send_goal(goal) rospy.sleep(1.0) client.cancel_goal() rospy.logwarn("Translation 5 canceled") rospy.sleep(6.0) # Test land goal = QuadMoveGoal(movement_type="land") # Sends the goal to the action server. client.send_goal(goal) # Waits for the server to finish performing the action. client.wait_for_result() if rospy.is_shutdown(): return rospy.logwarn("Land success: {}".format(client.get_result()))
def hit_roomba_land(): safety_client = SafetyClient('hit_roomba_abstract') # Since this abstract is top level in the control chain there is no need to check # for a safety state. We can also get away with not checking for a fatal state since # all nodes below will shut down. assert(safety_client.form_bond()) if rospy.is_shutdown(): return control_buttons.wait_for_start() # Creates the SimpleActionClient, passing the type of the action # (QuadMoveAction) to the constructor. (Look in the action folder) client = actionlib.SimpleActionClient("motion_planner_server", QuadMoveAction) # Waits until the action server has started up and started # listening for goals. client.wait_for_server() if rospy.is_shutdown(): return # Test takeoff goal = QuadMoveGoal(movement_type="takeoff") # Sends the goal to the action server. client.send_goal(goal) # Waits for the server to finish performing the action. client.wait_for_result() if rospy.is_shutdown(): return rospy.logwarn("Takeoff success: {}".format(client.get_result())) goal = QuadMoveGoal(movement_type="velocity_test", x_velocity=0.0, y_velocity=0.0, z_position=1.5) # Sends the goal to the action server. client.send_goal(goal) rospy.sleep(2.0) client.cancel_goal() rospy.logwarn("Done ascending") goal = QuadMoveGoal(movement_type="velocity_test", x_velocity=0.7, y_velocity=0.0, z_position=1.5) # Sends the goal to the action server. client.send_goal(goal) search_start_time = rospy.Time.now() rate = rospy.Rate(30) roomba_detected = False while not rospy.is_shutdown(): if rospy.Time.now() - search_start_time > rospy.Duration(2.5): rospy.loginfo("Searching for Roomba timed out") break elif len(roomba_array.data) > 0: roomba_detected = True rospy.loginfo("FOUND ROOMBA") break rate.sleep() client.cancel_goal() rospy.logwarn("Translation to roomba canceled") if roomba_detected: # change element in array to test diff roombas roomba_id = roomba_array.data[0].child_frame_id for i in range(0, 1): # Test tracking goal = QuadMoveGoal(movement_type="track_roomba", frame_id=roomba_id, time_to_track=0.0, x_overshoot=0, y_overshoot=0) # Sends the goal to the action server. client.send_goal(goal) # Waits for the server to finish performing the action. client.wait_for_result() rospy.logwarn("Track Roomba success: {}".format(client.get_result())) # Test hitting goal = QuadMoveGoal(movement_type="hit_roomba", frame_id = roomba_id) # Sends the goal to the action server. client.send_goal(goal) # Waits for the server to finish performing the action. client.wait_for_result() rospy.logwarn("Hit Roomba success: {}".format(client.get_result())) else: rospy.logerr("Roomba not found while searching, returning") goal = QuadMoveGoal(movement_type="velocity_test", x_velocity=-0.5, y_velocity=0.0, z_position=1.5) # Sends the goal to the action server. client.send_goal(goal) rospy.sleep(3.0) client.cancel_goal() rospy.logwarn("Translation to roomba canceled") goal = QuadMoveGoal(movement_type="land") # Sends the goal to the action server. client.send_goal(goal) # Waits for the server to finish performing the action. client.wait_for_result() if rospy.is_shutdown(): return rospy.logwarn("Land success: {}".format(client.get_result()))
def takeoff_land(): safety_client = SafetyClient('takeoff_land_abstract') # Since this abstract is top level in the control chain there is no need to check # for a safety state. We can also get away with not checking for a fatal state since # all nodes below will shut down. assert (safety_client.form_bond()) if rospy.is_shutdown(): return # Creates the SimpleActionClient, passing the type of the action # (QuadMoveAction) to the constructor. (Look in the action folder) client = actionlib.SimpleActionClient("motion_planner_server", QuadMoveAction) # Waits until the action server has started up and started # listening for goals. client.wait_for_server() if rospy.is_shutdown(): return rospy.sleep(5.0) for i in range(0, 2): # Test takeoff goal = QuadMoveGoal(movement_type="takeoff") # Sends the goal to the action server. client.send_goal(goal) # Waits for the server to finish performing the action. client.wait_for_result() if rospy.is_shutdown(): return rospy.logwarn("Takeoff success: {}".format(client.get_result())) #rospy.logwarn("Begin hovering") #goal = QuadMoveGoal(movement_type="velocity_test", x_velocity=0.0, y_velocity=0.0, z_position=0.4) # Sends the goal to the action server. #client.send_goal(goal) #rospy.sleep(10.0) #client.cancel_goal() #if rospy.is_shutdown(): return #rospy.logwarn("Done hovering") for i in range(0, 1): rospy.logwarn("Ascending") goal = QuadMoveGoal(movement_type="velocity_test", x_velocity=0.0, y_velocity=0.0, z_position=1.0) # Sends the goal to the action server. client.send_goal(goal) rospy.sleep(3.0) client.cancel_goal() if rospy.is_shutdown(): return rospy.logwarn("Done asending") rospy.logwarn("Descending") goal = QuadMoveGoal(movement_type="velocity_test", x_velocity=0.0, y_velocity=0.0, z_position=0.8) # Sends the goal to the action server. client.send_goal(goal) rospy.sleep(3.0) client.cancel_goal() if rospy.is_shutdown(): return rospy.logwarn("Done descending") # Test land goal = QuadMoveGoal(movement_type="land") # Sends the goal to the action server. client.send_goal(goal) # Waits for the server to finish performing the action. client.wait_for_result() if rospy.is_shutdown(): return rospy.logwarn("Land success: {}".format(client.get_result()))
from iarc7_safety.SafetyClient import SafetyClient from iarc7_msgs.msg import OrientationThrottleStamped from iarc7_msgs.msg import BoolStamped from iarc7_msgs.srv import Arm from geometry_msgs.msg import TwistStamped if __name__ == '__main__': rospy.init_node('waypoints_test', anonymous=True) rospy.wait_for_service('uav_arm') arm_service = rospy.ServiceProxy('uav_arm', Arm) rospy.logerr('done waiting') safety_client = SafetyClient('motors_test') safety_client.form_bond() command_pub = rospy.Publisher('uav_direction_command', OrientationThrottleStamped, queue_size=0) armed = False while armed == False: try: rospy.logerr('Trying to arm') armed = arm_service(True, True, False) except rospy.ServiceException as exc: rospy.logerr('Could not arm: ' + str(exc)) rospy.logerr('Armed') rate = rospy.Rate(30)
if rospy.is_shutdown(): raise rospy.exceptions.ROSInterruptException('No valid timestamp before shutdown') rate.sleep() # Make sure a message is recieved and published before connecting # to safety start_time = rospy.Time.now() if use_switches: while last_switch_message is None: assert((rospy.Time.now() - start_time) < rospy.Duration(switch_startup_timeout)) if rospy.is_shutdown(): raise rospy.exceptions.ROSInterruptException('No message before shutdown') rate.sleep() safety_client = SafetyClient('landing_detector') assert(safety_client.form_bond()) # 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():
def takeoff_land(): safety_client = SafetyClient('takeoff_translate_land_abstract') # Since this abstract is top level in the control chain there is no need to check # for a safety state. We can also get away with not checking for a fatal state since # all nodes below will shut down. assert(safety_client.form_bond()) if rospy.is_shutdown(): return # Creates the SimpleActionClient, passing the type of the action # (QuadMoveAction) to the constructor. (Look in the action folder) client = actionlib.SimpleActionClient("motion_planner_server", QuadMoveAction) # Waits until the action server has started up and started # listening for goals. client.wait_for_server() if rospy.is_shutdown(): return rospy.sleep(2.0) # Test takeoff goal = QuadMoveGoal(movement_type="takeoff") # Sends the goal to the action server. client.send_goal(goal) # Waits for the server to finish performing the action. client.wait_for_result() if rospy.is_shutdown(): return rospy.logwarn("Takeoff success: {}".format(client.get_result())) rospy.sleep(2.0) # Fly around in square all diagonals when possible. goal = QuadMoveGoal(movement_type="xyztranslate", x_position=4.0, y_position=4.0, z_position=3.0) client.send_goal(goal) client.wait_for_result() if rospy.is_shutdown(): return rospy.logwarn("Waypoint 1 success: {}".format(client.get_result())) goal = QuadMoveGoal(movement_type="xyztranslate", x_position=-4.0, y_position=-4.0, z_position=8.0) client.send_goal(goal) client.wait_for_result() if rospy.is_shutdown(): return rospy.logwarn("Waypoint 2 success: {}".format(client.get_result())) goal = QuadMoveGoal(movement_type="xyztranslate", x_position=4.0, y_position=-4.0, z_position=1.0) client.send_goal(goal) client.wait_for_result() if rospy.is_shutdown(): return rospy.logwarn("Waypoint 3 success: {}".format(client.get_result())) goal = QuadMoveGoal(movement_type="xyztranslate", x_position=-4.0, y_position=4.0, z_position=3.0) client.send_goal(goal) client.wait_for_result() if rospy.is_shutdown(): return rospy.logwarn("Waypoint 4 success: {}".format(client.get_result())) # Test land goal = QuadMoveGoal(movement_type="land") # Sends the goal to the action server. client.send_goal(goal) # Waits for the server to finish performing the action. client.wait_for_result() if rospy.is_shutdown(): return rospy.logwarn("Land success: {}".format(client.get_result()))
def velocity_test(): safety_client = SafetyClient('velocity_test_abstract') # Since this abstract is top level in the control chain there is no need to check # for a safety state. We can also get away with not checking for a fatal state since # all nodes below will shut down. assert (safety_client.form_bond()) if rospy.is_shutdown(): return # Creates the SimpleActionClient, passing the type of the action # (QuadMoveAction) to the constructor. (Look in the action folder) client = actionlib.SimpleActionClient("motion_planner_server", QuadMoveAction) # Waits until the action server has started up and started # listening for goals. client.wait_for_server() if rospy.is_shutdown(): return X_VELOCITY = 0.5 Y_VELOCITY = 0.5 X_DELAY = 2.0 Y_DELAY = 2.0 STOP_DELAY = 2.0 # Test takeoff goal = QuadMoveGoal(movement_type="takeoff") # Sends the goal to the action server. client.send_goal(goal) # Waits for the server to finish performing the action. client.wait_for_result() if rospy.is_shutdown(): return rospy.logwarn("Takeoff success: {}".format(client.get_result())) for i in range(0, 1): goal = QuadMoveGoal(movement_type="velocity_test", x_velocity=X_VELOCITY, y_velocity=0.0, z_position=0.8) # Sends the goal to the action server. client.send_goal(goal) rospy.sleep(X_DELAY) client.cancel_goal() rospy.logwarn("Translation 1 canceled") goal = QuadMoveGoal(movement_type="velocity_test", x_velocity=0.0, y_velocity=0.0, z_position=0.8) # Sends the goal to the action server. client.send_goal(goal) rospy.sleep(STOP_DELAY) client.cancel_goal() rospy.logwarn("Stop 1 canceled") goal = QuadMoveGoal(movement_type="velocity_test", x_velocity=0.0, y_velocity=-Y_VELOCITY, z_position=0.8) # Sends the goal to the action server. client.send_goal(goal) rospy.sleep(Y_DELAY) client.cancel_goal() rospy.logwarn("Translation 2 canceled") goal = QuadMoveGoal(movement_type="velocity_test", x_velocity=0.0, y_velocity=0.0, z_position=0.8) # Sends the goal to the action server. client.send_goal(goal) rospy.sleep(STOP_DELAY) client.cancel_goal() rospy.logwarn("Stop 2 canceled") goal = QuadMoveGoal(movement_type="velocity_test", x_velocity=-X_VELOCITY, y_velocity=0.0, z_position=0.8) # Sends the goal to the action server. client.send_goal(goal) rospy.sleep(X_DELAY) client.cancel_goal() rospy.logwarn("Translation 3 canceled") goal = QuadMoveGoal(movement_type="velocity_test", x_velocity=0.0, y_velocity=0.0, z_position=0.8) # Sends the goal to the action server. client.send_goal(goal) rospy.sleep(STOP_DELAY) client.cancel_goal() rospy.logwarn("Stop 3 canceled") goal = QuadMoveGoal(movement_type="velocity_test", x_velocity=0.0, y_velocity=Y_VELOCITY, z_position=0.8) # Sends the goal to the action server. client.send_goal(goal) rospy.sleep(Y_DELAY) client.cancel_goal() rospy.logwarn("Translation 4 canceled") goal = QuadMoveGoal(movement_type="velocity_test", x_velocity=0.0, y_velocity=0.0, z_position=0.8) # Sends the goal to the action server. client.send_goal(goal) rospy.sleep(STOP_DELAY) client.cancel_goal() rospy.logwarn("Stop 4 canceled") # Test land goal = QuadMoveGoal(movement_type="land") # Sends the goal to the action server. client.send_goal(goal) # Waits for the server to finish performing the action. client.wait_for_result() if rospy.is_shutdown(): return rospy.logwarn("Land success: {}".format(client.get_result()))
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
from iarc7_motion.msg import GroundInteractionGoal, GroundInteractionAction from iarc7_msgs.msg import TwistStampedArray from iarc7_msgs.srv import Arm from geometry_msgs.msg import TwistStamped from iarc7_safety.SafetyClient import SafetyClient def constrain(x, l, h): return min(h, max(x, l)) if __name__ == '__main__': rospy.init_node('waypoints_test') safety_client = SafetyClient('motion_planner') assert safety_client.form_bond() velocity_pub = rospy.Publisher('movement_velocity_targets', TwistStampedArray, queue_size=0) tf_listener = tf.TransformListener() while not rospy.is_shutdown() and rospy.Time.now() == 0: pass start_time = rospy.Time.now() arm_service = rospy.ServiceProxy('uav_arm', Arm) arm_service.wait_for_service() armed = False while not rospy.is_shutdown() and not armed:
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()
def test(): safety_client = SafetyClient('invalid_task_transition_test') # Since this abstract is top level in the control chain there is no need to check # for a safety state. We can also get away with not checking for a fatal state since # all nodes below will shut down. assert safety_client.form_bond() if rospy.is_shutdown(): return # Creates the SimpleActionClient, passing the type of the action # (QuadMoveAction) to the constructor. (Look in the action folder) client = actionlib.SimpleActionClient("motion_planner_server", QuadMoveAction) # Waits until the action server has started up and started # listening for goals. client.wait_for_server() if rospy.is_shutdown(): return rospy.sleep(2.0) # arbitrarily picked fifth roomba to test roomba_id = roomba_array.data[5].child_frame_id rospy.logwarn("Testing illegal state #1") goal = QuadMoveGoal(movement_type="track_roomba", frame_id=roomba_id, time_to_track=5.0, x_overshoot=0, y_overshoot=0) # Sends the goal to the action server. client.send_goal(goal) # Waits for the server to finish performing the action. client.wait_for_result() if rospy.is_shutdown(): return assert client.get_result().success == False rospy.sleep(2.0) # Testing trying to cancel takeoff rospy.logwarn("Testing illegal state #2, canceling takeoff") goal = QuadMoveGoal(movement_type="takeoff") # Sends the goal to the action server. client.send_goal(goal) rospy.sleep(1.0) client.cancel_goal() rospy.sleep(2.0) rospy.logwarn("RESET STATE") goal = QuadMoveGoal(movement_type="land") # Sends the goal to the action server. client.send_goal(goal) # Waits for the server to finish performing the action. client.wait_for_result() if rospy.is_shutdown(): return rospy.logwarn("Land success: {}".format(client.get_result())) rospy.sleep(2.0) rospy.logwarn("Testing illegal state #3") goal = QuadMoveGoal(movement_type="hit_roomba", frame_id=roomba_id) # Sends the goal to the action server. client.send_goal(goal) # Waits for the server to finish performing the action. client.wait_for_result() if rospy.is_shutdown(): return assert client.get_result().success == False rospy.sleep(2.0) rospy.logwarn("RESET STATE") goal = QuadMoveGoal(movement_type="takeoff") # Sends the goal to the action server. client.send_goal(goal) # Waits for the server to finish performing the action. client.wait_for_result() if rospy.is_shutdown(): return rospy.logwarn("Takeoff success: {}".format(client.get_result())) rospy.sleep(2.0) rospy.logwarn("Testing invalid RoombaID") goal = QuadMoveGoal(movement_type="track_roomba", time_to_track=5.0, x_overshoot=.5, y_overshoot=.5) # Sends the goal to the action server. client.send_goal(goal) # Waits for the server to finish performing the action. client.wait_for_result() if rospy.is_shutdown(): return assert client.get_result().success == False rospy.logwarn("Preparing to test illegal state #4") goal = QuadMoveGoal(movement_type="hit_roomba", frame_id=roomba_id) # Sends the goal to the action server. client.send_goal(goal) # Waits for the server to finish performing the action. client.wait_for_result() if rospy.is_shutdown(): return rospy.logwarn("Hit Roomba success: {}".format(client.get_result())) rospy.sleep(2.0) rospy.logwarn("Testing illegal state #4") goal = QuadMoveGoal(movement_type="track_roomba", frame_id=roomba_id, time_to_track=5.0, x_overshoot=.5, y_overshoot=.5) # Sends the goal to the action server. client.send_goal(goal) # Waits for the server to finish performing the action. client.wait_for_result() if rospy.is_shutdown(): return assert client.get_result().success == False rospy.sleep(2.0) rospy.logwarn("RESET STATE") goal = QuadMoveGoal(movement_type="height_recovery") # Sends the goal to the action server. client.send_goal(goal) # Waits for the server to finish performing the action. client.wait_for_result() if rospy.is_shutdown(): return rospy.logwarn("Height Recovery success: {}".format(client.get_result())) rospy.sleep(2.0) rospy.logwarn("Sending Test Task") goal = QuadMoveGoal(movement_type="test_task", takeoff_height=0.75) # Sends the goal to the action server. client.send_goal(goal) # Waits for the server to finish performing the action. client.wait_for_result() if rospy.is_shutdown(): return rospy.logwarn("Test Task success: {}".format(client.get_result())) rospy.sleep(2.0) rospy.logwarn("RESET STATE") goal = QuadMoveGoal(movement_type="land") # Sends the goal to the action server. client.send_goal(goal) # Waits for the server to finish performing the action. client.wait_for_result() if rospy.is_shutdown(): return rospy.logwarn("Land success: {}".format(client.get_result())) rospy.logwarn("Testing is complete")
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