Example 3
Example 3
The aim of example 3 is to combine the two previous examples and have Stretch utilize its laser scan data to avoid collision with objects as it drives forward.
Begin by running the following commands in a new terminal.
# Terminal 1
roslaunch stretch_core stretch_driver.launch
# Terminal 2
roslaunch stretch_core rplidar.launch
To set navigation mode and to activate the avoider.py node, type the following in a new terminal.
# Terminal 3
rosservice call /switch_to_navigation_mode
cd catkin_ws/src/stretch_tutorials/src/
python avoider.py
The Code
#!/usr/bin/env python
import rospy
from numpy import linspace, inf, tanh
from math import sin
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan
class Avoider:
"""
A class that implements both a LaserScan filter and base velocity control
for collision avoidance.
"""
def __init__(self):
"""
Function that initializes the subscriber, publisher, and marker features.
:param self: The self reference.
"""
self.pub = rospy.Publisher('/stretch/cmd_vel', Twist, queue_size=1) #/stretch_diff_drive_controller/cmd_vel for gazebo
self.sub = rospy.Subscriber('/scan', LaserScan, self.set_speed)
self.width = 1
self.extent = self.width / 2.0
self.distance = 0.5
self.twist = Twist()
self.twist.linear.x = 0.0
self.twist.linear.y = 0.0
self.twist.linear.z = 0.0
self.twist.angular.x = 0.0
self.twist.angular.y = 0.0
self.twist.angular.z = 0.0
def set_speed(self,msg):
"""
Callback function to deal with incoming LaserScan messages.
:param self: The self reference.
:param msg: The subscribed LaserScan message.
:publishes self.twist: Twist message.
"""
angles = linspace(msg.angle_min, msg.angle_max, len(msg.ranges))
points = [r * sin(theta) if (theta < -2.5 or theta > 2.5) else inf for r,theta in zip(msg.ranges, angles)]
new_ranges = [r if abs(y) < self.extent else inf for r,y in zip(msg.ranges, points)]
error = min(new_ranges) - self.distance
self.twist.linear.x = tanh(error) if (error > 0.05 or error < -0.05) else 0
self.pub.publish(self.twist)
if __name__ == '__main__':
rospy.init_node('avoider')
Avoider()
rospy.spin()
The Code Explained
Now let's break the code down.
#!/usr/bin/env python
import rospy
from numpy import linspace, inf, tanh
from math import sin
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan
numpy
and math
that are required within this code, thus linspace, inf, tanh, and sin are imported. The sensor_msgs.msg
import is so that we can subscribe to LaserScan
messages. The geometry_msgs.msg
import is so that we can send velocity commands to the robot.
self.pub = rospy.Publisher('/stretch/cmd_vel', Twist, queue_size=1)#/stretch_diff_drive_controller/cmd_vel for gazebo
pub = rospy.Publisher("/stretch/cmd_vel", Twist, queue_size=1)
declares that your node is publishing to the /stretch/cmd_vel topic using the message type Twist
.
self.sub = rospy.Subscriber('/scan', LaserScan, self.set_speed)
LaserScan
messages. When a message comes in, ROS is going to pass it to the function "set_speed" automatically.
self.width = 1
self.extent = self.width / 2.0
self.distance = 0.5
self.twist = Twist()
self.twist.linear.x = 0.0
self.twist.linear.y = 0.0
self.twist.linear.z = 0.0
self.twist.angular.x = 0.0
self.twist.angular.y = 0.0
self.twist.angular.z = 0.0
Twist
to use, and set everything to zero. We're going to do this when the class is initiating. Redefining this within the callback function, set_speed()
can be more computationally taxing.
angles = linspace(msg.angle_min, msg.angle_max, len(msg.ranges))
points = [r * sin(theta) if (theta < -2.5 or theta > 2.5) else inf for r,theta in zip(msg.ranges, angles)]
new_ranges = [r if abs(y) < self.extent else inf for r,y in zip(msg.ranges, points)]
error = min(new_ranges) - self.distance
self.twist.linear.x = tanh(error) if (error > 0.05 or error < -0.05) else 0
self.pub.publish(self.twist)
Twist
message.
rospy.init_node('avoider')
Avoider()
rospy.spin()
rospy.init_node(NAME, ...)
, is very important as it tells rospy the name of your node -- until rospy has this information, it cannot start communicating with the ROS Master. NOTE: the name must be a base name, i.e. it cannot contain any slashes "/".
Instantiate class with Avioder()
Give control to ROS with rospy.spin()
. This will allow the callback to be called whenever new messages come in. If we don't put this line in, then the node will not work, and ROS will not process any messages.