bat365 Homepage

Generate a ROS Control Plugin from Simulink

This example uses:

This example shows how to generate and build a ros_control plugin from a Simulink® model. In this example, you first configure a model to generate C++ code for a ros_control package. You then deploy the plugin on a virtual machine running Gazebo® to control a Pioneer 3DX 2-wheeled differential drive robot.

Prerequisites

This example requires Simulink Coder™.
Download a virtual machine using instructions in Get Started with Gazebo and Simulated TurtleBot
Review the Feedback Control of a ROS-Enabled Robot and Generate a Standalone ROS Node from Simulink example.
To ensure you have the proper third-party software, see ROS Toolbox System Requirements.

Overview

The ros_control framework provides a generic set of tools to create controllers for various types of robots.

This example focuses on auto-generating C++ code for a controller designed in Simulink® and packaging it and deploying that controller as a ros_control plugin as shown in the figure below. In this example, you generate code for a controller designed in Simulink, and deploy it as a ros_control plugin. The data flow of the controller is shown in the following diagram.

The Simulink model drivecontroller implements a simple closed-loop proportional controller. It receives the goal location from /command topic of type, geometry_msgs/Pose2D, and the current position of the robot from /p3dx/ground_truth/state topic of type, nav_msgs/Odometry. The controller sets the left and right wheel speed commands using VelocityJointInterface. This topology is shown in the diagram below.

Open Controller Simulink Model

After connecting to the ROS network, open the example controller model.

open_system('drivecontroller.slx');

The model implements a proportional controller. At each step, the algorithm orients the robot toward the desired location and drives it forward. Once the desired location is reached, the algorithm stops the robot.

Start ROS Core

In MATLAB®, change the current folder to a temporary location where you have write permission.
ros_control interface setting prepares the model for simulation to ensure that all blocks are properly initialized. This requires a valid connection to a ROS core.
Start the ROS core on the virtual machine using the rosdevice object

d = rosdevice;
runCore(d);

Another roscore / ROS master is already running on the ROS device. Use the 'stopCore' function to stop it.

Use rosinit to connect MATLAB to the ROS master running on the ROS device:

rosinit(d.DeviceAddress,11311)

Initializing global node /matlab_global_node_69673 with NodeURI http://192.168.93.1:49973/ and MasterURI http://192.168.93.144:11311.

Configure ROS Control Interfaces

The example controller model is configured for Robot Operating System (ROS). Use the ros_control Settings button to map the output ports of the model to correct resource type.

In ROS tab, under PREPARE section, click on ROS Control Settings button from the ROS Toolbox category.

In the Configure ROS Control dialog:

Set the name of the Controller C++ class to ControllerHost
In the Outports tab, set the Resource name of the output port 1, to base_right_wheel_joint and set the name of the output port 2, to base_left_wheel_joint.
Set the Resource type for both output ports to VelocityJointInterface.
From the Parameters tab in the dialog, enable Gain (Slider), Linear Velocity (Slider) and Distance Threshold for dynamic reconfigure.
Select Generate ros_control controller package checkbox.
Click OK to save the settings and close the dialog.

The dialog settings are shown below for reference.

Configure Connection to the ROS Device

Under the ROS tab, from the Deploy to drop-down, click Manage Remote Device.
In the Connect to a ROS device dialog that opens, enter all the information that Simulink needs to deploy the ROS node.
Specify IP address or host name of your ROS device, login credentials, and the Catkin workspace.
Change Catkin workspace to ~/Documents/mw_catkin_ws. This workspace has a Pioneer 3DX Gazebo® world and related files to run a simulated differential drive robot.

Deploy the ROS Control Plugin

Generate C++ code for the proportional controller and create a ros_control plugin package, automatically transfer, and build it on the ROS device.

Deploy the ROS Control Plugin

Configure Simulink to use the same ROS connection settings as MATLAB. Under the Simulation tab, in Prepare section, select ROS Network. Set the ROS Master (ROS 1) and Node Host network addresses to Default.
Under the ROS tab, click Deploy > Build & Load. If you get any errors about bus type mismatch, close the model, clear all variables from the base MATLAB workspace, and re-open the model.
Click on the View Diagnostics link at the bottom of the model toolbar to see the output of the build process.
Once the code generation completes, the ROS package is transferred to the Catkin workspace on the ROS device and built there.

Integrate Simulink ROS Control Plugin with Gazebo

Register Controller and Build Workspace

Modify the Pioneer 3DX controller configuration YAML file to register the generated controller, and build the workspace.

Replace the controller configuration YAML file using the rosdevice object. To use the controller plugin generated from the model, copy the pre-configured pioneer3dx.yaml file to the catkin workspace.

system(d,['cd ',d.CatkinWorkspace,' && cp src/pioneer_p3dx_model/p3dx_control/config/pioneer3dx.yaml src/pioneer_p3dx_model/p3dx_control/config/backup.pioneer3dx.yaml'])

ans =

  0×0 empty char array

putFile(d,fullfile(pwd,'pioneer3dx.yaml'),[d.CatkinWorkspace,'/src/pioneer_p3dx_model/p3dx_control/config/pioneer3dx.yaml'])

Build the workspace

system(d,['cd ',d.CatkinWorkspace,' && source ./devel/setup.bash && catkin_make'])

ans = 
    '[  0%] Built target sensor_msgs_generate_messages_eus
     [  1%] Generating dynamic reconfigure files from cfg/controller_params.cfg: /home/user/Documents/mw_catkin_ws/devel/include/drivecontroller/controller_paramsConfig.h /home/user/Documents/mw_catkin_ws/devel/lib/python3/dist-packages/drivecontroller/cfg/controller_paramsConfig.py
     [  1%] Built target std_msgs_generate_messages_nodejs
     [  1%] Built target std_msgs_generate_messages_py
     [  1%] Built target roscpp_generate_messages_cpp
     [  1%] Built target rosgraph_msgs_generate_messages_nodejs
     [  1%] Built target roscpp_generate_messages_eus
     Generating reconfiguration files for ControllerHost in drivecontroller
     Wrote header file in /home/user/Documents/mw_catkin_ws/devel/include/drivecontroller/ControllerHostConfig.h
     [  1%] Built target std_msgs_generate_messages_lisp
     [  1%] Built target roscpp_generate_messages_py
     [  1%] Built target std_msgs_generate_messages_eus
     [  1%] Built target roscpp_generate_messages_nodejs
     [  1%] Built target drivecontroller_gencfg
     [  1%] Built target std_msgs_generate_messages_cpp
     [  1%] Built target rosgraph_msgs_generate_messages_eus
     [  1%] Built target rosgraph_msgs_generate_messages_lisp
     [  1%] Built target roscpp_generate_messages_lisp
     [  1%] Built target rosgraph_msgs_generate_messages_py
     [  1%] Built target tf2_msgs_generate_messages_nodejs
     [  1%] Built target tf2_msgs_generate_messages_lisp
     [  1%] Built target tf2_msgs_generate_messages_eus
     [  1%] Built target actionlib_generate_messages_nodejs
     [  1%] Built target actionlib_generate_messages_lisp
     [  1%] Built target sensor_msgs_generate_messages_nodejs
     [  1%] Built target geometry_msgs_generate_messages_nodejs
     [  1%] Built target tf2_msgs_generate_messages_py
     [  1%] Built target geometry_msgs_generate_messages_cpp
     [  1%] Built target dynamic_reconfigure_generate_messages_cpp
     [  1%] Built target tf_generate_messages_lisp
     [  1%] Built target nav_msgs_generate_messages_cpp
     [  1%] Built target sensor_msgs_generate_messages_lisp
     [  1%] Built target nav_msgs_generate_messages_lisp
     [  1%] Built target geometry_msgs_generate_messages_eus
     [  1%] Built target dynamic_reconfigure_generate_messages_eus
     [  1%] Built target dynamic_reconfigure_gencfg
     [  1%] Built target dynamic_reconfigure_generate_messages_lisp
     [  1%] Built target actionlib_msgs_generate_messages_lisp
     [  1%] Built target dynamic_reconfigure_generate_messages_py
     [  1%] Built target nav_msgs_generate_messages_eus
     [  1%] Built target nav_msgs_generate_messages_nodejs
     [  1%] Built target actionlib_msgs_generate_messages_eus
     [  1%] Built target sensor_msgs_generate_messages_py
     [  1%] Built target geometry_msgs_generate_messages_py
     [  1%] Built target nav_msgs_generate_messages_py
     [  1%] Built target actionlib_generate_messages_eus
     [  1%] Built target actionlib_msgs_generate_messages_cpp
     [  1%] Built target rosgraph_msgs_generate_messages_cpp
     [  1%] Built target actionlib_generate_messages_py
     [  1%] Built target actionlib_msgs_generate_messages_nodejs
     [  1%] Built target geometry_msgs_generate_messages_lisp
     [  1%] Built target actionlib_generate_messages_cpp
     [  1%] Built target actionlib_msgs_generate_messages_py
     [  1%] Built target tf2_msgs_generate_messages_cpp
     [  1%] Built target tf_generate_messages_eus
     [  1%] Built target tf_generate_messages_py
     [  1%] Built target tf_generate_messages_nodejs
     [  1%] Built target dynamic_reconfigure_generate_messages_nodejs
     [  1%] Built target sensor_msgs_generate_messages_cpp
     [  2%] Generating dynamic reconfigure files from cfg/controller_params.cfg: /home/user/Documents/mw_catkin_ws/devel/include/drvctrl_pub/controller_paramsConfig.h /home/user/Documents/mw_catkin_ws/devel/lib/python3/dist-packages/drvctrl_pub/cfg/controller_paramsConfig.py
     [  3%] Generating dynamic reconfigure files from cfg/controller_params.cfg: /home/user/Documents/mw_catkin_ws/devel/include/shape_tracer/controller_paramsConfig.h /home/user/Documents/mw_catkin_ws/devel/lib/python3/dist-packages/shape_tracer/cfg/controller_paramsConfig.py
     Generating reconfiguration files for ControllerHost in drvctrl_pub
     Wrote header file in /home/user/Documents/mw_catkin_ws/devel/include/drvctrl_pub/ControllerHostConfig.h
     [  3%] Built target drvctrl_pub_gencfg
     Generating reconfiguration files for ControllerHost in shape_tracer
     Wrote header file in /home/user/Documents/mw_catkin_ws/devel/include/shape_tracer/ControllerHostConfig.h
     [  3%] Built target _turtlebot_actions_generate_messages_check_deps_TurtlebotMoveActionFeedback
     [  3%] Built target shape_tracer_gencfg
     [  3%] Built target tf_generate_messages_cpp
     [  3%] Built target _turtlebot_actions_generate_messages_check_deps_TurtlebotMoveGoal
     [  3%] Built target _turtlebot_actions_generate_messages_check_deps_FindFiducialFeedback
     [  3%] Built target _turtlebot_actions_generate_messages_check_deps_FindFiducialActionResult
     [  3%] Built target _turtlebot_actions_generate_messages_check_deps_FindFiducialGoal
     [  3%] Built target _turtlebot_actions_generate_messages_check_deps_TurtlebotMoveAction
     [  3%] Built target _turtlebot_actions_generate_messages_check_deps_FindFiducialAction
     [  3%] Built target _turtlebot_actions_generate_messages_check_deps_TurtlebotMoveActionResult
     [  3%] Built target _turtlebot_actions_generate_messages_check_deps_TurtlebotMoveActionGoal
     [  3%] Built target _turtlebot_actions_generate_messages_check_deps_FindFiducialActionGoal
     [  3%] Built target _turtlebot_actions_generate_messages_check_deps_TurtlebotMoveResult
     [  3%] Built target _turtlebot_actions_generate_messages_check_deps_TurtlebotMoveFeedback
     [  3%] Built target _turtlebot_actions_generate_messages_check_deps_FindFiducialActionFeedback
     Scanning dependencies of target drvctrl_pub
     Scanning dependencies of target drivecontroller
     [  4%] Building CXX object drvctrl_pub/CMakeFiles/drvctrl_pub.dir/src/drvctrl_pub_ctrlr_host.cpp.o
     Scanning dependencies of target shape_tracer
     [  5%] Building CXX object drivecontroller/CMakeFiles/drivecontroller.dir/src/drivecontroller_ctrlr_host.cpp.o
     [  5%] Built target _turtlebot_actions_generate_messages_check_deps_FindFiducialResult
     [  6%] Building CXX object shape_tracer/CMakeFiles/shape_tracer.dir/src/shape_tracer_ctrlr_host.cpp.o
     [ 19%] Built target turtlebot_actions_generate_messages_lisp
     [ 31%] Built target turtlebot_actions_generate_messages_nodejs
     [ 45%] Built target turtlebot_actions_generate_messages_py
     [ 58%] Built target turtlebot_actions_generate_messages_cpp
     [ 71%] Built target turtlebot_actions_generate_messages_eus
     [ 71%] Built target turtlebot_actions_gencpp
     [ 71%] Built target turtlebot_actions_generate_messages
     [ 73%] Built target turtlebot_move_action_server
     [ 76%] Built target find_fiducial_pose
     [ 77%] Linking CXX shared library /home/user/Documents/mw_catkin_ws/devel/lib/libdrvctrl_pub.so
     /usr/bin/c++ -fPIC -O3 -DNDEBUG  -shared -Wl,-soname,libdrvctrl_pub.so -o /home/user/Documents/mw_catkin_ws/devel/lib/libdrvctrl_pub.so CMakeFiles/drvctrl_pub.dir/src/drvctrl_pub.cpp.o CMakeFiles/drvctrl_pub.dir/src/drvctrl_pub_ctrlr_host.cpp.o CMakeFiles/drvctrl_pub.dir/src/drvctrl_pub_data.cpp.o CMakeFiles/drvctrl_pub.dir/src/rtGetInf.cpp.o CMakeFiles/drvctrl_pub.dir/src/rtGetNaN.cpp.o CMakeFiles/drvctrl_pub.dir/src/rt_nonfinite.cpp.o CMakeFiles/drvctrl_pub.dir/src/slros_busmsg_conversion.cpp.o CMakeFiles/drvctrl_pub.dir/src/slros_initialize.cpp.o CMakeFiles/drvctrl_pub.dir/src/slros_generic_param.cpp.o  -Wl,-rpath,/home/user/Documents/mw_catkin_ws/install/lib 
     [ 78%] Linking CXX shared library /home/user/Documents/mw_catkin_ws/devel/lib/libdrivecontroller.so
     /usr/bin/c++ -fPIC -O3 -DNDEBUG  -shared -Wl,-soname,libdrivecontroller.so -o /home/user/Documents/mw_catkin_ws/devel/lib/libdrivecontroller.so CMakeFiles/drivecontroller.dir/src/drivecontroller.cpp.o CMakeFiles/drivecontroller.dir/src/drivecontroller_ctrlr_host.cpp.o CMakeFiles/drivecontroller.dir/src/drivecontroller_data.cpp.o CMakeFiles/drivecontroller.dir/src/rtGetInf.cpp.o CMakeFiles/drivecontroller.dir/src/rtGetNaN.cpp.o CMakeFiles/drivecontroller.dir/src/rt_nonfinite.cpp.o CMakeFiles/drivecontroller.dir/src/slros_busmsg_conversion.cpp.o CMakeFiles/drivecontroller.dir/src/slros_initialize.cpp.o CMakeFiles/drivecontroller.dir/src/slros_generic_param.cpp.o  -Wl,-rpath,/home/user/Documents/mw_catkin_ws/install/lib 
     [ 85%] Built target drvctrl_pub
     [ 91%] Built target drivecontroller
     [ 92%] Linking CXX shared library /home/user/Documents/mw_catkin_ws/devel/lib/libshape_tracer.so
     /usr/bin/c++ -fPIC -O3 -DNDEBUG  -shared -Wl,-soname,libshape_tracer.so -o /home/user/Documents/mw_catkin_ws/devel/lib/libshape_tracer.so CMakeFiles/shape_tracer.dir/src/rtGetInf.cpp.o CMakeFiles/shape_tracer.dir/src/rtGetNaN.cpp.o CMakeFiles/shape_tracer.dir/src/rt_nonfinite.cpp.o CMakeFiles/shape_tracer.dir/src/shape_tracer.cpp.o CMakeFiles/shape_tracer.dir/src/shape_tracer_ctrlr_host.cpp.o CMakeFiles/shape_tracer.dir/src/shape_tracer_data.cpp.o CMakeFiles/shape_tracer.dir/src/slros_busmsg_conversion.cpp.o CMakeFiles/shape_tracer.dir/src/slros_initialize.cpp.o CMakeFiles/shape_tracer.dir/src/slros_generic_param.cpp.o  -Wl,-rpath,/home/user/Documents/mw_catkin_ws/install/lib 
     [100%] Built target shape_tracer
     Base path: /home/user/Documents/mw_catkin_ws
     Source space: /home/user/Documents/mw_catkin_ws/src
     Build space: /home/user/Documents/mw_catkin_ws/build
     Devel space: /home/user/Documents/mw_catkin_ws/devel
     Install space: /home/user/Documents/mw_catkin_ws/install
     ####
     #### Running command: "make cmake_check_build_system" in "/home/user/Documents/mw_catkin_ws/build"
     ####
     ####
     #### Running command: "make -j4 -l4" in "/home/user/Documents/mw_catkin_ws/build"
     ####
     '

Start Pioneer 3DX Gazebo World

In the Ubuntu VM, click on Gazebo Pioneer 3DX desktop icon to launch the Gazebo world.
Use rossvcclient to load the controller, sl_drivecontroller

allServices = rosservice('list');
controllerName = 'sl_drivecontroller';
loadSvcName = allServices(endsWith(allServices,'/load_controller'));
switchCtrlSvcName = allServices(endsWith(allServices,'/switch_controller'));
% Load controller
loadCtrlSvc = rossvcclient(loadSvcName{1},'DataFormat','struct','Timeout',10);
loadCtrlMsg = rosmessage(loadCtrlSvc);
loadCtrlMsg.Name = controllerName;
call(loadCtrlSvc,loadCtrlMsg)

ans = struct with fields:
    MessageType: 'controller_manager_msgs/LoadControllerResponse'
             Ok: 1

% Start controller
switchCtrlSvc = rossvcclient(switchCtrlSvcName{1},'DataFormat','struct','Timeout',10);
swtCtrlMsg = rosmessage(switchCtrlSvc);
swtCtrlMsg.StartControllers = {controllerName};
swtCtrlMsg.Strictness = swtCtrlMsg.BESTEFFORT;
call(switchCtrlSvc,swtCtrlMsg)

ans = struct with fields:
    MessageType: 'controller_manager_msgs/SwitchControllerResponse'
             Ok: 1

Alternatively, you can also use the rqt_controller_manager application as shown below:

To change the display name of the controller, sl_drivecontroller, in rqt_controller_manager UI,

Open pioneer3dx.yaml file and modify the structure name.
Follow the steps in Register Controller and Build Workspace to re-build the catkin workspace.
Restart Gazebo and rqt_controller_manager UI to load new configuration.

Send Commands to Move the Robot

To move the Pioneer 3DX robot, open the set_command model and click RUN from SIMULATE section in the SIMULATION tab. Observe the robot move in the Gazebo world.

Tune Controller Parameters with Dynamic Reconfigure

While the above model sends different commands, you can tune the controller parameters Gain (Slider), Linear Velocity (Slider) and Distance Threshold, using dynamic reconfigure framework. Use the following MATLAB code to send updated values for these parameters:

Display Parameter

dynParamSvc = rossvcclient('/p3dx/sl_drivecontroller/set_parameters',DataFormat='struct',Timeout=10);
msg = rosmessage(dynParamSvc);
resp = call(dynParamSvc,msg);
% List the Names and Values
for k=1:numel(resp.Config.Doubles)
    disp(resp.Config.Doubles(k));
end

    MessageType: 'dynamic_reconfigure/DoubleParameter'
           Name: 'GainSlider_gain'
          Value: 5

    MessageType: 'dynamic_reconfigure/DoubleParameter'
           Name: 'LinearVelocitySlider_gain'
          Value: 1

    MessageType: 'dynamic_reconfigure/DoubleParameter'
           Name: 'DistanceThreshold_const'
          Value: 0.5000

for k=1:numel(resp.Config.Ints)
    disp(resp.Config.Doubles(k));
end
for k=1:numel(resp.Config.Strs)
    disp(resp.Config.Doubles(k));
end
for k=1:numel(resp.Config.Bools)
    disp(resp.Config.Doubles(k));
end

Change Linear Velocity

% Set Linear Velocity (Slider) gain to 2
msg.Config.Doubles(1).Name = 'LinearVelocitySlider_gain';
msg.Config.Doubles(1).Value = 2;
call(dynParamSvc,msg)

ans = struct with fields:
    MessageType: 'dynamic_reconfigure/ReconfigureResponse'
         Config: [1×1 struct]

Observe that the linear velocity of the mobile robot has increased.

Change Distance Threshold

% Set Distance Threshold to 0.1 and Linear Velocity to 0.5
msg.Config.Doubles(1).Name = 'LinearVelocitySlider_gain';
msg.Config.Doubles(1).Value = 0.5;
msg.Config.Doubles(2).Name = 'DistanceThreshold_const';
msg.Config.Doubles(2).Value = 0.1;
call(dynParamSvc,msg)

ans = struct with fields:
    MessageType: 'dynamic_reconfigure/ReconfigureResponse'
         Config: [1×1 struct]

With smaller velocity and distance threshold, the robot should be able to go very close to the desired position.

Use rqt_reconfigure Application

Alternatively, you can also change these values from the rqt_reconfigure application on the Virtual Machine as shown in the snapshot below:

Shutdown the global MATLAB node

rosshutdown

Shutting down global node /matlab_global_node_69673 with NodeURI http://192.168.93.1:49973/ and MasterURI http://192.168.93.144:11311.

References

S. Chitta, E. Marder-Eppstein, W. Meeussen, V. Pradeep, A. Rodríguez Tsouroukdissian, J. Bohren, D. Coleman, B. Magyar, G. Raiola, M. Lüdtke and E. Fernandez Perdomo

"ros_control: A generic and simple control framework for ROS"
The Journal of Open Source Software, 2017. PDF