async def takeoff(self):
"""
Takeoff and start offboard mode.
Return true if success, false otherwise
"""
print("-- Arming")
await self.drone.action.arm()
# Takeoff the vehicle (later, we can assume that the drone is already flying when we run this script)
print("-- Taking off")
await self.drone.action.set_takeoff_altitude(3.0) # meters
await self.drone.action.takeoff()
await asyncio.sleep(8)
# start offboard mode (requires setting initial setpoint)
print("-- Setting initial setpoint")
await self.drone.offboard.set_velocity_ned(
VelocityNedYaw(0.0, 0.0, 0.0, 135))
print("-- Starting offboard")
try:
await self.drone.offboard.start()
except OffboardError as error:
print(f"Starting offboard mode failed with error code: \
{error._result.result}")
print("-- Disarming")
await self.drone.action.disarm()
return False
return True
def __init__(self):
self.positionCommands = PositionNedYaw(0.0, 0.0, 0.0, 0.0)
self.feedForwardVelocity = VelocityNedYaw(0.0, 0.0, 0.0, 0.0)
self.prevPoseTime = 0.0
self.estimateMsg = Odometry()
self.meas1_received = False
self.flightMode = 'none'
self.offBoardOn = False
self.sim = rospy.get_param('~sim', False)
self.mocap = rospy.get_param('~mocap', False)
self.in_air = 0
self.arm_status = 0
if self.sim == True:
self.systemAddress = rospy.get_param('~simSystemAddress',
"udp://:14540")
else:
self.systemAddress = rospy.get_param(
'~realSystemAddress', "serial:///dev/ttyUSB0:921600")
self.estimate_pub_ = rospy.Publisher('rover_odom',
Odometry,
queue_size=5,
latch=True)
self.commands_sub_ = rospy.Subscriber('commands',
Odometry,
self.commandsCallback,
queue_size=5)
if self.mocap:
self.positiion_measurement_sub_ = rospy.Subscriber(
'position_measurement',
PoseStamped,
self.positionMeasurementCallback,
queue_size=5)
async def stage1(self, id):
"""Position the drone above the large central target, returns true if successful"""
print("Docking stage 1")
self.log.writeLiteral("Docking stage 1")
debug_window=DebugWindow(1,self.target)
failed_frames = 0 # Number of frames since we detected the target
successful_frames = 0 # Number of frames within tolerance
pid_controller = PIDController(self.dt)
while True:
start_millis = int(round(time.time() * 1000))
img = self.camera_simulator.updateCurrentImage(self.east, self.north, self.down * -1.0, self.yaw)
errs = self.image_analyzer.process_image(img, 0, self.yaw)
if errs is None:
self.log.writeLiteral("No Errors")
failed_frames = failed_frames + 1
if self.down * -1.0 - self.target.getAlt() > self.MAX_HEIGHT: # drone moves above max height, docking fails
return False
if failed_frames > 1 / self.dt: # haven't detected target in 1 second
await self.drone.offboard.set_velocity_body(
VelocityBodyYawspeed(0, 0, -0.2, 0))
await asyncio.sleep(self.dt)
continue
failed_frames = 0
x_err, y_err, alt_err, rot_err, tags_detected = errs
debug_window.updateWindow(self.east, self.north, self.down * -1.0, self.yaw, tags_detected)
if self.logging:
self.log.write(self.east, self.north, self.down * -1.0, self.yaw, tags_detected,successful_frames,x_err,y_err,alt_err,rot_err)
x_err, y_err, alt_err = self.offset_errors(x_err, y_err, alt_err, rot_err, id)
east_velocity, north_velocity, down_velocity = pid_controller.get_velocities(x_err, y_err, alt_err, 0.4)
# Checks if drone is aligned with central target
if util.is_between_symm(alt_err, self.STAGE_1_TOLERANCE) and util.is_between_symm(x_err, self.STAGE_1_TOLERANCE) and util.is_between_symm(y_err, self.STAGE_1_TOLERANCE):
successful_frames += 1
else:
successful_frames = 0
# Ends loop if aligned for 1 second
if successful_frames == 1 / self.dt:
break
await self.drone.offboard.set_velocity_ned(
VelocityNedYaw(north_m_s=north_velocity, east_m_s=east_velocity, down_m_s=down_velocity, yaw_deg=rot_err)) # north, east, down (all m/s), yaw (degrees, north is 0, positive for clockwise)
current_millis = int(round(time.time() * 1000))
if current_millis - start_millis < self.dt:
await asyncio.sleep(self.dt - (current_millis - start_millis))
debug_window.destroyWindow()
return True
async def safe_land(self):
dt=0.05
for i in range (5/0.05): #Drone flies away up and to the north for 5 seconds
await self.drone.offboard.set_velocity_ned(
VelocityNedYaw(0.4, 0, -0.4, 0.0)) #These numbers can be changed to get out of GPS error margin
asyncio.sleep(dt)
await self.land()
async def __call__(self, drone):
await drone.arm(coordinate=[0.0,0.0,0.0],attitude=[0.0,0.0,0.0])
await drone.start_offboard()
print("-- Starting Circle")
#set aircraft to flip at 300 deg/s to the right (roll)
async for position_ned in drone.conn.telemetry.position_velocity_ned():
start_position = np.array([position_ned.position.north_m,position_ned.position.east_m])#,position_ned.position.down_m])
start_height = position_ned.position.down_m
# print('hi1')
break
circle_center = np.array([start_position[0] + self.radius,start_position[1]])
circle_otherside = np.array([start_position[0] + 2*self.radius,start_position[1]])
# print('hi')
async for position_ned in drone.conn.telemetry.position_velocity_ned():
currentposn = np.array([position_ned.position.north_m,position_ned.position.east_m])
angle_to_center = np.arctan2(*(circle_center-currentposn)[::-1])
distance2center = np.linalg.norm(circle_center-currentposn)
distanceController = self.radius - distance2center
if self.direction:
tangentangle = angle_to_center + ( np.pi / 2 )
else:
tangentangle = angle_to_center + ( np.pi / 2 ) + np.pi
# print('-------')
# print(tangentangle*180/np.pi)
# print((tangentangle+np.pi)*180/np.pi )
if self.direction:
newangle = tangentangle + distanceController/2.0
else:
newangle = tangentangle - distanceController/2.0
xvelocity = np.cos(newangle)*self.velocity
yvelocity = np.sin(newangle)*self.velocity
zdelta = position_ned.position.down_m - start_height
zvelocity = saturate(-0.1,0.1,(zdelta*-0.5))
await drone.conn.offboard.set_velocity_ned(VelocityNedYaw(xvelocity,yvelocity,zvelocity,0.0))
if self.otherside_flag:
if distance_between(circle_otherside, currentposn) < self.tolerance:
print("otherside")
self.otherside_flag = 0
else:
if distance_between(start_position, currentposn) < self.tolerance:
self.otherside_flag = 1
self.currentLoops += 1
if self.currentLoops >= self.desiredLoops:
print(f"{self.desiredLoops} completed!")
# print(f"-- Go to {self.target[0]}m North, {self.target[1]}m East, {self.target[2]}m Down within local coordinate system")
await drone.conn.offboard.set_position_ned(PositionNedYaw(*start_position, start_height, 0.0))
break
async def set_vel_yaw(self, vel, yaw, freq):
"""
Offboard method that sends velocity and yaw references to the PX4 autopilot of the the vehicle.
Parameters
----------
vel : np.array of floats with shape (3,1)
Desired linear velocity for drone, in local NED coordinates.
yaw : float
Desired yaw for the vehicle, in radians.
freq : float
Topic publishing frequency, in Hz.
"""
msg = VelocityNedYaw(vel[0][0], vel[1][0], vel[2][0],
math.degrees(yaw))
await self.system.offboard.set_velocity_ned(msg)
await asyncio.sleep(1 / freq)
async def __call__(self, drone):
# print(drone.conn.telemetry.armed)
await drone.arm(coordinate=[0.0,0.0,0.0],attitude=[0.0,0.0,0.0])
await drone.start_offboard()
# await drone.conn.offboard.set_velocity_ned(VelocityNedYaw(*self.target, 0.0))
flag = True
#implement a crude cross track controller
print(f"-- Landing at {self.positions[0]}m North, {self.positions[1]}m East")
async for position_ned in drone.conn.telemetry.position_velocity_ned():
ret, image = cam.read()
small_to_large_image_size_ratio = 0.5
# small_img= cv2.resize(img1,(0,0),fx=small_to_large_image_size_ratio, fy=small_to_large_image_size_ratio, interpolation=cv2.INTER_NEAREST)
small_frame= cv2.resize(frame,(0,0),fx=small_to_large_image_size_ratio, fy=small_to_large_image_size_ratio, interpolation=cv2.INTER_NEAREST)
x,y = surf(img1,small_frame)
currentposn = np.array([position_ned.position.north_m,position_ned.position.east_m, position_ned.position.down_m])
xerror = self.positions[0] - currentposn[0]
yerror = self.positions[1] - currentposn[1]
xvelocity = xerror *2
yvelocity = yerror *2
zvelocity = -0.5 *currentposn[2]
if(currentposn[2]>self.slowHeight):
zvelocity = -0.1 *currentposn[2]
await drone.conn.offboard.set_velocity_ned(VelocityNedYaw(xvelocity,yvelocity,zvelocity,0.0))
await asyncio.sleep(0.01)
if(currentposn[2]>-1):
# await drone.conn.action.land()
break
cam.release()
async def run(self):
drone = System()
print('system address = ', self.systemAddress)
await drone.connect(system_address=self.systemAddress)
print("Waiting for drone to connect...")
await asyncio.sleep(5)
async for state in drone.core.connection_state():
if state.is_connected:
print(f"Drone discovered with UUID: {state.uuid}")
break
#TODO publish all of these messages, so that rosbags contain this information
await drone.telemetry.set_rate_odometry(100)
asyncio.ensure_future(self.input_meas_output_est(drone))
asyncio.ensure_future(self.flight_modes(drone))
# await drone.telemetry.set_rate_attitude(1) #doesn't seem to affect euler?
# asyncio.ensure_future(self.print_euler(drone))
await drone.telemetry.set_rate_battery(0.1)
asyncio.ensure_future(self.print_battery(drone))
await drone.telemetry.set_rate_in_air(1)
asyncio.ensure_future(self.print_in_air(drone))
await drone.telemetry.set_rate_landed_state(1)
asyncio.ensure_future(self.print_landed_state(drone))
await drone.telemetry.set_rate_rc_status(0.1)
asyncio.ensure_future(self.print_landed_state(drone))
# await drone.telemetry.set_rate_gps_info(0.1) #This message slows down estimate by 99 hz.
# asyncio.ensure_future(self.print_gps_info(drone))
asyncio.ensure_future(self.print_status(drone))
asyncio.ensure_future(self.print_armed(drone))
asyncio.ensure_future(self.print_health(drone))
if self.sim == True:
await drone.action.arm()
await drone.offboard.set_position_velocity_ned(
PositionNedYaw(0.0, 0.0, 0.0, 0.0),
VelocityNedYaw(0.0, 0.0, 0.0, 0.0))
await drone.offboard.start()
print("Simulation starting offboard.")
print('end of run')
async def __call__(self, drone):
# print(drone.conn.telemetry.armed)
await drone.arm(coordinate=[0.0, 0.0, 0.0], attitude=[0.0, 0.0, 0.0])
await drone.start_offboard()
await drone.register_sensor("imu", drone.conn.telemetry.imu())
await drone.register_sensor(
"ned", drone.conn.telemetry.position_velocity_ned())
while drone.imu is None or drone.ned is None:
await asyncio.sleep(0)
# await drone.conn.offboard.set_velocity_ned(VelocityNedYaw(*self.target, 0.0))
#implement a crude cross track controller
while True:
currentposn = np.array(
[drone.ned.position.north_m, drone.ned.position.east_m])
trackangle = np.arctan2(*(self.positions[1] -
self.positions[0])[::-1])
angle_to_target = np.arctan2(*(self.positions[1] -
currentposn)[::-1])
delta_heading = trackangle - angle_to_target
distance2WP = np.linalg.norm(self.positions[1] - currentposn)
crosstrackerror = distance2WP * np.sin(delta_heading)
# crosstrackcorrection = crosstrackerror * 2.0
# projectedremainingtrack = (2.0/3.0)*distance2WP*cos(delta_heading)
newheading = angle_to_target - crosstrackerror * (3.14159 / 18)
xvelocity = np.cos(newheading) * self.target
yvelocity = np.sin(newheading) * self.target
# print(f'trackangle: {trackangle}')
# print(f'angle_to_target: {angle_to_target}')
# print(f'Current POSN: {currentposn}')
# print(f'Xvelocity: {xvelocity}')
# print(f'Yvelocity: {yvelocity}')
await drone.conn.offboard.set_velocity_ned(
VelocityNedYaw(xvelocity, yvelocity, 0.0, 0.0))
await asyncio.sleep(1)
async def __call__(self, drone):
await drone.arm(coordinate=[0.0,0.0,0.0],attitude=[0.0,0.0,0.0])
await drone.start_offboard()
await drone.register_sensor("imu", drone.conn.telemetry.imu())
await drone.register_sensor("ned", drone.conn.telemetry.position_velocity_ned())
while drone.imu is None or drone.ned is None:
await asyncio.sleep(0)
print("-- Starting Circle")
#set aircraft to flip at 300 deg/s to the right (roll)
start_position = np.array([drone.ned.position.north_m,drone.ned.position.east_m])
circle_center = np.array([drone.ned.position.north_m + self.radius,drone.ned.position.east_m])
while True:
currentposn = np.array([drone.ned.position.north_m,drone.ned.position.east_m])
angle_to_center = np.arctan2(*(circle_center-currentposn)[::-1])
distance2center = np.linalg.norm(circle_center-currentposn)
distanceController = self.radius - distance2center
if self.direction:
tangentangle = angle_to_center + ( np.pi / 2 )
else:
tangentangle = angle_to_center + ( np.pi / 2 ) + np.pi
print('-------')
print(tangentangle*180/np.pi)
print((tangentangle+np.pi)*180/np.pi )
if self.direction:
newangle = tangentangle + distanceController/2.0
else:
newangle = tangentangle - distanceController/2.0
xvelocity = np.cos(newangle)*self.velocity
yvelocity = np.sin(newangle)*self.velocity
await drone.conn.offboard.set_velocity_ned(VelocityNedYaw(xvelocity,yvelocity,0.0,0.0))
async def __call__(self, drone):
# print(drone.conn.telemetry.armed)
await drone.arm(coordinate=[0.0, 0.0, 0.0], attitude=[0.0, 0.0, 0.0])
await drone.start_offboard()
# await drone.conn.offboard.set_velocity_ned(VelocityNedYaw(*self.target, 0.0))
flag = True
#implement a crude cross track controller
print(
f"-- Landing at {self.positions[0]}m North, {self.positions[1]}m East"
)
async for position_ned in drone.conn.telemetry.position_velocity_ned():
currentposn = np.array([
position_ned.position.north_m, position_ned.position.east_m,
position_ned.position.down_m
])
xerror = self.positions[0] - currentposn[0]
yerror = self.positions[1] - currentposn[1]
xvelocity = xerror * 2
yvelocity = yerror * 2
zvelocity = -0.5 * currentposn[2]
if (currentposn[2] > self.slowHeight):
zvelocity = -0.1 * currentposn[2]
await drone.conn.offboard.set_velocity_ned(
VelocityNedYaw(xvelocity, yvelocity, zvelocity, 0.0))
await asyncio.sleep(0.01)
if (currentposn[2] > -1):
# await drone.conn.action.land()
break
async def stage2(self, id):
"""Position the drone above the peripheral target and descend"""
print("Docking stage 2")
self.log.writeLiteral("Docking stage 2")
debug_window = DebugWindow(2,self.target)
pid_controller = PIDController(self.dt)
successful_frames = 0
while True:
img = self.camera_simulator.updateCurrentImage(self.east, self.north, self.down * -1.0, self.yaw,id)
errs = self.image_analyzer.process_image(img, id, self.yaw)
checked_frames = 0
docking_attempts = 0
# Waits for one second to detect target tag, ascends to find central target if fails
while errs is None:
checked_frames += 1
self.log.writeLiteral("No errors, checked frames: "+str(checked_frames))
await self.drone.offboard.set_velocity_body(VelocityBodyYawspeed(0, 0, -0.1, 0))
if checked_frames > 1 / self.dt:
docking_attempts += 1
if docking_attempts > self.MAX_ATTEMPTS:
print("Docking failed")
self.log.writeLiteral("Docking failed")
await self.safe_land()
return
img = self.camera_simulator.updateCurrentImage(self.east, self.north, self.down * -1.0, self.yaw,id)
errs = self.image_analyzer.process_image(img,0,self.yaw)
# Ascends until maximum height or until peripheral target detected
while errs is None and self.down * -1.0 - self.target.getAlt() < self.MAX_HEIGHT_STAGE_2:
await self.drone.offboard.set_velocity_body(VelocityBodyYawspeed(0, 0, -0.2, 0))
img = self.camera_simulator.updateCurrentImage(self.east, self.north, self.down * -1.0, self.yaw, id)
errs = self.image_analyzer.process_image(img, id, self.yaw)
if not errs is None:
break
# If peripheral tag not found, ascends until maximum height or until central target detected
while errs is None and self.down * -1.0 - self.target.getAlt() < self.MAX_HEIGHT:
await self.drone.offboard.set_velocity_body(VelocityBodyYawspeed(0, 0, -0.2, 0))
img = self.camera_simulator.updateCurrentImage(self.east, self.north, self.down * -1.0, self.yaw)
errs = self.image_analyzer.process_image(img,0, self.yaw)
# Re-attempts stage 1
if not errs is None:
await self.stage1(id)
img = self.camera_simulator.updateCurrentImage(self.east, self.north, self.down * -1.0, self.yaw, id)
errs = self.image_analyzer.process_image(img, id, self.yaw)
checked_frames = 0
else: # If the central target cannot be found at maximum height (maybe could have re-attempt stage 1 again, not sure)
print("Docking failed")
await self.safe_land()
return
await asyncio.sleep(self.dt)
img = self.camera_simulator.updateCurrentImage(self.east, self.north, self.down * -1.0, self.yaw,id)
errs = self.image_analyzer.process_image(img, id,self.yaw)
x_err, y_err, alt_err, rot_err, tags_detected = errs
alt_err = alt_err - .05
if self.logging:
self.log.write(self.east, self.north, self.down * -1.0, self.yaw, tags_detected,successful_frames,x_err,y_err,alt_err,rot_err)
# Checks if drone is aligned with docking
if alt_err < self.STAGE_2_TOLERANCE * 2 and alt_err > -1 * self.STAGE_2_TOLERANCE and rot_err < 2.0 and rot_err > -2.0 and x_err > -1 * self.STAGE_2_TOLERANCE and x_err < 1 * self.STAGE_2_TOLERANCE and y_err > -1 * self.STAGE_2_TOLERANCE and y_err<self.STAGE_2_TOLERANCE:
successful_frames += 1
else:
successful_frames = 0
# Adjusts errors for distance to target to prevent overshoot
# Adjusted errors asymptote to 1 as alt_err increases, goes to 0 as alt_err decreases
OVERSHOOT_CONSTANT=.6 # use to adjust speed of descent, higher constant means faster, lower means less overshooting
# x_err*=np.tanh(OVERSHOOT_CONSTANT*alt_err*alt_err)
# y_err*=np.tanh(OVERSHOOT_CONSTANT*alt_err*alt_err)
# rot_err*=np.tanh(OVERSHOOT_CONSTANT*alt_err*alt_err)
# alt_err*=np.tanh(OVERSHOOT_CONSTANT*alt_err*alt_err)
# Ends loop if aligned for 1 second
if successful_frames == 1 / self.dt:
break
debug_window.updateWindow(self.east, self.north, self.down * -1.0, self.yaw, tags_detected)
east_velocity, north_velocity, down_velocity = pid_controller.get_velocities(x_err, y_err, alt_err, 0.1)
await self.drone.offboard.set_velocity_ned(
VelocityNedYaw(north_m_s=north_velocity, east_m_s=east_velocity, down_m_s=down_velocity, yaw_deg=rot_err)) # north, east, down (all m/s), yaw (degrees, north is 0, positive for clockwise)
await asyncio.sleep(self.dt)
async def run():
""" Does Offboard control using velocity NED coordinates. """
drone = System()
await drone.connect(system_address="udp://:14540")
print("Waiting for drone to connect...")
async for state in drone.core.connection_state():
if state.is_connected:
print(f"Drone discovered!")
break
print("-- Arming")
await drone.action.arm()
print("-- Setting initial setpoint")
await drone.offboard.set_velocity_ned(VelocityNedYaw(0.0, 0.0, 0.0, 0.0))
print("-- Starting offboard")
try:
await drone.offboard.start()
except OffboardError as error:
print(f"Starting offboard mode failed with error code: \
{error._result.result}")
print("-- Disarming")
await drone.action.disarm()
return
print("-- Go up 2 m/s")
await drone.offboard.set_velocity_ned(VelocityNedYaw(0.0, 0.0, -2.0, 0.0))
await asyncio.sleep(4)
print("-- Go North 2 m/s, turn to face East")
await drone.offboard.set_velocity_ned(VelocityNedYaw(2.0, 0.0, 0.0, 90.0))
await asyncio.sleep(4)
print("-- Go South 2 m/s, turn to face West")
await drone.offboard.set_velocity_ned(VelocityNedYaw(
-2.0, 0.0, 0.0, 270.0))
await asyncio.sleep(4)
print("-- Go West 2 m/s, turn to face East")
await drone.offboard.set_velocity_ned(VelocityNedYaw(0.0, -2.0, 0.0, 90.0))
await asyncio.sleep(4)
print("-- Go East 2 m/s")
await drone.offboard.set_velocity_ned(VelocityNedYaw(0.0, 2.0, 0.0, 90.0))
await asyncio.sleep(4)
print("-- Turn to face South")
await drone.offboard.set_velocity_ned(VelocityNedYaw(0.0, 0.0, 0.0, 180.0))
await asyncio.sleep(2)
print("-- Go down 1 m/s, turn to face North")
await drone.offboard.set_velocity_ned(VelocityNedYaw(0.0, 0.0, 1.0, 0.0))
await asyncio.sleep(4)
print("-- Stopping offboard")
try:
await drone.offboard.stop()
except OffboardError as error:
print(f"Stopping offboard mode failed with error code: \
{error._result.result}")