Exploring ROS with a 2 wheeled robot #12 – Bug 2

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!

Exploring ROS with a 2 wheeled robot #7 – Wall Follower Algorithm

Wall Follower Algorithm

Hello ROS Developers! In this post number #7, as we continue on the track of the video series, we are going to go line-by-line in order to achieve the Wall Follower Algorithm.

In order to start, you can use the ROSject we generate in the previous post, by copying it here.

Our goal is to achieve the following behavior:

1. Find the wall – Go straight+right until it reaches an obstacle
2. Turn left, if there is no obstacle in front of the robot
3. Go straight ahead, if there is an obstacle in the right side

Which is described in the image below:

Let’s start!

STEP 1

First things first, creating a new script file for the algorithm. I’m calling it follow_wall.py, and  it goes into the folder ~/catkin_ws/src/motion_plan/scripts/.

Some necessary libraries for our script:

#! /usr/bin/env python

# import ros stuff
import rospy
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry
from tf import transformations

import math

And just below, our global variables:

pub_ = None
regions_ = {
    'right': 0,
    'fright': 0,
    'front': 0,
    'fleft': 0,
    'left': 0,
}
state_ = 0
state_dict_ = {
    0: 'find the wall',
    1: 'turn left',
    2: 'follow the wall',
}

We are again separating the laser regions (right, front-right, front, front-left and left)
And our state_dict, which defines 3 main states: find the wall, turn left and follow the wall

STEP 2

Further, two necessary functions, but no magic or business logic here:

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:713]), 10),
    }

    take_action()


def change_state(state):
    global state_, state_dict_
    if state is not state_:
        print 'Wall follower - [%s] - %s' % (state, state_dict_[state])
        state_ = state

In the callback, we assign the means of the laser reading to the regions we defined
And the change_state is just a formal way to change the state, log and change the value of the variable state_

STEP 3

And now, we define the main function, the one that contains the logic of the wall following algorithm. It’s called here take_action:

def take_action():
    global regions_
    regions = regions_
    msg = Twist()
    linear_x = 0
    angular_z = 0
    
    state_description = ''
    
    d = 1.5
    
    if regions['front'] > d and regions['fleft'] > d and regions['fright'] > d:
        state_description = 'case 1 - nothing'
        change_state(0)
    elif regions['front'] < d and regions['fleft'] > d and regions['fright'] > d:
        state_description = 'case 2 - front'
        change_state(1)
    elif regions['front'] > d and regions['fleft'] > d and regions['fright'] < d:
        state_description = 'case 3 - fright'
        change_state(2)
    elif regions['front'] > d and regions['fleft'] < d and regions['fright'] > d:
        state_description = 'case 4 - fleft'
        change_state(0)
    elif regions['front'] < d and regions['fleft'] > d and regions['fright'] < d:
        state_description = 'case 5 - front and fright'
        change_state(1)
    elif regions['front'] < d and regions['fleft'] < d and regions['fright'] > d:
        state_description = 'case 6 - front and fleft'
        change_state(1)
    elif regions['front'] < d and regions['fleft'] < d and regions['fright'] < d:
        state_description = 'case 7 - front and fleft and fright'
        change_state(1)
    elif regions['front'] > d and regions['fleft'] < d and regions['fright'] < d:
        state_description = 'case 8 - fleft and fright'
        change_state(0)
    else:
        state_description = 'unknown case'
        rospy.loginfo(regions)

We are treating 8 possibilities of the three region readings of the laser (Like in the previous post, but now for a different behavior)

Switch to state Find the wall when:

1. No obstacles detected
2. Obstacle on the left side
3. Obstacles on the left and right sides

For the state Turn left:

1. Obstacles in front of the robot
2. Obstacles in front and left of the robot
3. Obstacles in front and right of the robot
4. Obstacles in front, left and right of the robot

And finally, we change to the state Follow the wall when:

1. Obstacle detected only on the right side of the robot

STEP 4

The next thing is to define the behavior of each state:

def find_wall():
    msg = Twist()
    msg.linear.x = 0.2
    msg.angular.z = -0.3
    return msg

def turn_left():
    msg = Twist()
    msg.angular.z = 0.3
    return msg

def follow_the_wall():
    global regions_
    
    msg = Twist()
    msg.linear.x = 0.5
    return msg

This is quite basic, but pay attention to the conventions!! Here we defined to keep the wall on the right side, so if you want to change this convention, you need to play with the signal of the variables. It’s also important it can change depending on the gazebo version and gazebo plugins you are using!! (E.g: During the development of this series, the gazebo version was changed from 4 to 7 and the Z-axis of the driver was changed!)

STEP 5

Finally, we just define our main function, where the main node, publishers, subscribers, and a loop, are defined:

def main():
    global pub_
    
    rospy.init_node('reading_laser')
    
    pub_ = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
    
    sub = rospy.Subscriber('/m2wr/laser/scan', LaserScan, clbk_laser)
    
    rate = rospy.Rate(20)
    while not rospy.is_shutdown():
        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()

if __name__ == '__main__':
    main()

After having it running, you may have the robot going around one of the walls. Don’t forget to spawn the robot near to a wall, or just spawn it anywhere and change its position manually, using Gazebo interface.

You must have something like the below:

I hope you liked this post!

If something went wrong in the process of following this tutorial, you can clone the ROSject generated here.

In the next post, we are gonna work with the Bug 0 algorithm using the previous scripts we have created: Wall following + Go to point!

Keep following our channels!

Cheers

[irp posts=”13235″ name=”Exploring ROS with a 2 wheeled Robot #6 – Robot Motion Planning”]

Exploring ROS with a 2 Wheeled Robot #4 – Laser Scan Reading

Hello ROS Developers!

In this post, we summarize the video – Exploring ROS using a 2 Wheeled Robot ep. 4, where we start using the Laser Scan data. Let’s start!

First thing, let’s open our ROSject finished with the previous post.

We had some minor adjustments (in order to calibrate the robot and fix some minor issues), that you can track here. They are the following:

• Adjust robot size and calibrate differential drive
• Update CMakeLists.txt
• Create a world file, that we are going to use in the next episodes
• Create a launch file to start the world

The ROSject we finished in the last post is this one, but, in order to get the one updated (with the fixed I mentioned), click here.

You must be able to launch a simulation like below:

After that, let’s start with the laser part!

Let’s start creating a new package, but this time at catkin_ws. This is because we are not creating a simulation, instead it’s a motion planning package, that will be able to command any kind of mobile robot! It’s important to understand we are not developing a package strict to our robot, but something generic.

In a web shell, execute the following:

user:~$ cd ~/catkin_ws/src/
user:~$ catkin_create_pkg motion_plan std_msgs geometry_msgs rospy sensor_msgs

And create our first script that interacts with the laser data> ~/catkin_ws/src/motion_plan/scripts/reading_laser.py:

#! /usr/bin/env python

import rospy

from sensor_msgs.msg import LaserScan

def clbk_laser(msg):
    # 720 / 5 = 144
    regions = [
        min(min(msg.ranges[0:143]), 10),
        min(min(msg.ranges[144:287]), 10),
        min(min(msg.ranges[288:431]), 10),
        min(min(msg.ranges[432:575]), 10),
        min(min(msg.ranges[576:713]), 10),
    ]
    rospy.loginfo(regions)

def main():
    rospy.init_node('reading_laser')

    sub = rospy.Subscriber('/m2wr/laser/scan', LaserScan, clbk_laser)

    rospy.spin()

if __name__ == '__main__':
    main()

Using a web shell, spawn the robot in the simulation we have running:

user:~$ roslaunch m2wr_description spawn.launch

With the spawning process finished, start the new script we`ve just created:

user:~$ rosrun motion_plan reading_laser.py

You shall see an output like the below:

[INFO] [1556996996.074220, 1870.728000]: [10, 8.899812698364258, 1.7639966011047363, 0.9439168572425842, 0.8716692924499512]
[INFO] [1556996996.127200, 1870.781000]: [10, 8.92310905456543, 1.7670421600341797, 0.9425056576728821, 0.8715123534202576]
[INFO] [1556996996.176019, 1870.830000]: [10, 8.915216445922852, 1.770876407623291, 0.9503755569458008, 0.8712261319160461]
[INFO] [1556996996.226823, 1870.881000]: [10, 8.923232078552246, 1.7670140266418457, 0.942345380783081, 0.8683222532272339]
[INFO] [1556996996.277591, 1870.931000]: [10, 8.91376781463623, 1.7766363620758057, 0.945663332939148, 0.8739328980445862]
[INFO] [1556996996.327826, 1870.981000]: [10, 8.925933837890625, 1.7535350322723389, 0.954210638999939, 0.8760458827018738]
[INFO] [1556996996.378767, 1871.031000]: [10, 8.916155815124512, 1.7713392972946167, 0.9581797122955322, 0.866176962852478]
[INFO] [1556996996.428421, 1871.081000]: [10, 8.912318229675293, 1.7645303010940552, 0.9598914384841919, 0.8663960695266724]
[INFO] [1556996996.478520, 1871.130000]: [10, 8.918071746826172, 1.786738634109497, 0.9538688659667969, 0.8684337139129639]
[INFO] [1556996996.529212, 1871.180000]: [10, 8.918127059936523, 1.7791880369186401, 0.9522199034690857, 0.8771331906318665]

This is a print of an array which contains 5 means of the laser, summarized in the image below:

You can move the robot using `cmd_vel` or using gazebo drag tool in order to see the values changing. In the image below we have the bottom of a shell under the gazebo window. You can check the 5th position of the array represents the area on the left side of the robot. In the other side, we have maximum value being read: 10 meters, on the right side of the robot.

In case something went wrong up to here, you can get a copy of the final version of this post here.

Feel free to explore the code, navigate using teleop or a custom node. In the next post, we’ll work on an algorithm to make the robot avoid obstacles in the world!

Cheers!