Debugging ROS2 Gazebo Plugins With VSCode
When working with Gazebo Simulation, there could be a need to debug it’s plugins and ROS2 code to root cause issues and bugs. This article is a tutorial on setting up interactive debugging gazebo plugins using Microsoft VSCode. ROS2 Foxy used but the method can be used with any ROS version.
Assumptions:
- Familiarity with MS VSCode.
- Familiarity with gdb and it’s usage.
- Plugin source code availability and it’s building with debug symbols. See this reference tutorial.
Gazebo runs in a client and server process. All the physics logic is carried out by service process named gzserver. It also host plugins dynamic libraries. The client (gzclient) is the front end counter part and responsible for rendering physics state on the screen using 3D libraries. This separations allows to run gzclient and gzserver on separate machines thus get better results for actual simulation and not effected by 3D rendering. The gzserver is alone enough to run off display simulation.
gazebo command is basically wrapper executable to run gzserver and gzclient processes.
https://classic.gazebosim.org/tutorials?tut=architecture&cat=get_started
Gazebo Plugin Types
Here is brief intro of gazebo plugins.
enum PluginType
{
/// \brief A World plugin
WORLD_PLUGIN,
/// \brief A Model plugin
MODEL_PLUGIN,
/// \brief A Sensor plugin
SENSOR_PLUGIN,
/// \brief A System plugin
SYSTEM_PLUGIN,
/// \brief A Visual plugin
VISUAL_PLUGIN,
/// \brief A GUI plugin
GUI_PLUGIN
};World plugins are declared and loaded via world file. Model plugin life time depends on Model existing in a world. Sensor plugins are to simulate camera and often associated with model. System plugins are loaded by the gzserver at process start even before any world or model is created.
Compiling Plugin with Debug Symbols
Interactive debugging requires debug symbols to be included in the binary as well as with minimal optimizations. Too much compiler optimization effect debugger’s ability to relate code with instructions. Best scenario for debugging would be to build binaries with -O0 flag (no optimization). -g flag instructs compiler to embed debug symbol into the executable.
Colcon command for building ROS2 package with debug symbols and minimal optimization.
colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=DebugCMAKE_BUILD_TYPE options. Can use any of the bold options.
1. Release: `-O3 -DNDEBUG`
2. Debug: `-O0 -g`
3. RelWithDebInfo: `-O2 -g -DNDEBUG`
4. MinSizeRel: `-Os -DNDEBUG`Best config for debugging will be Debug or RelWithDebInfo. Though -O0 may cause some performance degradation depending on code sensitivity. In rare cases, building code without optimization may hide the bugs related to synchronization.
Setting up VSCode for plugin debugging
Create a launch.json file in .vscode directory inside workspace. This can be done directly from command prompt or via VSCode GUI.
Replace any existing contents with below snippet.
launch.json file
{
"version": "0.2.0",
"configurations": [
{
"name": "(gdb) Attach",
"type": "cppdbg",
"request": "attach",
"processId": "${input:GetPID}",
"program" : "${input:GetPath}",
"MIMode": "gdb",
"sudo": true,
"setupCommands": [
{
"description": "Enable pretty-printing for gdb",
"text": "-enable-pretty-printing",
"ignoreFailures": true
},
{
"description": "Set Disassembly Flavor to Intel",
"text": "-gdb-set disassembly-flavor intel",
"ignoreFailures": true
}
]
}
],
"inputs": [
{
"id": "GetPID",
"type": "command",
"command": "shellCommand.execute",
"args": {
"command": "pgrep gzserver",
"description": "Select your target PID",
"useFirstResult": true,
}
},
{
"id": "GetPath",
"type": "command",
"command": "shellCommand.execute",
"args": {
"command": "readlink /proc/$(pgrep gzserver)/exe",
"description": "Select your target PID",
"useFirstResult": true,
}
}
]
}GetPID will return pid of gzserver process. GetPath is to get executable path. pid and path both are needed by launch.json to attach gdb with running process. (e.g gzserver)
Debugging plugins
A typical scenario would be to start simulation and then attach VSCode debugger to gzserver process. The code to be inspect get called after the process is attached (e.g spawn_entity). This is a straight forward scenario with below steps to follow. This method can be used for debugging any function code either one time or repeated calls. e.g Load(), Update()
Steps:
- Start gazebo
- Set break point on desired plugin function
- Attach to gzserver via vscode launch.json file
- Import model containing plugin reference into gazebo via spawn_entity.py script.
- Breakpoint should get hit once the function is called.
- Continue with Watch and other debugging steps
spawn_entity command
ros2 run gazebo_ros spawn_entity.py -file <path>/model.sdf -entity robot -x 0.0 -y 0.0 -z 0.0
Scenario 2
Debugging plugin startup code loaded with gzserver
There are situations when the problematic code is in the initialization calls and cause crash or passes to quickly even before debugger given chance to attach. In this situation, either can run gazebo under gdb or put a temporary delay via sleep in startup routines to cause plugin to wait for debugger to attach. This wait can end either using a variable value set or using a temp file check.
Running gzserver under gdb. A crash will get trapped by gdb (e.g assert, a real crash). Debugging can continue from there.
ros2 launch gazebo_ros gzserver.launch.py gdb:=trueThere are situations when the plugin is loaded with gzserver and there is a need to debug plugin startup code.
- Command line arguments (e.g -s option for system plugins).
- Referenced inside .world file
- Model is spawned within launch file
A work around for this scenario would be to force plugin to wait for debugger to attach before proceeding. This can be achieved by introducing a while wait loop at start of Load() or Constructor() calls.
There could be multiple ways to wait. Here two of them are listed.
Wait loop can wait on a file on local system. After ros2 launch, the gzserver will be blocked by the plugin due to below loop. This will give the opportunity to attach VS Code to gzserver and wait for symbols to get loaded, set a breakpoint on desired location and then create the file via ‘touch /tmp/go’ command to proceed. Assuming DEBUG_ON preprocessor flag is set to 1.
Using file creation:
#include <sys/stat.h>
...
...
#if DEBUG_ON
struct stat buffer;
while (stat ("/tmp/go", &buffer) != 0) {
sleep(1); // sleep for 1 seconds. can replace with usleep
}
#endiftouch /tmp/goAnother way to achieve same behavior is using a variable. In this case, set a breakpoint on sleep statement. Once the VSCode is attached to gzserver, this breakpoint will get hit. Change the i value to something other than 0 causing while loop to exit. Assuming real target breakpoint is already set which will get hit after the while loop is done.
Using variable update:
#if DEBUG_ON
int i = 0;
while (i == 0) {
sleep(1); // sleep for 1 seconds. can replace with usleep
}
#endifVSCode offers all standard debugger GUI features including Watch and Variable window, Call Stack and Breakpoint set/remove options.
References: