# 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

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]
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="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"/>
<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/

See you in the next post!

Topics:

### Check Out These Related Posts

1. in Step 1 of the second code chunk line 3, Yaw_=
Yaw_ is not set to anything, this is a syntax error. what is Yaw_ supposed to be = to?

thanks

• Hi @Gideon,

The value that must be there is just number “0” (as integer). Consider this for all the lines like this.

Thanks for reporting. It’s an issue in my code format for this blog post. I’m working to format it in a better way.

Hope it helped you! Cheers

2. hi
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
what exactly is the process here. especially did not understand up_eq.
I would appreciate if you help.
thanks.

3. Hello

Can you help me?

File “/home/fernando/catkin_ws/src/path_controller/scripts/path_controller.py”, line 28, in
initial_position_.x = rospy.get_param(‘initial_x’)
File “/opt/ros/melodic/lib/python2.7/dist-packages/rospy/client.py”, line 467, in get_param
return _param_server[param_name] #MasterProxy does all the magic for us
File “/opt/ros/melodic/lib/python2.7/dist-packages/rospy/msproxy.py”, line 123, in __getitem__
raise KeyError(key)
KeyError: ‘initial_x’

Thanks