Abstracting Parameters

Abstracting parameters from the code and placing them in a more accessible place is a common practice in software engineering. Without this, configuring codebases to a particular situation would be extremely labor-intensive. In terms of robotics, think about designing a package that can successfully navigate and control ground robots. In order to keep this package general and applicable to multiple ground vehicles, many parameters of the vehicle need to be abstracted, such as the maximum velocity, the maximum steering angle, the size, the sensor capabilities, etc. A good example of a package that is used in robotic navigation is move_base. This package is used in robots such as the PR2, Husky, and Turtlebot all shown below:

In this lab, we will learn how to configure our quadrotor using not only a parameter server but also how to operate on it through the use of services.

Learning Objectives

At the end of this lab, you should understand:

• How to use ROS parameters
• How to implement states within a finite-state automaton
• How to use ROS services

Updating Fastsim

There have been recent updates to the fastsim simulator, and so we recommend you run the following commands to make sure your simulator is up to date.

$ cd ~/fastsim

$ git pull

$ catkin build

This lab will use the pressure sensor, and so we need to add it to the launch file (~/fastsim/src/flightcontroller/launch/fly.launch):

<node name="pressure_sensor" pkg="sensor_simulators" type="pressure.py"/>

To test everything is working, run the simulator and check if the following topic is being published:

rostopic echo /uav/sensors/pressure

Adding a Safety State

To start the lab we will add a safety state to forbid the quadrotor from flying outside of a virtual cage. Verifying that a waypoint is within a virtual cage (a set of bounds) is a good practice as it makes sure that you do not accidentally send a waypoint to the quadrotor that causes it to fly away or crash into a known obstacle. In general, most commands sent to a robot that is going to result in the robot performing some action in the real-world should be verified for the safety of both the people around it and itself.

The first thing we need is to create a virtual cage in such a way that it is easy for the user to change the parameters of the cage. That way, if the user walks into a room of a different size, they can quickly and easily change the cage size without having to change the code.

Parameter Servers

We can do that using a parameter server. The ROS wiki describes the parameter server as: "A parameter server is a shared, multi-variate dictionary that is accessible via network APIs. Nodes use this server to store and retrieve parameters at runtime". In other words, our node can query this parameter server and get the virtual cage dimensions. The benefit of doing this is we can move setting the parameters into the launch file. That way, if we want to change the virtual cage size, all we have to do is change the values in the launch file. This is beneficial as any parameters you want to set get abstracted out of the code. That makes it easier for you to manage the code. It also makes it easier for people using your code as they don't need to understand your implementation to configure the parameters.

You could take this one step further and expose the parameters as command-line arguments, allowing you to pass the parameters in from the command line, making it even easier to change the parameters. However, we won't go into those details in this lab, for more information on this look under the Extra section.

Start by querying the parameter server for the virtual cage size and acceptance range from inside the state_safety_node. Add the following code in the node initialization:

# Get the acceptance range

self.acceptance_range = rospy.get_param("/state_safety_node/acceptance_range", 0.5)

# Getting the virtual cage parameters

cage_params = rospy.get_param('/state_safety_node/virtual_cage', {'x': 5, 'y': 5, 'z': 5})

cx, cy, cz = cage_params['x'], cage_params['y'], cage_params['z']

# Create the virtual cage

self.cage_x = [-1 * cx, cx]

self.cage_y = [-1 * cy, cy]

self.cage_z = [0, cz]

# Display incoming parameters

rospy.loginfo(str(rospy.get_name()) + ": Launching with the following parameters:")

rospy.loginfo(str(rospy.get_name()) + ": Param: cage x - " + str(self.cage_x))

rospy.loginfo(str(rospy.get_name()) + ": Param: cage y - " + str(self.cage_y))

rospy.loginfo(str(rospy.get_name()) + ": Param: cage z - " + str(self.cage_z))

rospy.loginfo(str(rospy.get_name()) + ": Param: acceptance range - " + str(self.acceptance_range))

Let's spend some time understanding how this code works.

First, we changed the acceptance range to be queried from the parameter server. We also give this parameter a default value of 0.5 (in case the parameter can not be found). Next, when we look at the virtual cage, we will notice that the default parameters are a dictionary of values. We abstract these dictionary values to cx, cy, and, cz. The next question you might ask is: "How do we set these values?". Well, if you remember, we set such values in the launch file. To set the parameters in the launch file (~/fastsim/src/flightcontroller/launch/fly.launch) change it as follows:

<node name="state_safety_node" pkg="simple_control" type="state_and_safety.py" output="screen">

  <param name="virtual_cage/x" type="double" value="5" />

  <param name="virtual_cage/y" type="double" value="5" />

  <param name="virtual_cage/z" type="double" value="2" />

  <param name="acceptance_range" type="double" value="0.5" />

</node>

You will notice that each of the parameters we query are now defined inside the launch file.

Verifying that Waypoints are within Cage

The next portion of this lab is to add a safety state to the quadrotor. The safety state will verify the command position and make sure it is inside the cage before transitioning to a moving state. The design for the final state_and_safety node will be as follows:

Note: Remember that in Lab 3 we designed a similar FSA that only had states 1 and 3.

The new design uses an FSA, which works as follows: First when we receive a position message, we transition to a verifying state. The verifying state checks if the published position is inside the virtual cage. If the new position is outside the virtual cage, the position is rejected, and the FSA transitions back into the hovering state. If the position is inside the virtual cage, the position is accepted, and the FSA transitions to the moving state. When the drone reaches the final position, the FSA transitions back to the hovering state, where the next command can be given to the quadrotor.

Update the state_and_safety.py node to implement the extended FSA.

Using ROS Services

For the next section of this lab we will implement two services that allow us to control our quadrotor's behavior. Services, like topics, are another way to pass data between nodes in ROS. Services differ from topics in two key ways, 1) services are synchronous procedure calls, whereas topics are asynchronous communication busses, and 2) services return a response to each request, whereas topics do not. Each service will have a defined input and a defined output format. The node which advertises the service is called a server. The node which calls the service is called the client.

Services are generally used when you need to run functions that require a response. Good scenarios to use a service are:

• When you want to execute discrete actions on a robot, for instance
  • Turning a sensor on or off
  • Taking a picture with a camera
• When you want to execute computations on another node

Implementing a Service

The first service we will be implementing will allow us to "calibrate" the pressure sensor. A pressure sensor returns the air pressure that at sea level is approximately 1,013.25 millibars. This calibration process will zero the pressure sensor by subtracting the baseline air pressure recorded when the service is called. The service will return the baseline pressure to the user.

The first step in implementing a service is to define a service definition file. A service definition file describes the service's input and outputs. Service definition files build directly upon ROS messages to describe the inputs and outputs of the service. The input to our service will be a boolean value from the std_msgs library that describes whether the pressure sensor should be zeroed or not. The output of our service is a float value from the std_msgs library that contains the current baseline pressure reading.

Unlike topic messages, there are no standard libraries for service types. This is because services are dynamic and build upon the existing ROS message types. Due to this, we need to 1) create the service definition file and 2) configure Catkin to build the service definition file. A similar process is done when you want to create custom message types used for topics.

Service definition files are typically put in a directory called srv.

• Start by creating the directory srv inside the sensor_simulators package.
• Inside the srv folder create the service definition file called calibrate.srv

Now, let's add content to the service definition file. Service definition files start with the definition of the input, followed by the definition of output. The input and output are separated using three dashes. Let's add the input and output for our pressure calibration service:

bool zero

---

float64 baseline

The next step is to configure the make files so that when we build our package, all the correct service code and class definitions are created. The service is being created inside the sensor_simulator package. Thus all changes to the Make files need to be done inside this package.

Update the sensor_simulators/CMakeList.txt file to include the message_generation package. message_generation will allow our package to generate the service message.

find_package(catkin REQUIRED COMPONENTS

   rospy

   std_msgs

   geometry_msgs

   message_generation   # Add this line

)

We then need to add the additional package dependencies to the sensor_simulators/package.xml file. Add the following lines inside this file:

<build_depend>message_generation</build_depend>

<build_export_depend>message_runtime</build_export_depend>

<exec_depend>message_runtime</exec_depend>

We then need to define which service message definition files we want to compile. Inside the sensor_simulators/CMakeList.txt package add our service definition file calibrate.srv:

add_service_files(

  FILES

  calibrate.srv

)

Finally, we need to update the message generation dependencies to include std_msgs as we are using a message from that library, namely bool and float64:

generate_messages(

  DEPENDENCIES

  std_msgs

)

Let's now check if we have done everything correctly. It is always a good idea to break long processes like this into smaller steps to allow you to identify early on if you have made a mistake. Let's compile the workspace and check if we can validate that the service call definition exists. To do this run:

$ cd ~/fastsim

$ catkin build

$ source ~/fastsim/devel/setup.bash

$ rossrv show calibrate.srv

>>> bool zero

>>> ---

>>> float64 baseline

If rossrv can find the service definition calibrate.srv you have configured your make files correctly, and catkin build can generate the message files. Catkin automatically generates three type definitions when we add a new service definition file. In general, if you create a service definition file <name>.srv, Catkin will create the following type definitions:

• <name>
• <name>Responce
• <name>Request

Therefore in our case, there are three new types we can reference in python, namely, calibrate, calibrateResponse, and calibrateRequest. A service that handles our calibration process will take in a calibrateRequest and returns a calibrateResponse. Keeping this in the back of our minds, let's move forward and implement the service handler inside the pressure node.

Start by importing the types created by Catkin. The pressure node is the server (receives the service request), and so we only need to import the calibrateResponse. Add the following import to pressure.py:

from sensor_simulators.srv import calibrate, calibrateResponse

Next, let's add the service to this node after the node initialization. This will advertise the service under the name calibrate_pressure. The service will have input and output as defined by the calibrate.srv and will redirect all service requests to the function self.CalibrateFunction:

self.service = rospy.Service('calibrate_pressure', calibrate, self.CalibrateFunction)

Finally, we can define the server function that will handle the service requests. You will notice that inside the pressure.py node, there is a class variable baseline_value that defaults to 0. This function will update that value:

# Saves the baseline value

def CalibrateFunction(self, request):

  # If we want to calibrate

  if request.zero == True:

    self.baseline_value = self.pressure

  else:

    self.baseline_value = 0

  # Return the new baseline value

  return calibrateResponse(self.baseline_value)

Let's again test if we have set the service up correctly. To test whether the service was set up correctly, launch the simulator. The first thing to check is that there are no errors when launching. If there are no errors, proceed to list the available services:

$ rosservice list

>>> /angle_calculator_node/get_loggers

>>> /angle_calculator_node/set_logger_level

>>> ...

You will notice that even for this small simulator, there are a lot of services. A quick way to find what you are looking for is to send all the output text to a grep command, known as piping to grep. grep commands are used for searching for keywords through text. Let's use one now:

$ rosservice list | grep pressure

>>> /calibrate_pressure

>>> /pressure_sensor/get_loggers

>>> /pressure_sensor/set_logger_level

This command will output all services that contain the word pressure. Keep in mind that piping to grep is not specific to ROS and so can be used at any time in Linux. Now that we know that our service is being advertised let's check the type of this service using:

$ rosservice info /calibrate_pressure

>>> Node: /pressure_sensor

>>> URI: rosrpc://robotclass:43965

>>> Type: sensor_simulators/calibrate

>>> Args: zero

We can see that everything is as expected. We can see the Node which provides the service. We can see what message type the service uses and what the input arguments are. We are now ready to test our service. Open three terminals. In the first terminal, we will build and launch the simulator. In the second terminal, we will echo the pressure topic to see if our calibration was successful. In the third terminal, we will call the calibration service.

Terminal 1:

$ source ~/fastsim/devel/setup.bash

$ catkin build

$ roslaunch flightcontroller fly.launch

Terminal 2:

$ source ~/fastsim/devel/setup.bash

$ rostopic echo /uav/sensors/pressure

>>> data: 1013.24774066

>>> ---

>>> data: 1013.24769358

>>> ---

>>> data: 1013.24773901

>>> ...

Terminal 3:

$ source ~/fastsim/devel/setup.bash

$ rosservice call /calibrate_pressure 'True'

>>> baseline: 1013.24756649

If everything was implemented correctly after running the command in terminal 3, you should see that the topic /uav/sensors/pressure is now publishing messages that have been zeroed according to the returned baseline.

Terminal 2:

>>> data: -0.00017854943485

>>> ---

>>> data: -0.000147954746467

>>> ---

>>> data: -0.00016459436074

Implementing your own service

Now use what you have learned to implement a service that will toggle the virtual cage created during Checkpoint 2 on and off. Inside the state_and_safety node in the simple_control package, create a service toggle_cage that has its inputs and outputs defined in the service definition file toggle_cage.srv. This service should take in a boolean input parameter called cage_on and output a boolean parameter success. The service should allow the user to turn the virtual cage on or off and should return whether the call was a success or not (hint: it should always succeed in turning the virtual cage on or off).

Congratulations, you are done with lab 4!

To Check

1. Change the parameters in the launch file to {3,3,3} and 0.25
   1. Show the output from running the simulation
2. Fly the drone inside your virtual cage. Your drone should:
   1. Fly to waypoints inside the virtual cage using commands send via the keyboard.
   2. Reject waypoints that are outside the virtual cage.
   3. You should be able to explain how you implemented the verification state.
3. Turn the virtual cage on or off using the toggle_cage service.
   1. Show that after your virtual cage is turned off, you can fly to a point outside the virtual cage.

Extra:

More information on implementing services.

More information on adding command-line arguments to a launch file.