In this section we are going to use a more advanced approach: ROS2 controller. As I mentioned above, connecting ROS2 to Gazebo is not enough: we need to be able, at some point in future, to replace Gazebo simulation with a real world robot. Also, it would be great if this replacement requires changing of just few lines of code.
Now, Gazebo plugin (libgazebo_ros_diff_drive.so) only works with Gazebo. Of course, some hardware manufacturer might create a plugin for a particular hardware, that can be used instead of Gazebo... but they do not do it. The reason is, there are other simulators, like WeBots, Unity, Nvidia's one...
So a universal approach is used instead: a tool based on a control theory, that can be (provided a simulator supports it) plugged in any simulator. This is what ROS2 control is used for.
$ tree src/diff_drive_controller_bot/
src/diff_drive_controller_bot/
├── CMakeLists.txt
├── config
│ │ └── my_controllers.yaml
├── description
│ ├── camera.xacro
│ ├── colors.xacro
│ ├── gazebo_control.xacro <--- REMOVED
│ ├── inertial_macros.xacro
│ ├── lidar.xacro
│ ├── robot_core.xacro
│ ├── robot.urdf.xacro
│ └── ros2_control.xacro <--- ADDED
├── launch
│ ├── launch_sim.launch
│ └── rsp.launch.py
├── meshes
│ └── body.stl
├── package.xml
└── rviz
└── urdf_config.rviz
The source code is available
here.
The base of the code is nearly identical to what we had for
a 2-wheeled robot with Gazebo plug-in. However, we have removed
support for Gazebo plug-in itself (gazebo_control.xacro) and
replaced it with support for ros2_control (ros2_control.xacro).
ros2_control.xacro:
This file has two sections: ros2_control and gazebo.
The ros2_control section declares the hardware we are going
to use: if we switch from simulation to a real robot, this is the
place we are going to change.
Note that to make such a change, we need to either have the hardware
driver (provided by manufacturer) or write it ourselves.
Right now, we use Gazebo as a "hardware".
We also list joints (same as we have in robot's URDF file) that the diff. drive controller is going to control. Currently, ROS2 supports velocity, position and effort command interfaces: I used velocity.
A gazebo section of a file specifies the control library we use and tells it where to look for configuration file (my_controllers.yaml).
This file contains settings that the diff. drive controller will use.
controller_manager:
ros__parameters:
update_rate: 30
use_sim_time: True
diff_cont:
type: diff_drive_controller/DiffDriveController
joint_broad:
type: joint_state_broadcaster/JointStateBroadcaster
diff_cont:
ros__parameters:
publish_rate: 30.0 # You can set this higher than the controller manager update rate, but it will be throttled
base_frame_id: base_link
left_wheel_names: ['left_wheel_joint']
right_wheel_names: ['right_wheel_joint']
wheel_separation: 0.35
wheel_radius: 0.05
use_stamped_vel: false
# --- The rest can be removed ---
wheel_separation_multiplier: 1.0
left_wheel_radius_multiplier: 1.0
right_wheel_radius_multiplier: 1.0
publish_rate: 50.0
odom_frame_id: odom
base_frame_id: base_link
pose_covariance_diagonal : [0.001, 0.001, 0.0, 0.0, 0.0, 0.01]
twist_covariance_diagonal: [0.001, 0.0, 0.0, 0.0, 0.0, 0.01]
open_loop: true
enable_odom_tf: true
cmd_vel_timeout: 0.5
#publish_limited_velocity: true
#velocity_rolling_window_size: 10
# Velocity and acceleration limits
# Whenever a min_* is unspecified, default to -max_*
linear.x.has_velocity_limits: true
linear.x.has_acceleration_limits: true
linear.x.has_jerk_limits: false
linear.x.max_velocity: 1.0
linear.x.min_velocity: -1.0
linear.x.max_acceleration: 1.0
linear.x.max_jerk: 0.0
linear.x.min_jerk: 0.0
angular.z.has_velocity_limits: true
angular.z.has_acceleration_limits: true
angular.z.has_jerk_limits: false
angular.z.max_velocity: 1.0
angular.z.min_velocity: -1.0
angular.z.max_acceleration: 1.0
angular.z.min_acceleration: -1.0
angular.z.max_jerk: 0.0
angular.z.min_jerk: 0.0
## joint_broad:
## ros__parameters:
In this file we provide parameters for diff. drive controller (diff_cont), like distance between wheels, and specify joint state broadcaster (joint_broad). Note that we are going to use these two names in our launch file.
Note the cmd_vel_timeout: 0.5 line: this is how long the command works. So if you don't want your robot to stop after 0.5 seconds, you can increate value of this parameter (not an elegant solution, we better send a new command ourselves).
This is the "main" robot description file that includes all other files. Note that, compared to the previous example, we have switched from gazebo_control.xacro to robot_core.xacro.
I have to admit that my current implementation is a bit messy.
The original idea was to have robot launch file in rsp.launch.py
and robot + Gazebo world launch file in launch_sim.launch
that calls it plus launches the world.
As we want to be able to switch from simulation to real robot and back,
we probably need parameter-based condition in our launch files,
something like
if is_simulation != 'true':
# Use Gazebo as a hardware
else:
# Use real hardware
This is not implemented yet, and I don't thing it is a good idea to add this sort of changes to a simple tutorial.
Currently, the rsp.launch.py starts node_controller and node_robot_state_publisher, but to run the simulation, we also need to create the (mentioned above) joint_broad and diff_cont. Also, the order maters, so I am doing it in a launch_sim.launch.
rsp.launch.py:
import os
from ament_index_python.packages import get_package_share_directory
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.actions import ExecuteProcess, IncludeLaunchDescription, RegisterEventHandler
from launch.event_handlers import OnProcessExit
from launch.conditions import IfCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
import xacro
package_name = 'diff_drive_controller_bot'
def generate_launch_description():
use_sim_time = LaunchConfiguration('use_sim_time')
pkg_path = os.path.join(get_package_share_directory(package_name))
xacro_file = os.path.join(pkg_path,'description','robot.urdf.xacro')
rviz_config_path = os.path.join(pkg_path,'rviz','urdf_config.rviz')
robot_description_config = xacro.process_file(xacro_file)
controller_config = os.path.join(pkg_path, "config", "my_controllers.yaml")
robot_description = {"robot_description": robot_description_config.toxml()}
node_controller = Node(
package="controller_manager",
executable="ros2_control_node",
parameters=[robot_description, controller_config],
output="both",
)
params = {'robot_description': robot_description_config.toxml(), 'use_sim_time': use_sim_time}
node_robot_state_publisher = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
output='screen',
parameters=[params]
)
node_rviz = Node(
package='rviz2',
executable='rviz2',
name='rviz2',
output='screen',
arguments=['-d', LaunchConfiguration('rvizconfig')],
)
# Launch!
return LaunchDescription([
DeclareLaunchArgument(
'use_sim_time',
default_value='True',
description='Use sim time if true'),
DeclareLaunchArgument(name='rvizconfig', default_value=rviz_config_path,
description='Absolute path to rviz config file'),
node_controller,
node_robot_state_publisher,
node_rviz
])
Here we run both our rsp.launch.py and also launch controllers and Gazebo with world file:
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.actions import ExecuteProcess, IncludeLaunchDescription, RegisterEventHandler
from launch.event_handlers import OnProcessExit
from launch_ros.actions import Node
def generate_launch_description():
# Include the robot_state_publisher launch file, provided by our own package. Force sim time to be enabled
# !!! MAKE SURE YOU SET THE PACKAGE NAME CORRECTLY !!!
package_name='diff_drive_controller_bot' #--- CHANGE ME
rsp = IncludeLaunchDescription(
PythonLaunchDescriptionSource([os.path.join(
get_package_share_directory(package_name),'launch','rsp.launch.py'
)]), launch_arguments={'use_sim_time': 'true'}.items()
)
# Include the Gazebo launch file, provided by the gazebo_ros package
gazebo = IncludeLaunchDescription(
PythonLaunchDescriptionSource([os.path.join(
get_package_share_directory('gazebo_ros'), 'launch', 'gazebo.launch.py')]),
)
load_joint_state_controller = ExecuteProcess(
cmd=['ros2', 'control', 'load_controller', '--set-state', 'start',
'joint_broad'],
output='screen'
)
load_diff_drive_base_controller = ExecuteProcess(
cmd=['ros2', 'control', 'load_controller', '--set-state', 'start',
'diff_cont'],
output='screen'
)
# Run the spawner node from the gazebo_ros package. The entity name not really matter if you only have a single robot.
spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py',
arguments=['-topic', 'robot_description',
'-entity', 'diff_drive_controller_bot'],
output='screen')
# Launch them all!
return LaunchDescription([
RegisterEventHandler(
event_handler=OnProcessExit(
target_action=spawn_entity,
on_exit=[load_joint_state_controller],
)
),
RegisterEventHandler(
event_handler=OnProcessExit(
target_action=load_joint_state_controller,
on_exit=[load_diff_drive_base_controller],
)
),
rsp,
gazebo,
spawn_entity,
])
The only difference here is that ROS2 diff drive controller "listens" to a different topic, so if we want to use teleop_twist_keyboard, we have to remap its topic:
# Terminal 1:
$ ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -r /cmd_vel:=/diff_cont/cmd_vel_unstamped
# Terminal 2:
$ cd ~/SnowCron/ros_projects/harsh/
$ colcon build --packages-select fwd_bot
$ source install/setup.bash
$ ros2 launch fwd_bot launch_sim.launch world:=src/worlds/shapes.sdf