Step 0 – Introduction

In this post, we implement Bug 1 algorithm to our existing robot! Let’s use some algorithms we have already done in order to have it all working!

At the end of this post, you must have the robot+algorithm working like shown at the image below:

Let’s go!

Step 1 – Importing libraries and defining states

We start creating a script file, it goes like that: ~/catkin_ws/src/motion_plan/scripts/bug1.py

(Don’t forget to assign the necessary permissions to the file)

chmod +x ~/catkin_ws/src/motion_plan/scripts/bug1.py

Open the file in the editor and start coding. We start from the necessary libraries:

#! /usr/bin/env python

# import ros stuff
import rospy
# import ros message
from geometry_msgs.msg import Point
from sensor_msgs.msg import LaserScan
from nav_msgs.msg import Odometry
from tf import transformations
# import ros service
from std_srvs.srv import *

import math

Now.. we define some global variables to store the goal and the current status of the robot and algorithm:

srv_client_go_to_point_ = None
srv_client_wall_follower_ = None
yaw_ = 0
yaw_error_allowed_ = 5 * (math.pi / 180) # 5 degrees
position_ = Point()
desired_position_ = Point()
desired_position_.x = rospy.get_param('des_pos_x')
desired_position_.y = rospy.get_param('des_pos_y')
desired_position_.z = 0
regions_ = None
state_desc_ = ['Go to point', 'circumnavigate obstacle', 'go to closest point']
state_ = 0
circumnavigate_starting_point_ = Point()
circumnavigate_closest_point_ = Point()
count_state_time_ = 0 # seconds the robot is in a state
count_loop_ = 0
# 0 - go to point
# 1 - circumnavigate
# 2 - go to closest point

Take a closer look.. You’ll notice there are some new variables (in comparison to Bug 0 alg.) to store, not only the state, but also some necessary points (starting point of circumnavigation state and closest point to the goal).

 

Step 2 – Defining callbacks

Second step, we define the callbacks, afterall we need to read the position (given by odometry) and the obstacles (laser reading). They go like:

# callbacks
def clbk_odom(msg):
    global position_, yaw_

    # position
    position_ = msg.pose.pose.position

    # yaw
    quaternion = (
        msg.pose.pose.orientation.x,
        msg.pose.pose.orientation.y,
        msg.pose.pose.orientation.z,
        msg.pose.pose.orientation.w)
    euler = transformations.euler_from_quaternion(quaternion)
    yaw_ = euler[2]

def clbk_laser(msg):
    global regions_
    regions_ = {
        'right':  min(min(msg.ranges[0:143]), 10),
        'fright': min(min(msg.ranges[144:287]), 10),
        'front':  min(min(msg.ranges[288:431]), 10),
        'fleft':  min(min(msg.ranges[432:575]), 10),
        'left':   min(min(msg.ranges[576:719]), 10),
    }

There is nothing new here, if you compare to the Bug 0 we have implemented before!

Step 3 – Helper functions

This part doesn’t contain rules of the robot behavior, but it’s important to help other functions to work properly (and clearer), since we are helping them to get values and assigning states in a simpler way.

The first function is change_state, we had something like this one to other algorithms:

def change_state(state):
    global state_, state_desc_
    global srv_client_wall_follower_, srv_client_go_to_point_
    global count_state_time_
    count_state_time_ = 0
    state_ = state
    log = "state changed: %s" % state_desc_[state]
    rospy.loginfo(log)
    if state_ == 0:
        resp = srv_client_go_to_point_(True)
        resp = srv_client_wall_follower_(False)
    if state_ == 1:
        resp = srv_client_go_to_point_(False)
        resp = srv_client_wall_follower_(True)
    if state_ == 2:
        resp = srv_client_go_to_point_(False)
        resp = srv_client_wall_follower_(True)

Then, we create the function calc_dist_points. It’s used to know the distance from the current position of the robot to interesting points (e.g: entry point of circumnavigation state, goal, etc.). It is quite simple, but we don’t want to have mathematical functions in our logics:

def calc_dist_points(point1, point2):
    dist = math.sqrt((point1.y - point2.y)**2 + (point1.x - point2.x)**2)
    return dist

 

Finally, a function to normalize angles. Very common for mobile robots algorithms:

def normalize_angle(angle):
    if(math.fabs(angle) > math.pi):
        angle = angle - (2 * math.pi * angle) / (math.fabs(angle))
    return angle

 

Step 4 – The core of it all!

Let’s go to the main function, where we have it all working together.

Part 1 – Defining ROS node, callbacks, services and the initial state

def main():
    global regions_, position_, desired_position_, state_, yaw_, yaw_error_allowed_
    global srv_client_go_to_point_, srv_client_wall_follower_
    global circumnavigate_closest_point_, circumnavigate_starting_point_
    global count_loop_, count_state_time_

    rospy.init_node('bug1')

    sub_laser = rospy.Subscriber('/m2wr/laser/scan', LaserScan, clbk_laser)
    sub_odom = rospy.Subscriber('/odom', Odometry, clbk_odom)

    rospy.wait_for_service('/go_to_point_switch')
    rospy.wait_for_service('/wall_follower_switch')

    srv_client_go_to_point_ = rospy.ServiceProxy('/go_to_point_switch', SetBool)
    srv_client_wall_follower_ = rospy.ServiceProxy('/wall_follower_switch', SetBool)

    # initialize going to the point
    change_state(0)

    rate_hz = 20
    rate = rospy.Rate(rate_hz)

 

And finally, our loop control:

    while not rospy.is_shutdown():
        if regions_ == None:
            continue

        if state_ == 0:
            if regions_['front'] > 0.15 and regions_['front'] < 1:
                circumnavigate_closest_point_ = position_
                circumnavigate_starting_point_ = position_
                change_state(1)

        elif state_ == 1:
            # if current position is closer to the goal than the previous closest_position, assign current position to closest_point
            if calc_dist_points(position_, desired_position_) < calc_dist_points(circumnavigate_closest_point_, desired_position_):
                circumnavigate_closest_point_ = position_

            # compare only after 5 seconds - need some time to get out of starting_point
            # if robot reaches (is close to) starting point
            if count_state_time_ > 5 and \
               calc_dist_points(position_, circumnavigate_starting_point_) < 0.2:
                change_state(2)

        elif state_ == 2:
            # if robot reaches (is close to) closest point
            if calc_dist_points(position_, circumnavigate_closest_point_) < 0.2:
                change_state(0)

        count_loop_ = count_loop_ + 1
        if count_loop_ == 20:
            count_state_time_ = count_state_time_ + 1
            count_loop_ = 0

        rate.sleep()

Depending on the state, we use one rule or another to do a transition.

For example, from state 0 – go to point, if there’s an obstacle ahead, we change to state 1 – circumnavigate obstacle

 

Step 5 – Launch it!

Let’s create a launch file and see it working!

Create a new file at ~/catkin_ws/src/motion_plan/launch/bug1.launch. The content is:

<?xml version="1.0" encoding="UTF-8"?>

<launch>
    <arg name="des_x" default="0" />
    <arg name="des_y" default="-3" />
    <param name="des_pos_x" value="$(arg des_x)" />
    <param name="des_pos_y" value="$(arg des_y)" />
    <node pkg="motion_plan" type="follow_wall.py" name="wall_follower" />
    <node pkg="motion_plan" type="go_to_point.py" name="go_to_point" />
    <node pkg="motion_plan" type="bug1.py" name="bug1" output="screen" />
</launch>

1 – You must launch the simulation like we did in previous chapters. Go to the simulation launcher and choose the world configured in our package:

2 – Spawn the robot

roslaunch m2wr_description spawn.launch y:=8

3 – Launch the algorithm

roslaunch motion_plan bug1.launch

 

Step 6 – Conclusion

Remember! If in any of the steps above you have missed something, you can always clone our ROSJect! Use this link to get your copy: http://www.rosject.io/l/b3a5b3c/

If you like it or have suggestion to improve it, leave a comment!

See you in the next post!