- Use the <remap> XML tag to interface the published Husky odometry with the existing Simulink control example.
- Adapt the Simulink control example to interface with the "twist_mux" node which passes velocity commands to the Husky.
- Write equivalent controllers in MATLAB and Python for comparison/contrast.
This example uses the Ubuntu 14, ROS Indigo, MATLAB 2015b environment.
For this example we use the a roslaunch to start the Husky simulation, Husky control/estimation nodes and rviz. To use this functionality you need to be able to work with a catkin workspace for custom packages. This skill is well documented in the ROS Basic Tutorials.
The ROS launch file, Simulink control model, MATLAB controller and Python controller are all available as an ROS package called nre_simhuskycontrol (https://github.com/bsb808/nre_simhuskycontrol). You can clone the repository using git or you can download a zip archive of the current source.
To get started you will want to review this examples/tutorials that we will be extending:
- The Mathworks example Feedback Control of a ROS-enabled Robot. This example shows how to use Simulink to control a virtual turtlebot in the Gazebo simulation environment. We'll use this Simulink controller as the basis for our Husky controller.
- The Husky ROS example Interfacing with Husky. This tutorial will make sure you have the appropriate husky-specific ROS packages installed. You should be able to use rviz to drive your Husky around and see the results in Gazebo.
Adapting Simulink Model for the Husky
The Feedback Control of a ROS-enabled Robot example illustrates how to use ROS subscription and publishing to close-the-loop for a simulated robot. Our first tasks is to make the necessary changes to allow this same Simulink model to interact with our Husky simulation. There are two communication issues
- The Simulink model from the example subscribes to the "/odom" topic for odometry messages, but the Husky publishes these messages on the "/odometry/filtered" topic. We'll adapt the Husky implementation using the <remap> tag in the launch file.
- The Simulink model from the example publishes velocity commands to the "/mobile_base/commands/velocity" topic, but the Husky uses the twist_mux node to arbitrate various velocity command inputs and publish a final velocity command on the "husky_velocity_controller/cmd_vel" topic.
This image shows a subset of the rqt_graph plot from running the turtlebot Simulink control example. The feedback loop is highlighted: The Simulink node receives position updates on the /odom topic and then published velcoity commands on the /mobile_base/commands/velocity topic.
The resulting Simulink model is the same as the Mathworks example:
The Command Velocity Publisher block looks like this...
Remapping Husky Odometry.
In the standalone Husky tutorial you launched the simulation using a command similar to...
roslaunch husky_gazebo husky_empty_world.launch
If you examine this launch file (husky_empty_world.launch) you will see that it sources a bunch of other XML files which in turn use another level of XML launch files. Untangling this hierarchy can be a bit tedious and makes it difficult to customize. In the source code for our example we've flattened this launch file (huskycontrol.launch) considerably so that all the steps are more exposed. You should read through the launch file (see source code)- you may be surprised at how many things are done.
In the huskycontrol.launch file you see a snippet like this...
The <remap> tag is the part we use to adapt our Husky simulation. By remapping the publications of the ekf_localization node we force the messages to appear on the /odom topic. This is what our Simulink model expects and is the input to our control algorithm.
Husky Velocity Inputs
The second step is to change the output of our Simulink model to be able to talk to the simulated Husky robot. This image shows a subset of the rqt_graph generated by running the standalone Husky tutorial. Here I used the rviz and the twist_marker_server to provide open-loop velocity commands to the Husky.
What you see is that the twist_mux node subscribes to the geometry_msgs/Twist messages on the /twist_marker_server/cmd_vel topic and publishes geometry_msgs/Twist messages to the /husky_velocity_controller/cmd_vel topic which go to the husky_velocity_controller node. The twist_mux package is well documented.
In the launch file we can find that the twist_mux node is called with a configuration file parameter. That snippet of the huskycontrol.launch file looks like...
The configuration file for the twist_mux node, twist_mux.yaml, starts with something like this...
This shows us what topics we can publish to and their relative priority. For this example we'll publish to the "cmd_vel" topic which has the lowest priority. To accomplish this we open the Simulink subsystem that does the publishing and change the topic of the ROS Publish bock to "/cmd_vel" so that it looks like this...
As an alternative to the Simulink model we could implement the same functionality using MATLAB. The code consists of two *.m files which are available in the source code (https://github.com/bsb808/nre_simhuskycontrol).
The high-level architecture of the program is to define a "MATLAB Callback". This is a function that is called when MATLAB receives a ROS message on the /odom topic, i.e., when the husky has a new estimate of its position this function is called. Within that function we implement the same algorithm as done in the Simulink model and then publish a velocity message to tell the husky what to do.
The first file is a script to define our position goal - where to drive the husky - and then subscribe to the /odom topic with a callback function.
This next *.m file defines the callback function. This is where we do the heavy lifting of implementing the algorithm.
You should be able to run the example in two steps. First - start the simulated Husky...
roslaunch nre_simhuskycontrol huskycontrol.launch
Second - in MATLAB run the script nre_huskycontrol_simple.m
You should see a bunch of messages in the MATLAB command window and the Husky in Gazebo drive to the spot.
As an alternative to the Simulink model we could implement the same functionality using Python and rospy. The code consists a single .py file which are available in the source code (https://github.com/bsb808/nre_simhuskycontrol).
I've tried to replicated the high-level architecture of the MATLAB implementation for the purposes of comparison: the script sets up a subscriber that listens on the odom topic and then calls the control algorithm when it receives odometry messages. The callback function implements the algorithm. Below is a listing of the code, but check the source repository for the latest.