UAV Setup Guides (Path Planning & Navigation) - qutas/info GitHub Wiki

Interfacing with Contrail

Contrail is a set of high-level guidance libraries, allowing for a planned path to be autonomously followed.

Minimal Point-to-Point Example

A minimal example of using the contrail action client in python is shown below:

#!/usr/bin/env python2

import roslib
roslib.load_manifest('contrail_manager')
import rospy
import actionlib
from actionlib_msgs.msg import GoalStatus

from contrail_manager.msg import TrajectoryAction, TrajectoryGoal
from geometry_msgs.msg import Vector3

if __name__ == '__main__':
	# Setup ROS and wait for contrail to connect
	rospy.init_node('load_waypoints', anonymous=True)

	rospy.loginfo("Waiting for contrail to connect...")
	client_base = actionlib.SimpleActionClient("/emulated_uav/mavel/contrail", TrajectoryAction)
	client_base.wait_for_server()

	# Build new goal message
	# https://github.com/qutas/contrail/blob/master/contrail_manager/action/Trajectory.action
	goal_base = TrajectoryGoal()

	# Start point
	goal_base.positions.append(Vector3(x=0.0,y=0.0,z=1.0))
	goal_base.yaws.append(0.0)
	# End point
	goal_base.positions.append(Vector3(x=1.0,y=0.0,z=1.0))
	goal_base.yaws.append(0.0)

	goal_base.duration = rospy.Duration.from_sec(10)

	# Set a start time to be "start imidiately"
	goal_base.start = rospy.Time(0)

	# Transmit the goal to contrail
	client_base.send_goal(goal_base)

Point-to-Point Navigation

A more in-depth example of implementing a waypoint navigator using contrail can be found here.

Asynchronous Point-to-Point Navigator

This next example expands on the Point-to-Point Navigation example to show an implementation that does not rely on the "wait_for_result()" function of contrail. This is necessary for projects where the navigator has multiple tasks to juggle (such as starting and stopping the mission dynamically). This example can be found here.

Starting and Stopping Planner

The final example is presented as a proper package, with a Python node that generates a set flight path, and handles an interruption to pause the flight for 5 seconds before continuing. See the Contrail interfacing example for more information.

Interfacing with Breadcrumb

Breadcrumb handles some of the background work required to perform navigation in a cluttered environment by offering a ROS service that will calculate an A* flight plan.

A minimum example is shown below. Before continuing, make sure you have downloaded breadcrumb, and are running it successfully (you will have to also start the uavusr_emulator, or something similar, to provide an occupancy grid for breadcrumb to use).

#!/usr/bin/env python2

import rospy

from breadcrumb.srv import RequestPath
from breadcrumb.srv import RequestPathRequest

if __name__ == '__main__':
	# Setup ROS and wait for contrail to connect
	rospy.init_node('path_planner', anonymous=True)

	# Wait to connect with Breadcrumb
	# Code will error if you try to connect to a service
	# that does not exist
	rospy.wait_for_service('/breadcrumb/request_path')
	srvc_bc = rospy.ServiceProxy('/breadcrumb/request_path', RequestPath)

	# Set up a path request for breadcrumb
	req = RequestPathRequest()
	req.start.x = -1.5
	req.start.y = -1.5
	req.start.z = 1.0
	req.end.x = 1.5
	req.end.y = 1.5
	req.end.z = 1.0

	res = srvc_bc(req)

	# Breadcrumb will return a vector of poses if a solution was found
	# If no solution was found (i.e. no solution, or request bad
	# start/end), then breadcrumb returns and empty vector
	if len(res.path.poses) > 0:
		# Loop through the solution returned from breadcrumb
		for i in xrange(len(res.path.poses) - 1):
			rospy.loginfo("    [%0.2f;%0.2f;%0.2f]",
						  res.path.poses[i].position.x,
						  res.path.poses[i].position.y,
						  res.path.poses[i].position.z)
	else:
		rospy.logerr("solution not found")

A more in-depth example (for multiple waypoints) is as follows:

#!/usr/bin/env python2

import rospy

from breadcrumb.srv import RequestPath
from breadcrumb.srv import RequestPathRequest

if __name__ == '__main__':
	# Setup ROS and wait for contrail to connect
	rospy.init_node('path_planner', anonymous=True)

	# List of waypoints (XYZ-Yaw)
	waypoints = [[-1.0,-1.0,0.0,0.0],
				 [ 1.5,-1.0,0.0,0.0],
				 [ 1.5, 1.5,0.0,0.0]]

	# Wait to connect with Breadcrumb
	# Code will error if you try to connect to a service
	# that does not exist
	rospy.wait_for_service('/breadcrumb/request_path')
	srvc_bc = rospy.ServiceProxy('/breadcrumb/request_path', RequestPath)

	# Loop through the list of waypoints
	for i in xrange(len(waypoints)-1):
		# Set up a path request for breadcrumb
		req = RequestPathRequest()
		req.start.x = waypoints[i][0]
		req.start.y = waypoints[i][1]
		req.start.z = waypoints[i][2]
		req.end.x = waypoints[i+1][0]
		req.end.y = waypoints[i+1][1]
		req.end.z = waypoints[i+1][2]

		res = srvc_bc(req)

		# Breadcrumb will return a vector of poses if a solution was found
		# If no solution was found (i.e. no solution, or request bad
		# start/end), then breadcrumb returns and empty vector
		# XXX: You could also use res.path_sparse (see breadcrumb docs)
		if len(res.path.poses) > 0:
			# Print the path to the screen
			rospy.loginfo("Segment %i:", i+1)
			rospy.loginfo("[%0.2f;%0.2f;%0.2f] => [%0.2f;%0.2f;%0.2f]",
						  req.start.x,req.start.y,req.start.z,
						  req.end.x,req.end.y,req.end.z)

			# Loop through the solution returned from breadcrumb
			for i in xrange(len(res.path.poses) - 1):
				rospy.loginfo("    [%0.2f;%0.2f;%0.2f]",
							  res.path.poses[i].position.x,
							  res.path.poses[i].position.y,
							  res.path.poses[i].position.z)
		else:
			rospy.logerr("solution not found")

See this package for a more in-depth example of interfacing breadcrumb with contrail to generate a safe flight plan.

Using TF2 for Frame transformations:

This guide explains how to use the concepts of the transformation library TF2. The primary resources used are the TF Broadcaster, and the TF Listener.

First of, download the following 3 scripts to your computer:

Next, make you've started roscore and launch the UAVUSR emulator as usual.

Run the Frame Transformations script. You will get a message saying "tf2_broadcaster_frames running.". The purpose of this script is two-fold, firstly, it provides the TF system with a source transformation to use, namely the transformation from the world to the UAV, named "map" and "uav" respectively. The second purpose is to output a static transformation that represents the transform from the UAV to the camera, named "uav" and "camera" respectively. With this data, a transformation chain is set up, that is, we can determine the transform between any of the 3 previously defined frames. We can use TF to provide us with the location of, say, where the camera is specifically relative to the map.

At this point, it may be good to open rviz, then add in the TF plugin. You should see all 3 frames displayed in the environment, looking something like:

rviz_tf_example

Next, run the Target Localization script. This script will do noting immediately, as it is simply waiting for a timestamp to be sent. It subscribes to the topic /emulated_uav/target_found, and is waiting for a timestamp that represents a time when a valid target transformation has been sent to TF.

Finally, run the Target Broadcaster. This script does the following:

  1. Sets up the required systems, then runs send_tf_target().
  2. A timestamp that represents when a target has been found. This is simulated by grabbing the current time.
  3. Estimates the target's location in the camera frame (target position relative to the camera). This is simulated by putting random values into the transformation.
  4. The transform is sent to TF.
  5. The timestamp is published to /emulated_uav/target_found.

This is when the magic happens! Going backwards down the chain now:

  1. The timestamp is received by the Target Localization script.
  2. Target Localization runs the function t = tfBuffer.lookup_transform("map", "target", msg_in.data, rospy.Duration(0.5)) which does the following:
    1. From the current TF information (tfBuffer) ...
    2. ... lookup the transform (`lookup_transform) ...
    3. ... from the map ("map") ...
    4. ... to the target ("target") ...
    5. ... at a specific time (msg_in.data) ...
    6. ... and allow this function to wait for up to 0.5 seconds for any data required to run this calculation (rospy.Duration(0.5)).
    7. The output of this is t, which is data in the form of geometry_msgs/TransformStamped, which represents the current pose of the "target" in the "map" frame.
  3. From here, Target Localization simply extracts the [XYZ] location of the target, and dumps it to the screen (but this could be further used to command the UAV to fly above the target).
  4. Lastly, rviz should briefly display the "target" transformation on the screen, although it will vanish as it was a once of measurement, which essentially can't be trusted to be correct after some amount of time.