Obstacle Detection/Avoidance

[MinhxNguyen7]

Description

We will develop and design logic for obstacle avoidance to be integrated with the current code base in GNC's commander node. Specifically within the commander node's execute waypoints function in the while loop on line 197. This loop is true while the mission isnt complete. We can use it to iteratively check if avoidance is nessecary.

uavf_2024/uavf_2024/gnc/commander_node.py

Lines 135 to 198 in 9140735

	def execute_waypoints(self, waypoints, yaws = None, altitude = 0.0):
	if yaws is None:
	yaws = [float('NaN')] * len(waypoints)
	self.last_wp_seq = -1
	self.log("Pushing waypoints")
	waypoints = [(self.last_global_pos.latitude, self.last_global_pos.longitude, TAKEOFF_ALTITUDE)] + waypoints
	yaws = [float('NaN')] + yaws
	self.log(f"Waypoints: {waypoints} Yaws: {yaws}")
	waypoint_msgs = [
	mavros_msgs.msg.Waypoint(
	frame = mavros_msgs.msg.Waypoint.FRAME_GLOBAL_REL_ALT,
	command = mavros_msgs.msg.CommandCode.NAV_WAYPOINT,
	is_current = False,
	autocontinue = True,
	param1 = 0.0,
	param2 = 5.0,
	param3 = 0.0,
	param4 = yaw,
	x_lat = wp[0],
	y_long = wp[1],
	z_alt = wp[2])
	for wp,yaw in zip(waypoints, yaws)]
	self.clear_mission_client.call(mavros_msgs.srv.WaypointClear.Request())
	self.log("Delaying before pushing waypoints.")
	time.sleep(1)
	self.log("Pushing waypoints.")
	self.waypoints_client.call(mavros_msgs.srv.WaypointPush.Request(start_index = 0, waypoints = waypoint_msgs))
	self.log("Delaying before setting mode.")
	time.sleep(1)
	self.log("Setting mode.")
	# mavros/px4 doesn't consistently set the mode the first time this function is called...
	# retry or fail the script.
	for _ in range(1000):
	self.mode_client.call(mavros_msgs.srv.SetMode.Request( \
	base_mode = 0,
	custom_mode = 'AUTO.MISSION'
	))
	time.sleep(0.2)
	if self.cur_state != None and self.cur_state.mode == 'AUTO.MISSION':
	self.log("Success setting mode")
	break
	else:
	self.log("Failure setting mode, quitting.")
	quit()
	self.log("Waiting for mission to finish.")
	while self.last_wp_seq != len(waypoints)-1:
	pass

Design

Integration

The approach taken will be to identify whether or not avoidance is needed via a vision system and drone ID. This will be quantified in seconds till collision. We will calculate this metric based on estimated positions of detected targets by using a Kalman Filter to forward future object state predictions. As of now the Unscented Kalman Filter is the decided approach from @EricPedley.

We can return s = -1 if there are no targets in-sight but for all non negative values of s we can decide a threshold to trigger obstacle avoidance. In general terms, if we are less than s seconds from collision trigger avoidance logic.

The obstacle avoidance will be implemented in a custom flight mode that the commander node can switch into (MISSION ---> AVOIDANCE ---> MISSION) inbetween the mission. More information about that interface is documented here

Important caveats about the obstacle avoidance approach

When the vehicle switches to avoidance mode there will be a manuver calculated to avoid the object based on the state of our vehicle when we initalized avoidance mode and the predictions. The calculated manuver trajectory is fixed and does not update based on new information, we are going under the assumption we have decent forward predictions of the object's movement.

Avoidance mode sends a series of waypoints and is "Completed" once it reaches the final waypoint. Our navigation logic will wait for avoidance mode to complete before continuing the execute waypoint function.

the calculated path will not dynamically update every second, its calcualted only once per avoidance mode trigger @EricPedley

Architecture

Development

waiting for us to finalize this methodology before we assign specific tasks, open to modifications / improvements
@EricPedley @MinhxNguyen7

Tasks

Localize other drones via DRONEID and VISION in ENU frame

• Monocular RGB obstacle detection #122
• Use Kalman Filter to fuse DroneID and Vision information to object's 3D state in ENU frame #173

Prediction

• Use Kalman Filter to forward object positions from 0-N number of seconds into the future
• Characterize an "Avoidance Zone" as a cone (or any convex shape) starting from object state at time 0 till time N. (the cone will be larger further into the future to model our "uncertainty" in predictions. We can tune this a predefined value.

Risk assessment

• Given we assume our trajectory is just a line from us to the next waypoint, check if it intersects with the avoidance region. Calculate time in S till the intersection.
• Determine some value N, where if collision time S <= N then trigger avoidance

sketch of intersection scenerio

Obstacle Avoidance

https://gitlab.com/vince-cs/graph-of-convex-sets-ros2-quadrotor-avoidance/-/tree/main/AvoidanceMode?ref_type=heads

trajectory planning solver used

Issues

• Related to Monocular RGB obstacle detection #122
• Related to Pose estimation for other drones based on bounding box detections #127

[MinhxNguyen7]

@EricPedley @Vince-C156 Can you guys add what we need to do and, optionally, who's doing it, so we all get an idea of what lays ahead?

[MinhxNguyen7]

Predictor gives a list of 3-tuples of matrix A, vector b, and time t, where the constraint is Ax < b, where x is our drone's equation, and t is the minimum time to intersecting the no-go zone.

Fly up if time is less than a certain threshold, we try to fly straight up. Otherwise, attempt our collision avoidance algorithm.

@Vince-C156 @EricPedley

[Vince-C156]

good stuff, working on a continuous time obstacle avoidance trajectory planner so the solver doesn't phase through the constraint