Step 0 – Introduction

Yes! We have skipped part number #11. That’s because it was an adjustment from ROS Indigo to ROS Kinetic. It’s not something necessary now, because we have already started using Kinetic.

In this post, we implement Bug 2 algorithm in 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, we’ll have the robot working as shown on the image below:

Step 1 – Importing libraries

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

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

chmod +x ~/catkin_ws/src/motion_plan/scripts/bug2.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
from gazebo_msgs.msg import ModelState
from gazebo_msgs.srv import SetModelState
# 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()
initial_position_ = Point()
initial_position_.x = rospy.get_param('initial_x')
initial_position_.y = rospy.get_param('initial_y')
initial_position_.z = 0
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
count_state_time_ = 0 # seconds the robot is in a state
count_loop_ = 0
# 0 - go to point
# 1 - wall following

Like in Bug 1 algorithm, we are storing the state of the robot. I put some description of the states in an Array called state_desc_. There are also 2 new arguments being received: initial_x and initial_y. They are used to restore the robot position in case you want to restart the algorithm. Very useful to test the algorithm many times in a row!

 

Step 2 – Defining callbacks

Here we define callbacks, the very same as before, no big deal here. But don’t miss this part of the code:

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

 

Step 3 – Helper functions

The first helper function is change_state. We had one like this in Bug 1, but we have different rules and states now. Let’s check it:

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)

We have only 2 states: wall_follower and go_to_point. The different is that we are not driving the robot in a straight line to the desired point, but following the original line. The one from the initial position to the desired one. This will be implemented on the main function.

The next one: distance_to_line:

def distance_to_line(p0):
    # p0 is the current position
    # p1 and p2 points define the line
    global initial_position_, desired_position_
    p1 = initial_position_
    p2 = desired_position_
    # here goes the equation
    up_eq = math.fabs((p2.y - p1.y) * p0.x - (p2.x - p1.x) * p0.y + (p2.x * p1.y) - (p2.y * p1.x))
    lo_eq = math.sqrt(pow(p2.y - p1.y, 2) + pow(p2.x - p1.x, 2))
    distance = up_eq / lo_eq

    return distance

This function is used to calculate the distance of the robot to the initial line. Super important for this algorithm and it’s used all the time during its execution.

Finally, we’ll use again the normalize_angle function:

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 check the main function

Part 1 – ROS node, callbacks, services and 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 count_state_time_, count_loop_

    rospy.init_node('bug0')

    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')
    rospy.wait_for_service('/gazebo/set_model_state')

    srv_client_go_to_point_ = rospy.ServiceProxy('/go_to_point_switch', SetBool)
    srv_client_wall_follower_ = rospy.ServiceProxy('/wall_follower_switch', SetBool)
    srv_client_set_model_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)

    # set robot position
    model_state = ModelState()
    model_state.model_name = 'm2wr'
    model_state.pose.position.x = initial_position_.x
    model_state.pose.position.y = initial_position_.y
    resp = srv_client_set_model_state(model_state)

    # initialize going to the point
    change_state(0)

    rate = rospy.Rate(20)

This is something necessary, the same standard we have been doing so far.

Part 2 – The loop logic

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

        distance_position_to_line = distance_to_line(position_)

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

        elif state_ == 1:
            if count_state_time_ > 5 and \
               distance_position_to_line < 0.1:
                change_state(0)

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

        rospy.loginfo("distance to line: [%.2f], position: [%.2f, %.2f]", distance_to_line(position_), position_.x, position_.y)
        rate.sleep()

We are all the time calculating the distance to the original line. Depending on the presence of obstacles, we switch between wall_follower and go_to_point.

The point to be used as goal is always the nearest in the line.

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/bug2.launch. The content is:

 

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

<launch>
    <arg name="initial_x" default="0" />
    <arg name="initial_y" default="8" />
    <arg name="des_x" default="0" />
    <arg name="des_y" default="-4" />
    <param name="initial_x" value="$(arg initial_x)" />
    <param name="initial_y" value="$(arg initial_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" />
    <node pkg="motion_plan" type="go_to_point.py" name="go_to_point" />
    <node pkg="motion_plan" type="bug2.py" name="bug2" output="screen" />
</launch>

We improved it a little bit. We are restarting our robot position every time we launch it. That’s why we have to new arguments.

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:

But.. just before, change the world to the previous one we worked before (~/simulation_ws/src/my_worlds/launch/world.launch):

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

<launch>
  <arg name="robot" default="machines"/>
  <arg name="debug" default="false"/>
  <arg name="gui" default="true"/>
  <arg name="headless" default="false"/>
  <arg name="pause" default="false"/>
  <arg name="world" default="world02" />
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" value="$(find my_worlds)/worlds/$(arg world).world"/>
    <arg name="debug" value="$(arg debug)" />
    <arg name="gui" value="$(arg gui)" />
    <arg name="paused" value="$(arg pause)"/>
    <arg name="use_sim_time" value="true"/>
    <arg name="headless" value="$(arg headless)"/>
    <env name="GAZEBO_MODEL_PATH" value="$(find simulation_gazebo)/models:$(optenv GAZEBO_MODEL_PATH)"/>
  </include>
</launch>

 

2 – Spawn the robot

roslaunch m2wr_description spawn.launch y:=8

3 – Launch the algorithm

roslaunch motion_plan bug2.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/b550d08/

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

See you in the next post!