async def run():
""" Does Offboard control using position NED coordinates. """
drone = System()
await drone.connect(system_address="udp://:14540")
print("-- Arming")
#await drone.action.arm()
print("-- Setting initial setpoint")
await drone.offboard.set_position_ned(PositionNedYaw(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
test_position = 50.0
# Set the goal in PositionNedYaw form
position_delta_goal_yaw = PositionNedYaw(test_position, test_position,
-50.0, 0.0)
await set_position_ned_local(position_delta_goal_yaw, drone)
for i in range(1, 60):
await asyncio.sleep(0.5)
print(f"-- Working on Fake task {i}")
#
# Test 2: what is set_position_ned in reference to?
#
test_position = 0
# Set the goal in PositionNedYaw form
position_goal_yaw = PositionNedYaw(test_position, test_position, -50.0,
0.0)
# Convert to PositionNed
position_goal = PositionNedYaw_to_PositionNed(position_goal_yaw)
print("-- Go 0m North, 0m East, -0m Down within local coordinate system")
await drone.offboard.set_position_ned(position_goal_yaw)
#print("-- Landing Drone")
#await drone.action.return_to_launch()
await asyncio.sleep(30)
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])
print("-- Starting offboard")
try:
await drone.conn.offboard.start()
except OffboardError as error:
print(
f"Starting offboard mode failed with error code: {error._result.result}"
)
print("-- Disarming")
await drone.conn.action.disarm()
return
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(*self.target, 0.0))
async for position_ned in drone.conn.telemetry.position_velocity_ned():
position = np.array([
position_ned.position.north_m, position_ned.position.east_m,
position_ned.position.down_m
])
if distance_between(self.target, position) < self.tolerance:
print(
f"-- Arrived @ {self.target[0]}m North, {self.target[1]}m East, {self.target[2]}m Down within local coordinate system"
)
break
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 set_position_ned_local(position_delta_yaw, drone):
"""
This function allows for setting of location with reference to the current position
"""
async for position in drone.telemetry.position_velocity_ned():
current_position = position.position
break
#Create composite position attributes based on current position + delta
new_north_m = current_position.north_m + position_delta_yaw.north_m
new_east_m = current_position.east_m + position_delta_yaw.east_m
new_down_m = current_position.down_m + position_delta_yaw.down_m
#Create PositionNedYaw object with new composite position
position_goal_yaw = PositionNedYaw(new_north_m, new_east_m, new_down_m, 0)
#setting the position set point
print(
f"-- Go {position_delta_yaw.north_m} North, {position_delta_yaw.east_m} East, {position_delta_yaw.down_m} Down"
)
await drone.offboard.set_position_ned(position_goal_yaw)
# Convert to PositionNed
position_goal = PositionNedYaw_to_PositionNed(position_goal_yaw)
print("-- Creating Task: Checking if complete")
asyncio.create_task(coop_is_converged(position_goal, drone))
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 __call__(self, drone):
await drone.arm(coordinate=[0.0, 0.0, 0.0], attitude=[0.0, 0.0, 0.0])
await drone.start_offboard()
async for position_ned in self.leaderdrone.conn.telemetry.position_velocity_ned(
):
position = np.array([
position_ned.position.north_m, position_ned.position.east_m
]) #,position_ned.position.down_m])
follow_point = np.array(
[position[0] - self.radius, start_position[1]])
circle_otherside = np.array(
[start_position[0] + 2 * self.radius, start_position[1]])
await drone.conn.offboard.set_position_ned(
PositionNedYaw(*follow_point, 0.0))
async def set_pos_yaw(self, pos, yaw, time):
"""
Offboard method that sends position and yaw references to the PX4 autopilot of the the drone.
Parameters
----------
pos : np.array of floats with shape (3,1)
Desired position for the drone, in local NED coordinates.
yaw : float
Desired yaw for the vehicle, in radians.
time: float
Time, in seconds, during which the selected position and yaw references will be sent to the PX4 autopilot.
"""
t = 0
while (t < time):
msg = PositionNedYaw(pos[0][0], pos[1][0], pos[2][0],
math.degrees(yaw))
await self.system.offboard.set_position_ned(msg)
await asyncio.sleep(1 / 50)
t += 1 / 50
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 arm(self,
coordinate: List[float] = None,
attitude: List[float] = None):
async for arm in self.conn.telemetry.armed():
if arm is False:
try:
print(f"{self.name}: arming")
await self.conn.action.arm()
print(f"{self.name}: Setting initial setpoint")
if coordinate is not None:
await self.conn.offboard.set_position_ned(
PositionNedYaw(*coordinate, 0.0))
if attitude is not None:
await self.conn.offboard.set_attitude(
Attitude(*attitude, 0.0))
except Exception as bla: # add exception later
print(bla)
break
else:
break
async def run():
# Init the drone
drone = System()
print("Waiting for drone to connect...")
await drone.connect(system_address="serial:///dev/serial0:921600")
print("Waiting for drone to have a global position estimate...")
async for health in drone.telemetry.health():
if health.is_global_position_ok:
print("Global position estimate ok")
break
print("-- Arming")
await drone.action.arm()
print("-- Setting initial setpoint")
await drone.offboard.set_position_ned(PositionNedYaw(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 0m North, 0m East, 10m Up within local coordinate system")
await drone.offboard.set_position_ned(PositionNedYaw(0.0, 0.0, -10.0, 0.0))
await asyncio.sleep(15)
print(
"-- Go 0m North, 0m East, stay at 10m Up within local coordinate system, rotate 360 dgrees slowly"
)
for i in range(0, 360, 3):
await drone.offboard.set_position_ned(
PositionNedYaw(0.0, 0.0, -10.0, i))
samples = sdr.read_samples(num_samples)
Pxx, freqs = mlab.psd(samples, Fs=sdr.sample_rate)
async for position in drone.telemetry.position():
latitude = position.latitude_deg
longitude = position.longitude_deg
break
latitude_a = np.full(len(freqs), latitude)
longitude_a = np.full(len(freqs), longitude)
freqs += sdr.center_freq
amplitude_dB = 10 * np.log10(np.abs(Pxx))
i_a = np.full(len(freqs), i)
original_stdout = sys.stdout
with open('filename.txt', 'a') as f:
sys.stdout = f
res = "\n".join(
"{} {} {} {} {}".format(x, y, z, w, i)
for x, y, z, w, i in zip(freqs, amplitude_dB, latitude_a,
longitude_a, i_a))
print(res)
sys.stdout = original_stdout
if i == 360:
break
await asyncio.sleep(5)
print("-- Stopping offboard")
try:
await drone.offboard.stop()
except OffboardError as error:
print(
f"Stopping offboard mode failed with error code: {error._result.result}"
)
async def run():
drone = System()
await drone.connect(system_address="serial:///dev/ttyUSB0")
print("Waiting for drone to connect...")
async for state in drone.core.connection_state():
if state.is_connected:
print(f"Drone discovered with UUID: {state.uuid}")
break
print_mission_progress_task = asyncio.ensure_future(
print_mission_progress(drone))
running_tasks = [print_mission_progress_task]
termination_task = asyncio.ensure_future(
observe_is_in_air(drone, running_tasks))
mission_items = []
#parameters = (latitude_deg, longitude_deg, relative_altitude_m, speed_m_s, is_flying_through
# gimbal_pitch, gimbal_yaw, camera_action, loiter_time_s, camera_photo_interval_s)
# Waypoint1
mission_items.append(
MissionItem(37.4539753, 126.9518641, 1, 2, True, float('nan'),
float('nan'), MissionItem.CameraAction.NONE, float('nan'),
float('nan')))
# Waypoint2
mission_items.append(
MissionItem(37.4540048, 126.9518121, 1, 2, True, float('nan'),
float('nan'), MissionItem.CameraAction.NONE, float('nan'),
float('nan')))
# Waypoint3
mission_items.append(
MissionItem(37.454065, 126.9518092, 1, 2, True, float('nan'),
float('nan'), MissionItem.CameraAction.NONE, float('nan'),
float('nan')))
mission_plan = MissionPlan(mission_items)
await drone.mission.set_return_to_launch_after_mission(False)
print("-- Uploading mission")
await drone.mission.upload_mission(mission_plan)
await asyncio.sleep(1)
print("-- Arming")
await drone.action.arm()
##########################################
### Departing a Truck & Mission Starts ###
##########################################
print("-- Starting mission")
await drone.mission.start_mission()
await termination_task
##########################################
### Mission Ends & Go back to a Truck ###
##########################################
while True:
# if the target is not within the camera yet
if not target_coord.is_detected:
await drone.action.goto_location(target_gps.latitude,
target_gps.longitude, flying_alt,
0)
# if the target is detected within the camera, chanage to offboard mode to finetune the drone's position
else:
if not drone.offboard.is_active:
print("-- Setting initial setpoint")
await drone.offboard.set_position_ned(
PositionNedYaw(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
else:
print(
f"-- Go {target_coord.del_north}m North, {target_coord.del_east}m East, within local coordinate system"
)
await drone.offboard.set_position_ned(
PositionNedYaw(target_coord.del_north,
target_coord.del_east, 0.0, 0.0))
async def run():
drone = System()
await drone.connect(system_address="udp://:14540")
print("-- 시동")
await drone.action.arm()
await drone.offboard.set_position_ned(PositionNedYaw(0.0, 0.0, 0.0, 0.0))
print("-- offboard control 시작")
try:
await drone.offboard.start()
except OffboardError as error:
print(
"Starting offboard mode failed with error code: {error._result.result}"
)
print("-- Disarming")
await drone.action.disarm()
return
print("-- 고도 10m 상승")
await drone.offboard.set_position_ned(PositionNedYaw(0.0, 0.0, -10.0, 0.0))
await asyncio.sleep(10)
print("-- 10m 전진")
await drone.offboard.set_position_ned(PositionNedYaw(
10.0, 0.0, -10.0, 0.0))
await asyncio.sleep(3)
print("-- 90도 우측 회전 후 전진")
await drone.offboard.set_position_ned(
PositionNedYaw(10.0, 0.0, -10.0, 90.0))
await asyncio.sleep(1)
await drone.offboard.set_position_ned(
PositionNedYaw(10.0, 10.0, -10.0, 90.0))
await asyncio.sleep(3)
print("-- 배송지 도착 : 착륙")
await drone.offboard.set_position_ned(PositionNedYaw(
10.0, 10.0, 0.0, 90.0))
await asyncio.sleep(15)
print("-- 배송 완료 : 이륙")
await drone.offboard.set_position_ned(
PositionNedYaw(10.0, 10.0, -10.0, 90.0))
await asyncio.sleep(10)
print("-- 90도 우측 회전 후 전진")
await drone.offboard.set_position_ned(
PositionNedYaw(10.0, 10.0, -10.0, 180.0))
await asyncio.sleep(1)
await drone.offboard.set_position_ned(
PositionNedYaw(0.0, 10.0, -10.0, 180.0))
await asyncio.sleep(3)
print("-- offboard 종료 : RTL")
try:
await drone.offboard.stop()
except OffboardError as error:
print(
f"Stopping offboard mode failed with error code: {error._result.result}"
)
def PositionNed_to_PositionNedYaw(PositionNed):
return PositionNedYaw(PositionNed.north_m, PositionNed.east_m,
PositionNed.down_m, 0.0)