def __init__(self, shutdown_flag):
# Create vehicle interface
self._vehicle = flightsys.Vehicle()
# Define queue size
self._queue_size = 1
# Create publishers
self._vel_pub = rospy.Publisher('/mavros/setpoint_velocity/cmd_vel',
TwistStamped,
queue_size=self._queue_size)
self._pos_pub = rospy.Publisher('/mavros/setpoint_position/local',
PoseStamped,
queue_size=self._queue_size)
self._att_pub = rospy.Publisher(
'/mavros/setpoint_attitude/att_throttle',
Float64,
queue_size=self._queue_size)
# Threading event
self.shutdown_flag = shutdown_flag
# Setup variables
self._log_tag = "[FC] "
self._takeoff_altitude = 2.0
self._target_velocity = (0.0, 0.0, 0.0)
self._target_position = (0.0, 0.0, 0.0)
self._target_position = (0.0, 0.0, 0.0)
self._target_position = None
self._allowed_error = 0.25
self._mode = Control_Mode.INITIAL
self._previous_mode = Control_Mode.INITIAL
# Set rospy rate
self._rate = rospy.Rate(10)
# Start thread
try:
# Create thread object
self.thread = threading.Thread(target=self.loop)
# Start thread
self.thread.start()
# Allow thread to start
rospy.sleep(1)
except Exception as e:
rospy.logerr("Unable to start flight control thread: %s" % str(e))
def __init__(self):
# Create vehicle interface
self._vehicle = flightsys.Vehicle()
# Store parameters
self.log_tag = "[FD] "
self._queue_size = 1
# Flight information
self._current_flight = None
self._previous_flight = None
# Create rospy rate
self._rate = rospy.Rate(10)
# Log startup
rospy.loginfo(self.log_tag + "Starting flight director...")
# Store flights
self._available_flights = []
# Setup services
self._start_flight_srv = rospy.Service('/redbird/flight_director/start_flight', StartFlight, self.start_flight_service_handler)
self._get_flights_srv = rospy.Service('/redbird/flight_director/get_flights', GetFlights, self.get_flights_service_handler)
self._kill_flight_srv = rospy.Service('/redbird/flight_director/kill_flight', KillFlight, self.kill_flight_service_handler)
# Setup publishers
self._flight_state_pub = rospy.Publisher('/redbird/flight_director/flight_state', FlightState, queue_size=self._queue_size)
# Spawn flight state thread
try:
# Create thread object
self._flight_state_thread = threading.Thread(target=self.flight_state_thread, args=())
# Start thread
self._flight_state_thread.start()
# Allow thread to start
rospy.sleep(1)
except Exception as e:
rospy.logerr(self.log_tag + "Unable to start flight state monitor thread: %s" % e)
# Log service ready
rospy.loginfo(self.log_tag + "Flight services ready")
# Register shutdown hook
rospy.on_shutdown(self.shutdown)
def __init__(self, shutdown_flag, vehicle_control_handlers):
"""
Args:
vehicle_control_handlers (VehicleControlInterfaces): the way the
Controller send its desired states (e.g., velocity, position, etc)
"""
# Create vehicle interface
self._vehicle = flightsys.Vehicle()
self._queue_size = 1
if type(vehicle_control_handlers) is not VehicleControlInterfaces:
raise TypeError("type mismatch")
self.vehicle_ctrl_handlers = vehicle_control_handlers
# Threading event
self.shutdown_flag = shutdown_flag
# Setup variables
self._log_tag = "[FC] "
self._takeoff_altitude = 2.0
self._target_velocity = (0.0, 0.0, 0.0)
self._target_position = (0.0, 0.0, 0.0)
self._target_position = None
self._allowed_error = 0.25
self._mode = Control_Mode.INITIAL
self._previous_mode = Control_Mode.INITIAL
# Set rospy rate
self._rate = rospy.Rate(10)
# Start thread
try:
# Create thread object
self.thread = threading.Thread(target=self.loop)
# Start thread
self.thread.start()
# Allow thread to start
rospy.sleep(1)
except Exception as e:
rospy.logerr("Unable to start flight control thread: %s" % str(e))
def run(self):
"""Runs the flight inside of a try-except block."""
try:
# Create vehicle
self.vehicle = flightsys.Vehicle()
# Create controller
vel_pub = rospy.Publisher('/mavros/setpoint_velocity/cmd_vel',
TwistStamped,
queue_size=self._queue_size)
pos_pub = rospy.Publisher('/mavros/setpoint_position/local',
PoseStamped,
queue_size=self._queue_size)
att_pub = rospy.Publisher('/mavros/setpoint_attitude/att_throttle',
Float64,
queue_size=self._queue_size)
vehicle_control_interface = flightsys.VehicleControlInterfaces(
set_target_velocity=vel_pub.publish,
set_target_position=pos_pub.publish,
set_target_thrust=att_pub.publish)
self.controller = flightsys.Controller(self.shutdown_flag,
vehicle_control_interface)
# Reset flight diagnositcs
self.start_time = 0.0
self.end_time = 0.0
self.end_reason = Flight_End_Reason.INITIAL
# Flight starting
rospy.loginfo(self.log_tag +
"Flight ready. Waiting for OFFBOARD...")
# Wait for offboard
while not self.is_running():
pass
# Set start time
self.start_time = rospy.get_time()
rospy.loginfo(self.log_tag +
"OFFBOARD entered. Starting flight...")
# Arm the vehicle
self.vehicle.arm()
rospy.loginfo(self.log_tag +
"Waiting for full FLOW initialization...")
self.sleep(10)
# Start flight
rospy.loginfo(self.log_tag + "Starting flight!")
self.flight()
# Set end time
self.end_time = rospy.get_time()
# Set end type
self.end_reason = Flight_End_Reason.NATURAL
# Flight complete
rospy.loginfo(self.log_tag + "Flight complete")
# Set poison
self.shutdown_flag.set()
except Exception as e:
# Set poison
self.shutdown_flag.set()
# Record end time
self.end_time = rospy.get_time()
# Set end state
self.end_reason = Flight_End_Reason.KILLED
# Log error
rospy.logerr(self.log_tag + "%s" % str(e))
def run(self):
"""Runs the flight inside of a try-except block."""
try:
# Create vehicle
self.vehicle = flightsys.Vehicle()
# Create controller
self.controller = flightsys.Controller(self.shutdown_flag)
# Reset flight diagnositcs
self.start_time = 0.0
self.end_time = 0.0
self.end_reason = Flight_End_Reason.INITIAL
# Flight starting
rospy.loginfo(self.log_tag +
"Flight ready. Waiting for OFFBOARD...")
# Wait for offboard
while not self.is_running():
pass
# Set start time
self.start_time = rospy.get_time()
rospy.loginfo(self.log_tag +
"OFFBOARD entered. Starting flight...")
# Arm the vehicle
self.vehicle.arm()
rospy.loginfo(self.log_tag +
"Waiting for full FLOW initialization...")
self.sleep(10)
# Start flight
rospy.loginfo(self.log_tag + "Starting flight!")
self.flight()
# Set end time
self.end_time = rospy.get_time()
# Set end type
self.end_reason = Flight_End_Reason.NATURAL
# Flight complete
rospy.loginfo(self.log_tag + "Flight complete")
# Set poison
self.shutdown_flag.set()
except Exception as e:
# Set poison
self.shutdown_flag.set()
# Record end time
self.end_time = rospy.get_time()
# Set end state
self.end_reason = Flight_End_Reason.KILLED
# Log error
rospy.logerr(self.log_tag + "%s" % str(e))