Issue with libSimpleTrackedVehiclePlugin and ROS control

[lvanasse]

What I'm trying to achieve and what I think might be the issue

Hi,

I'm currently working on the simulation of my student club's tracked robot (Markhor) with Gazebo 9.16 and ROS Melodic. I want to be able to send cmd_vel through the network to a ROS control diff_drive controller and make the robot move.

My issue is, when trying to control a robot using libSimpleTrackedVehiclePlugin, it seems like it doesn't communicate between the ROS controller and the track. This results in the robot not moving.

Also when I'm using the libKeysToCmdVelPlugin the robot moves but I don't want to control it through Gazebo.

I tried to isolate the issue and tested Clearpath's Husky repository which is using the same type of ROS control controller and it worked. Then I tried replacing the tracks on my robot with the wheels used on the husky and it worked.

So now I'm trying to figure out why our robot with the libSimpleTrackedVehiclePlugin is not moving. Since the same ROS control setup works with the husky wheel, I suspect that it might be something with the libSimpleTrackedVehiclePlugin.

How to replicate the issue

Download & packages installation

The repository needs to be inside a workspace. So you might want to create a repository and a src folder:

mkdir -p markhor_ws/src

cd markhor_ws/src

The repository of our robot can be found here: https://github.com/clubcapra/markhor/tree/diff_drive

You just need to download it and place it inside the src folder we just created. Also, the code for this feature is inside the branch diff_drive.

git clone https://github.com/clubcapra/markhor.git

cd markhor

git checkout diff_drive

Once this is done, you can download the dependencies with this command inside the markhor_ws folder:

cd ../..

rosdep install --from-paths src --ignore-src -r -y

Once all the downloads are done, we compile the project, by simply running the catkin_make command inside the markhor_ws folder.

Now we should be good to test for the issue.

Markhor robot with the husky wheels

First, inside a terminal, we need to source the projet

source devel/setup.bash

Then we want to launch the simulation (make sure it is sourced with the project setup script):

roslaunch markhor_gazebo test_world_husky.launch 

Then in a second terminal, we need to launch a cmd_vel node publisher:

rosrun teleop_twist_keyboard teleop_twist_keyboard.py cmd_vel:=/markhor/diff_drive_controller/cmd_vel

Now if you try to use this terminal to move the robot with your keyboard it should move relatively slowly.

Also, everything inside the repository, that has the suffix "_husky" means that it was adapted to use the husky wheels for the simulation.

Markhor robot with the libSimpleTrackedVehiclePlugin tracks

Same thing as for the Markhor robot with the husky wheels, but we only change the launch file name. If you didn't try the simulation with the husky wheels you might need to source the workspace here.

roslaunch markhor_gazebo test_world.launch 

Then in a second terminal, we need to launch a cmd_vel node publisher:

rosrun teleop_twist_keyboard teleop_twist_keyboard.py cmd_vel:=/markhor/diff_drive_controller/cmd_vel

Now if you try to use this terminal to move the robot with your keyboard it shouldn't move. But, if you select the robot by clicking on it inside the simulation and then using your arrow keys to move it, it should work.

How it is organized inside markhor_gazebo

You can find the related files in our repository inside the diff_drive branch: https://github.com/clubcapra/markhor/tree/diff_drive

launch files

test_world.launch

Inside the test_world.launch I'm simply launching Gazebo with the simple.world World.

spawn_markhor.launch

Inside the spawn_markhor I set where the robot will be inside the simulation. Then I launch the control_track.launch file and I set the spawn_model to robot_description argument, which is set inside the control_track.launch.

control_track.launch

In this file, I set the robot_description with the xacro command and the markhor.urdf.xacro file, which is inside the markhor_description folder.

Then I load the config file for the controller and launch the controller manager, which creates the node for the diff_drive controller.

world file

simple.world

This world file is almost a copy of the tracked_vehicule_simple.world file. We just remove the model definition so we could use our own.

Note: Also inside the tracked_vehicle_simple.world there's a comment that says:

   <!--
      A demo world for tracked vehicles.
       Drive the vehicle by keyboard using arrows, or by publishing the
       desired velocity to ~/<vehicle name>/cmd_vel
       (x and yaw command the motion).
   -->

But I've tried it and it doesn't seem to work with the "~/\<vehicle name>/cmd_vel" option, only with the keyboard.

How should I solve the issue

So I'm kind of stuck with this issue and would like to know if I could have any pointers or insights on how to solve it. I've read the code for SimpleTrackedVehiclePlugin (and the files that it inherited), but couldn't find something suspicious relating to my issue.

I'm wondering what are my options, should I try and debug it by compiling and running Gazebo? Or is there something else I could try?

[inaciose]

I come across the same issue. I belive we need to change the plugin source file to subscribe a proper rostopic and recompile the plugin again. The source is at the following url: https://github.com/osrf/gazebo/blob/gazebo11/plugins/TrackedVehiclePlugin.cc

I dont have time for now to understand what i need to change.

[lvanasse]

I have a workaround for now, but I might pick up the issue and propose a fix for it since it's a feature that doesn't really work right now.

The issue seems to be around here : https://github.com/osrf/gazebo/blob/5f17068f47f7c6f2e368a8e3a71a175d043fdd38/plugins/TrackedVehiclePlugin.cc#L150

[inaciose]

The issue seems to be around here :

https://github.com/osrf/gazebo/blob/5f17068f47f7c6f2e368a8e3a71a175d043fdd38/plugins/TrackedVehiclePlugin.cc#L150

Its not only that line its the whole plugin that need to be rewrite, because it is a gazebo plugin, and need to be changed to a ros_gazebo plugin.

Take a look at the differences at the skidSteering plugins

SkidSteer for gazebo https://github.com/osrf/gazebo/blob/gazebo11/plugins/SkidSteerDrivePlugin.cc

SkidSteer for ros_gazebo https://github.com/ros-simulation/gazebo_ros_pkgs/blob/noetic-devel/gazebo_plugins/src/gazebo_ros_skid_steer_drive.cpp

[lprobsth]

Did any of you make progress in adapting the SimpleTrackedVehicle gazebo plugin to a gazebo_ros plugin?

I'm thinking about doing it now. But the implementation process is quite huge and I am not comfortable with the redundancy of reemplementing the code. (why should the plugin exist two times - why work without the ROS interface at all...)

Maybe it's possible to use the existing ignition topics for cmd_vel and tracks_speed in a new bridge that interfaces with ROS cmd_vel and calculates the odometry. I've searched for this kind of plugin and didn't find anything. This would be much easier to implement while the core functionality would remain in the gazebo plugin.

[lvanasse]

I did not try anything for this issue. If you want to give it a shot, it's all yours.

I'm also not sure why the ROS implementation wasn't done in the first place and why it's such an undertaking to make it work with ROS.

Also, I'm not really familiar with the Gazebo architecture to help you there.

Let me know if you happen to have something that working, and I'll try to test it on my end and maybe help you a bit.

[lprobsth]

Sorry for the late update - I'm currently in the last week of my semester project...

Here is the first version of the plugin that bridges to ROS: https://github.com/lprobsth/gazebo_ros_tracked_vehicle_interface

I have tested the package with gazebo11 and ROS melodic. I'm not sure if other version work. (removing gazebo9 and installing gazebo11 can be a little bit tricky - http://gazebosim.org/tutorials?tut=ros_wrapper_versions&cat=connect_ros#UsingaspecificGazeboversionwithROS).

Package dependencies should be installable with rosdep.

After compilation you have to configure the search paths of gazebo - I have the following export in my .bashrc: export GAZEBO_PLUGIN_PATH=$GAZEBO_PLUGIN_PATH:/path/to/catkin_ws/devel/lib/

Update your SDF for usage of the plugin:

    <plugin name='tracked_vehicle_interface' filename='libgazebo_ros_tracked_vehicle_interface.so'>
      <commandROSTopic>/cmd_vel</commandROSTopic>
      <commandIGNTopic>~/[robot_ns or world_ns]/cmd_vel_twist</commandIGNTopic>
      <robotBaseFrame>base_link</robotBaseFrame>
      <tracks_separation>[track separation in meter]</tracks_separation>
      <odometrySource>[encoder,world]</odometrySource>
    </plugin>

[fab-sch]

I have the same issue on ROS2… the control within gazebo works fine. But when I type ros2 topic list, there is no cmd_vel topic active on which I can publish. Does anybody know how to deal with that problem on ROS2 (humble)

Thank you