Exploring ROS with a 2 wheeled robot #8 – Bug 0

Work with the Bug 0 algorithm Exploring ROS with a 2 wheeled robot

Written by Marco Arruda

07/06/2019

Work with the Bug 0 algorithm

In this post, we are creating a new script to implement the Bug 0 behavior. This one that will drive our robot to a give position, switching between “go to point” and “wall follower” algorithms.


Step 1 – Introduction

First, let’s check how it is going to work, in a top-down view, we show the proposed architecture below:

We are launching 3 different nodes: Bug 0Go to point and Wall follower.

For the 2 nodes we have created before, we will adjust (create the switch service) in order to make them work together. Let’s go to code!


Step 2 – Go-to-point adjustments

I start here from the Go to point node, let’s check what we have to change!

We need to import a library for the service we are about to create:

from std_srvs.srv import *

And create a global variable to switch it to ON and OFF (starting by OFF)

active_ = False

The desired position is not anymore a fixed position, instead it comes from ROS parameter server:

desired_position_.x = rospy.get_param('des_pos_x')
desired_position_.y = rospy.get_param('des_pos_y')

Create a function to receive the service call:

# service callback
def go_to_point_switch(req):
    global active_
    active_ = req.data
    res = SetBoolResponse()
    res.success = True
    res.message = 'Done!'
    return res

And the main function is going to be changed a bit.
It’s basically considering the variable active_ before following its original rules.
Here it goes the code:

def main():
    global pub, active_
    
    rospy.init_node('go_to_point')
    pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
    sub_odom = rospy.Subscriber('/odom', Odometry, clbk_odom)
    srv = rospy.Service('go_to_point_switch', SetBool, go_to_point_switch)
    
    rate = rospy.Rate(20)
    while not rospy.is_shutdown():
        if not active_:
            continue
        else:
            if state_ == 0:
                fix_yaw(desired_position_)
            elif state_ == 1:
                go_straight_ahead(desired_position_)
            elif state_ == 2:
                done()
            else:
                rospy.logerr('Unknown state!')
        
        rate.sleep()

Step 3 – Wall follower adjustments

For the wall follower, we have to do some similar modifications, let’s check:

Importing services library:

from std_srvs.srv import *

And create a global variable to switch it to ON and OFF (starting by OFF), as well:

active_ = False

Define the function to be called when the service is requested:

def wall_follower_switch(req):
    global active_
    active_ = req.data
    res = SetBoolResponse()
    res.success = True
    res.message = 'Done!'
    return res

And the main function must, also, take into account the _active global variable before publishing to the robot differential drive:

def main():
    global pub_, active_
    
    rospy.init_node('wall_follower')
    pub_ = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
    sub = rospy.Subscriber('/m2wr/laser/scan', LaserScan, clbk_laser)
    srv = rospy.Service('wall_follower_switch', SetBool, wall_follower_switch)
    
    rate = rospy.Rate(20)
    while not rospy.is_shutdown():
        if not active_:
            rate.sleep()
            continue
        
        msg = Twist()
        if state_ == 0:
            msg = find_wall()
        elif state_ == 1:
            msg = turn_left()
        elif state_ == 2:
            msg = follow_the_wall()
            pass
        else:
            rospy.logerr('Unknown state!')
        
        pub_.publish(msg)
        
        rate.sleep()

Step 4 – Bug 0 – The Manager Node

We are done with our previous created nodes, now let’s create a new one to manage them:

Let’s start creating a new file ~/catkin_ws/src/motion_plan/scripts/bug0.py

First, let’s import the 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

Then, we define some global variables, in charge of controlling the flow of the script:

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', 'wall following']
state_ = 0
# 0 - go to point
# 1 - wall following

Now, let’s define the callback methods (laser and odometry subscribers):

# 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),
    }

Two other functions are created, in order to help us switching between states and calculate normal angles:

def change_state(state):
    global state_, state_desc_
    global srv_client_wall_follower_, srv_client_go_to_point_
    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)
        

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

Finally, our main function:

def main():
    global regions_, position_, desired_position_, state_, yaw_, yaw_error_allowed_
    global srv_client_go_to_point_, srv_client_wall_follower_
    
    rospy.init_node('bug0')
    
    sub_laser = rospy.Subscriber('/m2wr/laser/scan', LaserScan, clbk_laser)
    sub_odom = rospy.Subscriber('/odom', Odometry, clbk_odom)
    
    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 = rospy.Rate(20)
    while not rospy.is_shutdown():
        if regions_ == None:
            continue
        
        if state_ == 0:
            if regions_['front'] > 0.15 and regions_['front'] < 1:
                change_state(1)
        
        elif state_ == 1:
            desired_yaw = math.atan2(desired_position_.y - position_.y, desired_position_.x - position_.x)
            err_yaw = normalize_angle(desired_yaw - yaw_)
            
            if math.fabs(err_yaw) < (math.pi / 6) and \
               regions_['front'] > 1.5:
                change_state(0)
            
            if err_yaw > 0 and \
               math.fabs(err_yaw) > (math.pi / 4) and \
               math.fabs(err_yaw) < (math.pi / 2) and \
               regions_['left'] > 1.5:
                change_state(0)
                
            if err_yaw < 0 and \
               math.fabs(err_yaw) > (math.pi / 4) and \
               math.fabs(err_yaw) < (math.pi / 2) and \
               regions_['right'] > 1.5:
                change_state(0)
            
        rate.sleep()

if __name__ == "__main__":
    main()

We are susbcribing the odometry and laser because:

  1. In order to define the state, we need to know where the robot is
  2. The laser tells us if it’s necessary to follow a wall, or if we can go ahead

In both states (0 and 1), we are checking the laser readings. Depending on it, we can change the state to the other one.

Let’s check if it works!

 


 

Step 5 – Launching Bug 0

Finally, we create a launch file (~/catkin_ws/src/motion_plan/launch/bug0.launch), in order to start everything in a single command.

Here it goes the way we propose:

<launch>
    <arg name="des_x" />
    <arg name="des_y" />
    <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" output="screen" />
    <node pkg="motion_plan" type="go_to_point.py"
          name="go_to_point" output="screen" />
    <node pkg="motion_plan" type="bug0.py"
          name="bug0" output="screen" />
</launch>

Let’s test it!


Step 6 – Testing

We first launch the world we have been using in the previous post. (my_worlds world.launch)

And from a web shell, spawn the robot at the given position:

roslaunch m2wr_description spawn.launch x:=0 y:=8

Wait a few seconds, when the robot is ready, start the launch file with the Bug 0 algorithm:

roslaunch motion_plan bug0.launch des_x:=0 des_y:=-8

What is happening there?

Below we see an image describing the steps and state changes during the Bug 0 motion:

It starts going to the point, finds a wall, circumnavigates it, go to the point again..

And this happens some times until it reaches the goal!


Step 7 – Conclusion

If you lost some step, you can always clone the ROSject generated from this series. Here it goes the one for the current one: http://www.rosject.io/l/a283a96/

In the next post, we will explore a failure case for Bug 0 and look to another solution: Bug 1 algorithm!

Cheers!

[irp posts=”13273″ name=”Exploring ROS with a 2 wheeled robot #7 – Wall Follower Algorithm”]

Topics:
Masterclass 2023 batch2 blog banner

Check Out These Related Posts

129. ros2ai

129. ros2ai

I would like to dedicate this episode to all the ROS Developers who believe that ChatGPT or...

read more

0 Comments

Submit a Comment

Your email address will not be published.

This site uses Akismet to reduce spam. Learn how your comment data is processed.

Pin It on Pinterest

Share This