In this post, we will see how to make a robot detect and avoid an obstacle in its front. We will move the robot forward until it detects there’s an obstacle (the wall) closer than 1 meter. Then, we will stop the robot so that it does not collide with the wall.
PS: This ROS project is part of our ROS Mini Challenge series, which gives you an opportunity to win an amazing ROS Developers T-shirt! This challenge is already solved. For updates on future challenges, please stay tuned to our Twitter channel.
Step 1: Grab a copy of the ROS Project that contains the code for the challenge
Click here to get your own copy of the project. If you don’t have an account on the ROS Development Studio, you will need to create one. Once you create an account or log in, we will copy the project to your workspace. That done, open your ROSject using the Open button. This might take a few moments, please be patient.
You should now see a notebook with detailed instructions about the challenge. This post includes a summary of these instructions as well as the solution to the challenge.
PS: Please ignore the Claim your Prize! section because…well…you are late the party 🙂
Step 2: Start the Simulation and get the robot moving
- Click on the Simulations menu and then Choose launch file… . In the dialog that appears, select rotw5.launch under rosbot_gazebo package. Then click the Launch button. You should see a Gazebo window popup showing the simulation: a ROSbot in front of a wall.
- Get the robot moving. Pick a Shell from the Tools menu and run the following commands:
user:~$ source ~/catkin_ws/devel/setup.bash user:~$ rosrun rotw5_pkg detect_wall.py
Yay, the robot moved. But, wait, it didn’t detect and avoid the obstacle – it crashed into the wall! We don’t want that, so let’s fix it in the next section!
Step 3: Identify the problem, man!
So the robot didn’t stop as we expected. As the challenge hinted, something must be wrong with the code, especially the part that should detect the wall and stop the robot. Let’s see.
Fire up the IDE from the Tools menu and browse to catkin_ws/src/rotw5_pkg/src/detect_wall.py
#! /usr/bin/env python
import rospy
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist
def callback(msg):
#If the distance to an obstacle in front of the robot is bigger than 1 meter, the robot will move forward
if msg.ranges[360] > 1:
move.linear.x = 0.5
move.angular.z = 0.0
#If the distance to an obstacle in front of the robot is smaller than 1 meter, the robot will stop
if msg.ranges[360] < 1:
move.linear.x = 0.0
move.angular.z = 0.0
pub.publish(move)
rospy.init_node('rotw5_node')
sub = rospy.Subscriber('/scan', LaserScan, callback) #We subscribe to the laser's topic
pub = rospy.Publisher('/cmd_vel', Twist)
move = Twist()
rospy.spin()
Now, this is the part that should stop the robot. It says that if there’s an obstacle less than 1 meter away, we should set both linear and angular velocities to zero (stopping the robot)
#If the distance to an obstacle in front of the robot is smaller than 1 meter, the robot will stop
if msg.ranges[360] < 1:
move.linear.x = 0.0
move.angular.z = 0.0
This is not being executed obviously because the condition msg.ranges[360] < 1 is never True! Let’s test this assumption:
- In the IDE, add the following lines to the
detect_wall.py, after the comment below. Then save.
#If the distance to an obstacle in front of the robot is smaller than 1 meter, the robot will stop print "Number of readings: ", print len(msg.ranges) print "Reading at position 360:", msg.ranges[360]
- From the Shell: Press Ctrl + C to end the current program. Then stop the robot by publishing zero velocities to the
/cmd_veltopic. Press Ctrl + C again and then call the Gazebo service that resets the world.
user:~$ rostopic pub /cmd_vel geometry_msgs/Twist "linear:
x: 0.0
y: 0.0
z: 0.0
angular:
x: 0.0
y: 0.0
z: 0.0"
publishing and latching message. Press ctrl-C to terminate
user:~$ rosservice call /gazebo/reset_world "{}"
- Run the program again and observe the output:
user:~$ rosrun rotw5_pkg detect_wall.py Number of readings: 720 Reading at position 360: inf # ... Number of readings: 720 Reading at position 360: 11.7952680588
You should notice that several readings at 360 are printed, and none of them is less than 1. Whew!
Step 4: Now let’s solve the problem
We saw from the previous section that our problem was the condition msg.ranges[360] < 1, but how did we arrive at this condition?
- We knew the laser sensor has 720 scan points stored in the
rangesarray, as seen in the output of our print statement in Step 3. - We assumed that the laser had a range of 180 degrees, covering the front of the robot only, so that position 360 is at the front-middle of the robot. But this is false, and we can confirm this by running the following command:
user:~$ rostopic echo /scan -n1
header:
seq: 49846
stamp:
secs: 639
nsecs: 47000000
frame_id: "laser"
angle_min: -3.14159011841
angle_max: 3.14159011841
angle_increment: 0.00873877573758
time_increment: 0.0
The command prints out a single message from the /scan topic. We can deduce the range from the angle_max and angle_min values: angle_max - angle_min = 2pi = 360 deg! Buff! So position 360/720 of the scan messages could be somewhere at the back of the robot. No wonder it didn’t detect the wall.
Now, we have to figure out which position is the front-middle of the robot. Depending on how the sensor was installed, this could be any position. For this robot, the sensor was installed with position “0″ at the front-center, so the readings of positions “0″ or 720 could represent the front-center. Let’s use the position “0″ and test again.
- In the IDE, change the front-middle position from
360to “0″:
#! /usr/bin/env python
import rospy
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist
def callback(msg):
#If the distance to an obstacle in front of the robot is bigger than 1 meter, the robot will move forward
if msg.ranges[0] > 1:
move.linear.x = 0.5
move.angular.z = 0.0
#If the distance to an obstacle in front of the robot is smaller than 1 meter, the robot will stop
print "Number of ranges: ", len(msg.ranges)
print "Reading at position 0:", msg.ranges[0]
if msg.ranges[0] < 1:
move.linear.x = 0.0
move.angular.z = 0.0
pub.publish(move)
rospy.init_node('rotw5_node')
sub = rospy.Subscriber('/scan', LaserScan, callback) #We subscribe to the laser's topic
pub = rospy.Publisher('/cmd_vel', Twist)
move = Twist()
rospy.spin()
- From the Shell: Press Ctrl + C to end the current program. Then stop the robot by publishing zero velocities to the
/cmd_veltopic. Press Ctrl + C again and then call the Gazebo service that resets the world. - Run the program again and observe the output and see if the robot detects and avoids the obstacle:
user:~$ rosrun rotw5_pkg detect_wall.py Number of ranges: 720 Reading at position 0: 2.49150013924 Number of ranges: 720 #... Number of ranges: 720 Reading at position 0: 1.92249274254 #... Number of ranges: 720 Reading at position 0: 0.991922438145
Yay, finally we are able to make the robot detect and avoid the obstacle!
Extra: Video of this post
We made a video showing how this challenge was solved. If you prefer “sights and sounds” to “black and white”, here you go:
Related Resources
Feedback
Did you like this post? Do you have questions about what was explained? Whatever the case, please leave a comment on the comments section below, so we can interact and learn from each other.
If you want to learn about other ROS or ROS2 topics, please let us know in the comments area and we will do a video or post about it.
Hi. How do I use PointCloud2 instead of LaseScan to detect obstacles in front
what’s happening, distinguished blog on suety loss. akin helped.