Week 4. Navigation Stack

We can launch a stage demo using:

roslaunch rosplan_stage_demo empty_stage_single_robot.launch

Using rviz

You can now launch rviz configured for the demo with:

rosrun rviz rviz -d $(rospack find rosplan_stage_demo)/config/rosplan_stage_demo.rviz

Pose Estimation

You can give the robot hints on where it is in the world using “2D Pose Estimate” tool.

You can tell the robot where it should be looking.

Navigation Goal

Set navigation goal using the “2D Nav Goal” tool.

The robot will try to navigate around the map now.

Navigation Stack Components

One important node is move_base which, given a goal in the world, will attempt to reach it.

A cost map is an image which provides data around the world, black pixels represent obstacles and white pixels represent free space. We can also call this an occupancy grid.

The global planner just generates the shortest path through a map. The local planner takes this information and also takes into consideration the local map (“what the robot can see”) and the robot’s capabilities (“acceleration”).

Recovery Behaviour

In a situation where an obstacle appears, is remembered and then the robot discovers another obstacle:

The robot will run through a sequence of steps similar to:

• Do a clearing rotation where the robot clears cost maps in some area outside of its immediate radius (such as 3m).
• Aggressively reset and keep clearing cost maps around a smaller radius, i.e. 2m, 1m.
• If it still cannot do a clearing rotation, then the navigation is aborted.

Sending Goals as Messages

We can send a goal through the /move_base_simple/goal topic which tells the global planner what we want to do in the world. This is the target Pose.

Custom Messages Types

Messages are defined as *.msg files within ROS packages in a msg folder. Each type file looks like this:

fieldtype1 fieldname1
fieldtype2 fieldname2
fieldtype3 fieldname3

# example:
int32 x
int32 y

When creating a new message, follow instructions in CMakeLists.txt for building.

Quaternions

We have two ways of representing a rotation of an object, using: axis-angle: $\left[\begin{array}{c}\vec{v},\alpha \end{array}\right]$ quaternion: $\left[\begin{array}{c}\sin \frac{\alpha }{2}\cdot \vec{v},\cos \frac{\alpha }{2}\end{array}\right]=\left[\begin{array}{c}\sin \frac{\alpha }{2}\cdot v_x,\sin \frac{\alpha }{2}\cdot v_y,\sin \frac{\alpha }{2}\cdot v_z,\cos \frac{\alpha }{2}\end{array}\right]$

Example: Computing for ROS

Lets say we want to turn 90 deg to the left:

When we represent this in 3D space, we know that we are rotating around the z-axis:

So our rotation vector is $v=\left[\begin{array}{ccc}0& 0& 1\end{array}\right]$ and our angle is $\alpha =\frac{\pi }{2}$, hence:

$$\begin{array}{rl}q& =\left[\begin{array}{cccc}\sin \frac{\pi }{4}\cdot 0& \sin \frac{\pi }{4}\cdot 0& \sin \frac{\pi }{4}\cdot 1& \cos \frac{\pi }{4}\end{array}\right]\\ & =\left[\begin{array}{cccc}0& 0& 0.71& 0.71\end{array}\right]\end{array}$$

Lets say that we want to continue turning further by another 45 deg, so our target rotation:

$$\begin{array}{rl}v& =\left[\begin{array}{ccc}0& 0& 1\end{array}\right]\\ \alpha & =\frac{\pi }{2}+\frac{\pi }{4}\\ & =\frac{3}{4}\pi \\ & \\ q& =\left[\begin{array}{cccc}0& 0& \sin \frac{3}{8}\pi & \cos \frac{3}{8}\pi \end{array}\right]\\ & =\left[\begin{array}{cccc}0& 0& 0.92& 0.38\end{array}\right]\end{array}$$

Example 2: Compute orientation of -30 deg around z-axis.

Begin by converting to radians: $-30^{\circ }=-\frac{1}{6}\pi \text{ rad}$. Hence, $v=\left[\begin{array}{ccc}0& 0& 1\end{array}\right],\alpha =-\frac{1}{6}\pi$. Calculating the quaternion:

$$\begin{array}{rl}q& =\left[\begin{array}{cccc}0& 0& \sin \left(\begin{array}{c}-\frac{1}{12}\pi \end{array}\right)& \cos \left(\begin{array}{c}-\frac{1}{12}\pi \end{array}\right)\end{array}\right]\\ & =\left[\begin{array}{cccc}0& 0& -0.26& 0.97\end{array}\right]\end{array}$$

Example 3: Compute orientation of 270 deg around z-axis.

Begin by converting to radians: $270^{\circ }=\frac{3}{2}\pi \text{ rad}$. Hence, $v=\left[\begin{array}{ccc}0& 0& 1\end{array}\right],\alpha =\frac{3}{2}\pi$. Calculating the quaternion:

$$\begin{array}{rl}q& =\left[\begin{array}{cccc}0& 0& \sin \left(\begin{array}{c}\frac{3}{4}\pi \end{array}\right)& \cos \left(\begin{array}{c}\frac{3}{4}\pi \end{array}\right)\end{array}\right]\\ & =\left[\begin{array}{cccc}0& 0& 0.71& -0.71\end{array}\right]\end{array}$$

Action Lib

Action lib provides a client-server interface / action protocol built on top of ROS messages. It provides a simple API for users to request goals (on the client) or to execute goals (on the server) via function calls and callbacks.

Much like messages, we define actions in a *.action files in the action directory. Here is an example of one such action:

# define the goal
uint32 dishwasher_id
---
# define the result
uint32 total_dishes_cleaned
---
# define a feedback message
float32 percent_complete

Based on this, 6 messages are automatically generated by catkin.

Documentation

Building with catkin: http://wiki.ros.org/actionlib#Build_a_package_by_Catkin Action client: http://wiki.ros.org/actionlib#Using_the_ActionClient Action server: http://wiki.ros.org/actionlib#Implementing_an_ActionServer