Map-Based Navigation¶

Background¶

The objective of this tutorial is to use a map of interest and request the robot to go to certain locations on that map. In the previous tutorial Building a Map with a RIA-E100, you already learned how to build a map with the RIA-E100 robot. You will learn how to use such a map to program navigation missions for the robot. Remember that a map in ROS consists of two files, namely a .yaml file that contains information about the map, and a .pgm file that contains the map itself. In this tutorial, we are going to consider the following University floor map to develop our navigation example

Figure 1. University Floor Map

Later on, as an exercise, you will use your own map to develop a navigation mission for your RIA-E100.

In the map of Figure 1, we identify four locations of interests namely, the Cafe area, Office1, Office2 and Office3. Other locations can also be defined, but we just used these representative four locations for illustration purposes. If we want to develop a ROS program that allow the robot to navigate to those locations, we first need to know what are the (x,y) coordinate of these locations onto the map. This is what will be explained later. Then, we use these coordinates to define a navigation mission that we submit to the robot’s navigation stack to execute it. Remember that any robot on ROS runs the move_base navigation stack which allow the robot to find a path towards a goal and execute the path following while avoiding obstacles. Refer to SLAM demo in ria-r100-demo-apps: for a refresher.

Tutorial Files¶

You can find the whole cpp and python files in our GitHub repository. They are located in /src/RIA-E100/navigation/ folder.

We will aslo use the launch file navigation.launch from the e100-navigation pachage that contains all the needed ROS nodes for this tutorial. Here is the content of the navigation.launch file.

<?xml version="1.0"?>
<launch>

<!-- Run Map Server -->
<node name="map_server" pkg="map_server" type="map_server" args="$(find e100_navigation)/maps/psu_map.yaml">
 <param name="frame_id" value="map"/>
</node>

<!-- Run AMCL -->

<!--if EKF is used Gmapping will use odom as its odom-frame , else use odom_laser-->
<arg name="efk" default="$(env USE_EKF)"/>

<group if="$(eval arg('efk')==true)">
 <include file="$(find e100_navigation)/launch/include/amcl.launch"/>
</group>

<group if="$(eval arg('efk')==false)">
 <include file="$(find e100_navigation)/launch/include/amcl_laser.launch.xml"/>
</group>

<!-- Run Move Base -->
<include file="$(find e100_navigation)/launch/move_base.launch"/>

</launch>

We observe that there are three nodes:

• The first one runs the map_server node, which manages the maps loading and saving, it also loads the map specified in the path “psu_map” you can change it to any map you like.
• The second node: will run one of two nodes amcl or amcl_laser depending on the environment you will run the robot in. These nodes give you the current coordinates of the robot in the map
• The last launch file is move_base.launch which gives you the ability to send the robot to an (x,y) coordinates.

Finding the Coordinates for Locations of Interest¶

The first thing you need to do is to find the (x,y) coordinates of the locations where you want the robot to go. These locations depends on the map size and resolutions. The easiest way to do so is to use rviz to visualize the map and then getting the coordinate using the 2D Pose Estimate button. Let us start the RVIZ simulator with the University floor map as follow:

roslaunch e100_bringup minimal.launch

In new terminal run

roslaunch e100_navigation navigation.launch

In new third terminal run

roslaunch e100_description view.launch

The rviz simulator will open. you will see your robot in the map and you can change the default pos from the 2D Pose Estimate button.

Figure 2: RIA-E100 Simulation Environment

Now, you want to find the coordinate of any location of interest. open another terminal and write

rostopic echo /amcl_pose

This command will display any new /amcl_pose, which is the ROS topic represents the location of the robot on the map. Now, click on 2D Pose Estimate button, then click on any location of interest on the map. Go back to the terminal where wrote rostopic echo /amcl_pose, and you will find the coordinate of the point select as in the following figure:

Figure 3: Finding the Location of Interest

Take note of the (x,y) coordinates for the Cafe and the three offices. They should be close to these values:

Cafe (15.50, 10.20)
Office1 (27.70,12.50)
Office2 (30.44,12.50)
Office3 (35.20,12.50)

Writing the navigation program¶

Now, we are ready to write a program that allows the robot to navigate to the goal location. Here is the snapshot of the code in C++:

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49

#include <ros/ros.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>


/** function declarations **/
bool moveToGoal(double xGoal, double yGoal);
char choose();

/** declare the coordinates of interest **/
double xCafe = 15.50;
double yCafe = 10.20;
double xOffice1 = 27.70 ;
double yOffice1 = 12.50;
double xOffice2 = 30.44 ;
double yOffice2 = 12.50;
double xOffice3 = 35.20 ;
double yOffice3 = 13.50;

bool goalReached = false;
 int main(int argc, char** argv){
   ros::init(argc, argv, "map_navigation_node");
   ros::NodeHandle n;
   ros::spinOnce();

   char choice = 'q';
   do{
      choice =choose();
      if (choice == '0'){
         goalReached = moveToGoal(xCafe, yCafe);
      }else if (choice == '1'){
         goalReached = moveToGoal(xOffice1, yOffice1);
      }else if (choice == '2'){
         goalReached = moveToGoal(xOffice2, yOffice2);
      }else if (choice == '3'){
         goalReached = moveToGoal(xOffice3, yOffice3);
      }
      if (choice!='q'){
         if (goalReached){
            ROS_INFO("Congratulations!");
            ros::spinOnce();

         }else{
            ROS_INFO("Hard Luck!");
         }
      }
   }while(choice !='q');
   return 0;
}

The program structure is very simple. The user is requested to press a key, and based on his selection, the defined method moveToGoal(x,y) will be executed, where x and y define the coordinates of the locations of interest. Lines 11-18 define the location of points of interest that we determined in the last section Finding the Coordinates for Locations of Interest. Then, from lines 38-46, if the goal is successully reached, the program will display congratulations text message. Otherwise, it will display hard luck message.

Now, let us focus more on how the method moveToGoal(x,y) is implemented. Here is the code.

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41

bool moveToGoal(double xGoal, double yGoal){

   //define a client for to send goal requests to the move_base server through a SimpleActionClient
   actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> ac("move_base", true);

   //wait for the action server to come up
   while(!ac.waitForServer(ros::Duration(5.0))){
      ROS_INFO("Waiting for the move_base action server to come up");
   }

   move_base_msgs::MoveBaseGoal goal;

   //set up the frame parameters
   goal.target_pose.header.frame_id = "map";
   goal.target_pose.header.stamp = ros::Time::now();

   /* moving towards the goal*/

   goal.target_pose.pose.position.x =  xGoal;
   goal.target_pose.pose.position.y =  yGoal;
   goal.target_pose.pose.position.z =  0.0;
   goal.target_pose.pose.orientation.x = 0.0;
   goal.target_pose.pose.orientation.y = 0.0;
   goal.target_pose.pose.orientation.z = 0.0;
   goal.target_pose.pose.orientation.w = 1.0;

   ROS_INFO("Sending goal location ...");
   ac.sendGoal(goal);

   ac.waitForResult();

   if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED){
      ROS_INFO("You have reached the destination");
      return true;
   }
   else{
      ROS_INFO("The robot failed to reach the destination");
      return false;
   }

}

Line 4 defines a client that is responsible for sending navigation goal request to the move_base server. In fact, the navigation stack of ROS has a MoveBaseAction action server that receives navigation goals request, and then finds a global path from the robot location to the goal location through the Global Path Planner, and once a path is found, it executes the path while avoiding obstacle through the Local Path Planner until it reaches the destination or fails to do so for any reason (like unexpected obstacle found on path).

Lines 7-8 will wait until it finds the action server and will not proceed longer until it makes sure that it is alive and will receive navigation requests. This is in fact a blocking instruction.

Line 11 defines a goal location and lines 13-25 specify the parameters of that location. It is easy to understand that lines 19-25 define the coordinates for the goal location. In particular the orientation component values expressed in quaternion refers to a heading equal to zero degrees.

Lines 14-15 are extremely important, in particular line 14. Line 14 goal.target_pose.header.frame_id = "map" specifies the reference frame for that location. In this example, it is specified as the map frame, which simply means that the coordinates will be considered in the global reference frame related to the map itself. In other words, it is the absolute position on the map. In case where the reference frame is set with respect to the robot, namely goal.target_pose.header.frame_id = "base_link" (like in this tutorial), the coordinate will have a completely other meaning. In fact, in this case the coordinate will represent the (x,y) coordinate with respect to robot base frame attached to the robot, so it is a relative position rather than a absolute position as in the case of using the map frame.

   move_base_msgs::MoveBaseGoal goal;

   //set up the frame parameters
   goal.target_pose.header.frame_id = "base_link"; //or "base_footprint"
   goal.target_pose.header.stamp = ros::Time::now();

   /* moving towards the goal*/
   goal.target_pose.pose.position.x =  1.0;
   goal.target_pose.pose.position.y =  0;
   goal.target_pose.pose.position.z =  0.0;
   goal.target_pose.pose.orientation.x = 0.0;
   goal.target_pose.pose.orientation.y = 0.0;
   goal.target_pose.pose.orientation.z = 0.0;
   goal.target_pose.pose.orientation.w = 1.0;

Line 15 adds a timestamp to the goal location.

Line 28 sends the goal location request to the move_base action server, and wait for its execution as shown in line 31 (ac.waitForResult()). Note that this request is sychrounous, which means it will block until the result is sent back to the requesting client object. After it finishes, we can check if the goal succeded or failed and output a message to the user accordingly.

In what follow, we present the equivalent code in Python, which does exactly the same thing:

:linenos:

import rospy
import actionlib
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from math import radians, degrees
from actionlib_msgs.msg import *
from geometry_msgs.msg import Point

class map_navigation():

  def choose(self):

    choice='q'

    rospy.loginfo("|-------------------------------|")
    rospy.loginfo("|PRESSE A KEY:")
    rospy.loginfo("|'0': Cafe ")
    rospy.loginfo("|'1': Office 1 ")
    rospy.loginfo("|'2': Office 2 ")
    rospy.loginfo("|'3': Office 3 ")
    rospy.loginfo("|'q': Quit ")
    rospy.loginfo("|-------------------------------|")
    rospy.loginfo("|WHERE TO GO?")
    choice = input()
    return choice

  def __init__(self):

    sc = SoundClient()
    path_to_sounds = "/home/ros/catkin_ws/src/gaitech_edu/src/sounds/"

    # declare the coordinates of interest
    self.xCafe = 15.50
    self.yCafe = 10.20
    self.xOffice1 = 27.70
    self.yOffice1 = 12.50
    self.xOffice2 = 30.44
    self.yOffice2 = 12.50
    self.xOffice3 = 35.20
    self.yOffice3 = 13.50
    self.goalReached = False
    # initiliaze
    rospy.init_node('map_navigation', anonymous=False)
    choice = self.choose()

    if (choice == 0):

      self.goalReached = self.moveToGoal(self.xCafe, self.yCafe)

    elif (choice == 1):

      self.goalReached = self.moveToGoal(self.xOffice1, self.yOffice1)

    elif (choice == 2):

      self.goalReached = self.moveToGoal(self.xOffice2, self.yOffice2)

    elif (choice == 3):

      self.goalReached = self.moveToGoal(self.xOffice3, self.yOffice3)

    if (choice!='q'):

      if (self.goalReached):
        rospy.loginfo("Congratulations!")
        #rospy.spin()



       else:
        rospy.loginfo("Hard Luck!")

    while choice != 'q':
      choice = self.choose()
      if (choice == 0):

        self.goalReached = self.moveToGoal(self.xCafe, self.yCafe)

      elif (choice == 1):

        self.goalReached = self.moveToGoal(self.xOffice1, self.yOffice1)

      elif (choice == 2):

        self.goalReached = self.moveToGoal(self.xOffice2, self.yOffice2)

      elif (choice == 3):

        self.goalReached = self.moveToGoal(self.xOffice3, self.yOffice3)

      if (choice!='q'):

        if (self.goalReached):
          rospy.loginfo("Congratulations!")
          #rospy.spin()


        else:
          rospy.loginfo("Hard Luck!")


  def shutdown(self):
        # stop robot
        rospy.loginfo("Quit program")
        rospy.sleep()

  def moveToGoal(self,xGoal,yGoal):

      #define a client for to send goal requests to the move_base server through a SimpleActionClient
      ac = actionlib.SimpleActionClient("move_base", MoveBaseAction)

      #wait for the action server to come up
      while(not ac.wait_for_server(rospy.Duration.from_sec(5.0))):
              rospy.loginfo("Waiting for the move_base action server to come up")


      goal = MoveBaseGoal()

      #set up the frame parameters
      goal.target_pose.header.frame_id = "map"
      goal.target_pose.header.stamp = rospy.Time.now()

      # moving towards the goal*/

      goal.target_pose.pose.position =  Point(xGoal,yGoal,0)
      goal.target_pose.pose.orientation.x = 0.0
      goal.target_pose.pose.orientation.y = 0.0
      goal.target_pose.pose.orientation.z = 0.0
      goal.target_pose.pose.orientation.w = 1.0

      rospy.loginfo("Sending goal location ...")
      ac.send_goal(goal)

      ac.wait_for_result(rospy.Duration(60))

      if(ac.get_state() ==  GoalStatus.SUCCEEDED):
              rospy.loginfo("You have reached the destination")
              return True

      else:
              rospy.loginfo("The robot failed to reach the destination")
              return False

if __name__ == '__main__':
    try:

        rospy.loginfo("You have reached the destination")
        map_navigation()
        rospy.spin()

    except rospy.ROSInterruptException:
        rospy.loginfo("map_navigation node terminated.")

Testing with Your Own Map on a Real Robot¶

It is easy to apply the navigation code on a real robot. However, you should deploy it using the map of the environment where you are going to run the experiments.

First, using the instructions of the Building a Map with a RIA-E100 tutorial, create the map of your experimental environment.

Now, you need to make changes to the launch file to consider the information of the map of your environment. Follow these steps:

• Step 1. Put the .yaml and .pgm map files in the /e100-navigation/maps/ folder. Let us assume they are called mymap.yaml and myamap.pgm.
• Step 2. copy the C++ code or python code to a local file and past it in one of the packages that you are useing in your workspace e.g you can past it in e100_navigation folder and name the file as my_navigation.py.
• Step 3. update the goals location that you got in Finding the Coordinates for Locations of Interest in you navigation code.
• Step 4. prepare you robot buy launching breing up file and navigation file, it the terminal type:

roslaunch e100_bringup minimal.launch

in new treminal type:

roslaunch e100_navigation navigation.launch

• Step 5. run your code on a new terminal as a ros node:

rosrun e100_navigation my_navigation.py

Now you can send your RIA-E100 to any goal you have defined before.