# Exploring ROS with a 2 wheeled robot #10 – Bug 1  ## 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

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

See you in the next post!

Topics:

### Check Out These Related Posts

1. Thanks for this helpful tutorial. I have a question about velocities you used in go_to_point.py and follow_wall.py scripts. Why did you choose these velocities? Is there any computation behind it, or is just empirical?

2. 