Page tree
Skip to end of metadata
Go to start of metadata

This example is an extension of the Mathworks ROS examples and the Husky ROS simulation tutorials. In this example we will...

  • 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.

Development Environment

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.

Source Code

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.

Starting Point

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.

Instructions

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

  1. 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.
  2. 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...

<!-- Here we use remap to change the publication topic for the odometry -->
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization">
  <rosparam command="load" file="$(find husky_control)/config/localization.yaml" />
  <remap from="odometry/filtered" to="odom" />
</node>

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...

<node pkg="twist_mux" type="twist_mux" name="twist_mux">
  <rosparam command="load" file="$(find husky_control)/config/twist_mux.yaml" />
  <remap from="cmd_vel_out" to="husky_velocity_controller/cmd_vel"/>
</node>

The configuration file for the twist_mux node, twist_mux.yaml, starts with something like this...

topics:
- name    : joy
  topic   : joy_teleop/cmd_vel
  timeout : 0.5
  priority: 10
- name    : interactive_marker
  topic   : twist_marker_server/cmd_vel
  timeout : 0.5
  priority: 8
- name    : autonomy
  topic   : platform_control/cmd_vel
  timeout : 0.5
  priority: 2
- name    : external
  topic   : cmd_vel
  timeout : 0.5
  priority: 1

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...

MATLAB Implementation

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.

nre_huskycontrol_simple.m
% Necessary if not already done
%rosinit('127.0.0.1')
% Set waypoint for Husky to drive to
goal = [8,8];  % Goal position in x/y
% Setup publisher
cmdpub = rospublisher('/cmd_vel');
cmdmsg = rosmessage('geometry_msgs/Twist');
% Setup subscription - which implemets our controller.
% We pass the publisher, the message to publish and the goal as 
% additional parameters to the callback function.
odomsub = rossubscriber('/odom',{@huskyOdomCallback,cmdpub,cmdmsg,goal});

This next *.m file defines the callback function.  This is where we do the heavy lifting of implementing the algorithm.  

huskyOdomCallback.m
function huskyOdomCallback(~,message,pub,msg,goal)
% Implementation of proportional position control 
% For comparison to Simulink implementation
% Tunable parameters
wgain = 5; % Gain for the angular velocity [rad/s / rad]
vconst = 1; % Linear velocity when far away [m/s]
distThresh = 0.5; % Distance treshold [m]
% Generate a simplified pose
pos = message.Pose.Pose;
quat = pos.Orientation;
% From quaternion to Euler
angles = quat2eul([quat.W quat.X quat.Y quat.Z]);
theta = angles(1);
pose = [pos.Position.X, pos.Position.Y, theta];  % X, Y, Theta 
% Proportional Controller
v = 0; % default linear velocity
w = 0; % default angluar velocity
distance = sqrt((pose(1)-goal(1))^2+(pose(2)-goal(2))^2);
if (distance > distThresh)
    v = vconst;
    desireYaw = atan2(goal(2)-pose(2),goal(1)-pose(1));
    u = desireYaw-theta;
    bound = atan2(sin(u),cos(u));
    w = min(0.5 , max(-0.5, wgain*bound));
end
% Publish
msg.Linear.X = v;
msg.Angular.Z = w;
send(pub,msg);
fprintf('huskyOdomCallback: x=%4.1f,y=%4.1f dist=%4.2f, cmd.v=%4.2f, cmd.w=%4.2f\n', ...
    pose(1),pose(2),distance, v, w);
end

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.

Python Implementation

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.

nre_simhuskycontrol.py
#!/usr/bin/env python
import rospy, tf
import geometry_msgs.msg, nav_msgs.msg
from math import *
def huskyOdomCallback(message,cargs):
    # Implementation of proportional position control 
    # For comparison to Simulink implementation
    # Callback arguments 
    pub,msg,goal = cargs
    # Tunable parameters
    wgain = 5.0 # Gain for the angular velocity [rad/s / rad]
    vconst = 1.0 # Linear velocity when far away [m/s]
    distThresh = 0.5 # Distance treshold [m]
    # Generate a simplified pose
    pos = message.pose.pose
    quat = pos.orientation
    # From quaternion to Euler
    angles = tf.transformations.euler_from_quaternion((quat.x,quat.y,
                                                       quat.z,quat.w))
    theta = angles[2]
    pose = [pos.position.x, pos.position.y, theta]  # X, Y, Theta 
    
    # Proportional Controller
    v = 0 # default linear velocity
    w = 0 # default angluar velocity
    distance = sqrt((pose[0]-goal[0])**2+(pose[1]-goal[1])**2)
    if (distance > distThresh):
        v = vconst
        desireYaw = atan2(goal[1]-pose[1],goal[0]-pose[0])
        u = desireYaw-theta
        bound = atan2(sin(u),cos(u))
        w = min(0.5 , max(-0.5, wgain*bound))
        
    # Publish
    msg.linear.x = v
    msg.angular.z = w
    pub.publish(msg)
    
    # Reporting
    print('huskyOdomCallback: x=%4.1f,y=%4.1f dist=%4.2f, cmd.v=%4.2f, cmd.w=%4.2f\n'%(pose[0],pose[1],distance,v,w))
########################################
# Main Script
# Initialize our node
rospy.init_node('nre_simhuskycontrol',anonymous=True)
    
# Set waypoint for Husky to drive to
goal = [0,0]  # Goal position in x/y
# Setup publisher
cmdmsg = geometry_msgs.msg.Twist()
cmdpub = rospy.Publisher('/cmd_vel',geometry_msgs.msg.Twist, queue_size=10)
# Setup subscription - which implemets our controller.
# We pass the publisher, the message to publish and the goal as 
# additional parameters to the callback function.
rospy.Subscriber('odom',nav_msgs.msg.Odometry,huskyOdomCallback, 
                 (cmdpub,cmdmsg,goal))
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()