Walkthrough: ROS Parameters and the wanderbot

1. The message constructor sets all fields to zero by default
2. This makes the TurtleBot drive in a straight line forward at 0.25 \(m/s\).
3. Publish the command velocities to achieve the two robot states we want
4. Checks the time and toggle between the red and green light
5. This allows us to change states after three seconds

• First start the TurtleBot simulator with roslaunch turtlebot_gazebo turtlebot_world.launch, and then in another terminal run the red_light_green_light.py node, and in a third terminal run rostopic list. Using tools like rostopic info and rqt_graph we can discover that the simulator is subscribing to the /cmd_vel_mux/input/teleop topic.
• If we run rostopic type /cmd_vel_mux/input/teleop we see that it's of type geometry_msgs/Twist. The TurtleBot expects command velocities on that topic, however we've named our topic in the node cmd_vel. To get our cmd_vel to publish to the right topic we will remap it. Remapping allows us to, at runtime, change what topics and services a node is looking for and providing.
• In order to remap the /cmd_vel topic, we can use a command like rosrun wanderbot red_light_green_light.py cmd_vel:=cmd_vel_mux/input/teleop We see that the TurtleBot moves for three seconds, then stops for three. If we want to see what this looks like in Gazebo we run roslaunch turtlebot_gazebo turtlebot_world.launch like we did in the location monitor example in the previous Walkthrough.
• This is a pretty dumb robot. It just moves and stops and doesn't care about the world around it. Technically you can argue it's not a robot because it's only computing and actuating while not sensing anything. We'll change that in the next section. The TurtleBot comes equipped with a Mirosoft Kinect. The Kinect is capable of producing full 3D representations of data called point clouds. Point clouds are a bit too complicated for right now. So we will instead work with a small fraction of the data. The TurtleBot drivers publish a narrow horizontal slice of the depth data as a sensor_msgs/LaserScan message on the scan topic. The form of this data is similar to the data that you'd get from a laser scanner. It differs from real laser scans by having a smaller maximum range and more limited field of view.

1. Import the LaserScan message since we'll be using it
2. These are the two variables we'll use for our controller logic, driving state and time
3. This stores the minimum range our simulated laser scanner detects. We could decide to use the middle element of the ranges array instead with range_ahead = msg.ranges[len(msg.ranges)/2] . Alternatively we could use msg.range_min to use the minimum value the laser scanner is capable of sensing (assuming our driver has properly set this). Think about how these choices would affect the TurtleBot's behavior?
4. Set the command velocities for moving forward at 0.2 \(m/s\)
5. If there's no obstacle within 1.2m or the robot has been moving for 10 seconds, stop moving.
6. Stop moving for 5 seconds.
7. Spin on the spot for 5 seconds
8. Check that 5 seconds have elapsed
9. Change states after 10 seconds
10. Publish the messages

• Don't forget to remap the cmd_vel topic again.
• Inspect the sensor_msgs/LaserScan message and /scan topic. Can you understand what all the fields in the message mean?
• Can you think of a better algorithm for the TurtleBot to explore it's environment without hitting anything or driving off into nowhere?
• Try adding a ROS Timer to periodically read the robot's state (spinning vs moving), and print out the robot's state to the screen when a state change is detected. Also calculate how many seconds elapsed before the robot changed state.

A parameter server is a shared, multi-variate dictionary that is accessible via network APIs… this simply means it's a fancy dictionary. Nodes use the server to store and retrive parameters while running. Which means you can tweak values of a parameter and have multiple instances of the same node with differing functionalities. The example on the ROS wiki uses a situation where you want to give different cameras different exposure settings but still use the same driver node.

The parameter server can store strings, integers, floats, booleans, lists, dictionaries, iso8601 dates, and base64-encoded data. One thing to note is that dictionaries must have string keys. Parameters must be actively queried by the nodes that are interested in their values, they are most suitable for configuration information that will not change (much) over time. If you are interested in having your node automatically run a callback when a parameter changes value (instead of constantly polling), you should look into dynamic_reconfigure. Parameters may be defined in a YAML file and then loaded to the parameter server with rosparam load or via the rosparam tag in launch files.

rosparam is a command-line tool for getting, setting, and deleting parameters from the ROS Parameter Server. It also enables loading and dumping Parameter Server state to a file.

Commands:
    rosparam set    set parameter
    rosparam get    get parameter
    rosparam load   load parameters from file
    rosparam dump   dump parameters to file
    rosparam delete delete parameter
    rosparam list   list parameter names

Often in the ROS world, you'll see nodes use private parameters. A private parameter is still publicly accessible, like all parameters, however its fully-qualified name is formed by prepending the node's name to the parameter's name. By always having the name of the node come before the parameter value, it helps end users sort out which nodes are likely to be looking at a given parameter.

There are several ways of setting variables to initial values through the use of the parameter server. One is through a launch file, and another is from the command line. We set parameters at the command line at runtime using the := syntax. The ROS node below illustrates the concept of using private parameters.

This is a simple ROS node that logs the first key stroke from the keyboard and publishes them to the keys topic. We'll be subscribing to it and using it's output to teleoperate the TurtleBot.

#!/usr/bin/env python
import sys,select,tty,termios
import rospy
from std_msgs.msg import String

if __name__=='__main__':
    rospy.init_node("keyboard_driver")
    key_pub = rospy.Publisher('keys', String, queue_size=1)
    rate = rospy.Rate(100)
    #save attributes
    old_attr = termios.tcgetattr(sys.stdin)
    tty.setcbreak(sys.stdin.fileno())
    print "Publishing keystrokes. Press Ctrl-C to exit..."
    while not rospy.is_shutdown():
        if select.select([sys.stdin],[],[],0)[0] == [sys.stdin]:
            key_pub.publish(sys.stdin.read(1))
        rate.sleep()
    #Reset console to standard mode
    termios.tcsetattr(sys.stdin,termios.TCSADRAIN,old_attr)

In this ROS node we subscribe to the keys topic and use it to teleoperate the TurtleBot. We also scale and ramp the velocities and accelerations being sent to the TurtleBot

#!/usr/bin/env python
import rospy
import math
from std_msgs.msg import String
from geometry_msgs.msg import Twist

key_mapping = {'w':[0,1],'a':[1,0],'s':[0,-1],'d':[-1,0],'x':[0,0]} #1

class KeysTwistWithRamp(object):
    '''
    Ramp Motion Commands
    '''
    def __init__(self):
        rospy.init_node('keys_to_twist_with_ramps')
        self.g_twist_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)
        self.g_last_twist = Twist()
        self.g_target_twist = Twist()
        self.g_last_twist_send_time = rospy.Time.now()
        self.rate = rospy.Rate(20) #2
        self.g_vel_scales = [0.1, 0.1]  #3
        self.g_vel_ramps = [1,1] #4

    def ramped_vel(self, v_prev, v_target, t_prev, t_now, ramp_rate):
        step = ramp_rate * (t_now - t_prev).to_sec() #5
        sign = 1.0 if (v_target > v_prev) else -1.0
        error = math.fabs(v_target-v_prev)
        if error < step: #6
            return v_target
        else:
            return v_prev + sign * step #7

    def ramped_twist(self, prev, target, t_prev, t_now, ramps): #8
        tw = Twist()
        tw.angular.z = self.ramped_vel(prev.angular.z, target.angular.z, t_prev, t_now, ramps[0])
        tw.linear.x =  self.ramped_vel(prev.linear.x, target.linear.x, t_prev, t_now, ramps[1])
        return tw

    def send_twist(self): #9
        t_now = rospy.Time.now()
        self.g_last_twist = self.ramped_twist(self.g_last_twist,self.g_target_twist,self.g_last_twist_send_time,t_now,self.g_vel_ramps)
        self.g_last_twist_send_time = t_now #10
        self.g_twist_pub.publish(self.g_last_twist)

    def keys_callback(self, msg):
        if len(msg.data) == 0 or not key_mapping.has_key(msg.data[0]):
            return  #11
        vels = key_mapping[msg.data[0]] #12
        self.g_target_twist.angular.z = vels[0] * self.g_vel_scales[0]
        self.g_target_twist.linear.x = vels[1] * self.g_vel_scales[1]

    def fetch_param(self,name,default): #13
        if rospy.has_param(name): #14
            return rospy.get_param(name)
        else:
            rospy.logwarn("parameter %s not defined. Defaulting to %.3f" % (name, default)) #15
            return default


def main():
    key_pub_rate = KeysTwistWithRamp()
    rospy.Subscriber('keys', String, key_pub_rate.keys_callback) #16
    key_pub_rate.g_vel_scales[0] = key_pub_rate.fetch_param('~angular_scale', 0.1)
    key_pub_rate.g_vel_scales[1] = key_pub_rate.fetch_param('~linear_scale', 0.1)
    key_pub_rate.g_vel_ramps[0]  = key_pub_rate.fetch_param('~angular_accel', 1.0)
    key_pub_rate.g_vel_ramps[1]  = key_pub_rate.fetch_param('~linear_accel', 1.0)

    while not rospy.is_shutdown():
        key_pub_rate.send_twist()
        key_pub_rate.rate.sleep()


if __name__=='__main__':
    main()

1. A dictionary that maps key presses to movement
   • w: Forward,
   • s: Backwards,
   • x: stop key
   • a: rotate clockwise,
   • d: anti-clockwise
2. Publish the Twist message to the topic at 20Hz
3. Initialize the scale at a small value. Scaling in theory allows us to use this car on multiple platforms. 1 \(m/s\) is fast for a TurtleBot, however it's slow for cars or a bigger mobile platforms.
4. Units: \(m/s^2\)
5. Convert time to seconds
6. We can get there within this timestep we're done
7. Take a step toward the target
8. In this function we instantiate a new twist. We then ramp the velocities coming in and split it into linear and angular velocities and return the new twist with the ramped velocities.
9. In this function,the ramp is applied to the twist message being published
10. Reset current time for next function call
11. Return nothing if the key pressed isn't in key_mapping
12. Take the first key pressed as the key for the dictionary and assign the value to vels. Remember the dictionary command velocities are stored as a list
13. A function to get parameter names or use default values
14. Check for the existence of parameters and fetch parameter value from parameter server if they exist
15. Warn if we don't have any parameters and return the default value we specify. logwarn() is a ROS logging cal that prints colourized text to the screen which is useful in debugging. Other logging calls are loginfo(), logwarn(), logerr() and logfatal()
16. Subscribe to the keys topic and set the scale and ramp values to the parameter values(either the ones we specify or the default ones)

To make everything easier to start, we are going to use a launch file. Create a directory in your package called launch/. Then create a file called keys_to_twist_with_ramps.launch and fill it in with the content below. Once done, you'll be able to launch the file with roslaunch wanderbot keys_to_twist_with_ramps.launch. Note that this launch file defaults to starting the simulator as well. If you want this behavior, be sure to stop any already-running Gazebo instances you have. If you'd like to prevent this launch file from starting the simulator, pass sim:=false at the end of your roslaunch invocation.

Also note that the launch-prefix="xterm -e" attribute is used when starting the key_publisher.py node. This causes a new window to pop up running the terminal emulator xterm. We do this because it is convenient to be able to have a separate window for parsing just your keyboard commands. We use xterm instead of the default terminal emulator (which is called gnome-terminal) because it is a basic terminal emulator that starts up very quickly. Note that for this line to work you'll need to have xterm installed. You could check if it is installed with dpkg -l |grep xterm. If it is not installed, you can install it with sudo apt install xterm.

In another terminal run rqt_plot cmd_vel_mux/input/teleop/linear/x cmd_vel_mux/input/teleop/angular/z to see how the velocities are modulated.