Navigation tutorials¶

This section contains some tasks related with robot navigation and strategies for obstacle avoidance.

Reach the goal!¶

We have prepared for you a maze that includes robot, some goal positions and a closed map.

Your task is simple: Move the robot through the maze and reach the goals number 1 and 2 as faster as possible.

You can start the program by running:

cd ~/catkin_ws
source devel/setup.bash
roslaunch education_robotics tutorials_navigation.launch

In the same way as before, you can edit a template located at: education_robotics/src/tutorials/2_navigation.py

You should get something like this:

Map and odometry: obstacle avoidance¶

Once you have succeeded with the previous task, you are ready for increasing the complexity of next task:

Now, you should create an algorithm that allows the robot to avoid obstacles and explore the map until reaches goals number 1 and 2.

In order to do that, you can use robot odometry and map for making predictions about coming obstacles.

A simple template is available on: education_robotics/src/tutorials/3_obstacle_avoidance.py

This script subscribes for odometry and map information, and allows you to estimate the coming obstacles based on current robot position and fixed map.

Let’s have a look at part of script:

Create a callback for map subscriber, the map is represented as an image and after processing is saved into global variable (self.map)

# Update map
def callback_map(self, data):
    try:
        temp_map = self.bridge.imgmsg_to_cv2(data, "bgr8")

        # convert into gray scale
        (rows, cols, channels) = temp_map.shape
        if cols > 0 and rows > 0 :
            self.map = cv2.cvtColor(temp_map,cv2.COLOR_RGB2GRAY)

    except CvBridgeError as e:
        print(e)

Subscribe for odometry information and save it into global variables.

# update position and orientation of robot (odometry)
def process_odometry_message(self, odometry_msg):
    self.robot_position_x = odometry_msg.pose.pose.position.x
    self.robot_position_y = odometry_msg.pose.pose.position.y
    self.robot_orientation = odometry_msg.pose.pose.orientation.y

    quaternion = (
        odometry_msg.pose.pose.orientation.x,
        odometry_msg.pose.pose.orientation.y,
        odometry_msg.pose.pose.orientation.z,
        odometry_msg.pose.pose.orientation.w)

    (self.robot_roll, self.robot_pitch, self.robot_yaw) = euler_from_quaternion(quaternion)

Take into account that odometry is given in meters and the map is given in pixels, the relationship between them is:

1 meter = 100 pixels

This part does a basic obstacle detection, it considers a “safety boundary region” around the robot and a loop detect any obstacle inside this area.

# define a safety boundary region around robot
boundary = 40
detected_obstacle = False

# loop inside boundary, check if there are obstacles
for y in range( int((self.robot_position_y*100) - boundary), int((self.robot_position_y*100) + boundary) ):
    for x in range( int((self.robot_position_x*100) - boundary), int((self.robot_position_x*100) + boundary) ):
        map_point = self.map[x, y]
        if map_point < 1:
            # Obstacle detected!
            detected_obstacle = True

            # Here design your own routine for obstacle avoidance and navigation!

            # for logging: visualize using console plugin in rqt.
            # rospy.loginfo("close to obstacle y %f  x %f p %f", y , x, map_point)

            break

Once you have modified the source code (python script), you can test it by running:

cd ~/catkin_ws
source devel/setup.bash
roslaunch education_robotics tutorials_obstacle_avoidance.launch

You should have something like this: Gazebo simulator with robot and map + RQT interface with tools.