From 978ae25202987a91ef66ea0b666078f077bed26e Mon Sep 17 00:00:00 2001 From: Saitejabojja07 Date: Mon, 30 Mar 2026 01:00:36 +0530 Subject: [PATCH 1/3] SpeedLimiter before odometry update --- .vscode/c_cpp_properties.json | 18 +++++ .vscode/launch.json | 24 +++++++ .vscode/settings.json | 68 +++++++++++++++++++ .../src/diff_drive_controller.cpp | 19 +++--- 4 files changed, 119 insertions(+), 10 deletions(-) create mode 100644 .vscode/c_cpp_properties.json create mode 100644 .vscode/launch.json create mode 100644 .vscode/settings.json diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json new file mode 100644 index 0000000000..ad8ab09bd3 --- /dev/null +++ b/.vscode/c_cpp_properties.json @@ -0,0 +1,18 @@ +{ + "configurations": [ + { + "name": "linux-gcc-x64", + "includePath": [ + "${workspaceFolder}/**" + ], + "compilerPath": "/usr/bin/gcc", + "cStandard": "${default}", + "cppStandard": "${default}", + "intelliSenseMode": "linux-gcc-x64", + "compilerArgs": [ + "" + ] + } + ], + "version": 4 +} diff --git a/.vscode/launch.json b/.vscode/launch.json new file mode 100644 index 0000000000..d9e9b0d8ec --- /dev/null +++ b/.vscode/launch.json @@ -0,0 +1,24 @@ +{ + "version": "0.2.0", + "configurations": [ + { + "name": "C/C++ Runner: Debug Session", + "type": "cppdbg", + "request": "launch", + "args": [], + "stopAtEntry": false, + "externalConsole": false, + "cwd": "/home/sai/saiteja/ros2_controllers/diff_drive_controller/src", + "program": "/home/sai/saiteja/ros2_controllers/diff_drive_controller/src/build/Debug/outDebug", + "MIMode": "gdb", + "miDebuggerPath": "gdb", + "setupCommands": [ + { + "description": "Enable pretty-printing for gdb", + "text": "-enable-pretty-printing", + "ignoreFailures": true + } + ] + } + ] +} diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000000..ee1337ac5c --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,68 @@ +{ + "C_Cpp_Runner.cCompilerPath": "gcc", + "C_Cpp_Runner.cppCompilerPath": "g++", + "C_Cpp_Runner.debuggerPath": "gdb", + "C_Cpp_Runner.cStandard": "", + "C_Cpp_Runner.cppStandard": "", + "C_Cpp_Runner.msvcBatchPath": "", + "C_Cpp_Runner.useMsvc": false, + "C_Cpp_Runner.warnings": [ + "-Wall", + "-Wextra", + "-Wpedantic", + "-Wshadow", + "-Wformat=2", + "-Wcast-align", + "-Wconversion", + "-Wsign-conversion", + "-Wnull-dereference" + ], + "C_Cpp_Runner.msvcWarnings": [ + "/W4", + "/permissive-", + "/w14242", + "/w14287", + "/w14296", + "/w14311", + "/w14826", + "/w44062", + "/w44242", + "/w14905", + "/w14906", + "/w14263", + "/w44265", + "/w14928" + ], + "C_Cpp_Runner.enableWarnings": true, + "C_Cpp_Runner.warningsAsError": false, + "C_Cpp_Runner.compilerArgs": [], + "C_Cpp_Runner.linkerArgs": [], + "C_Cpp_Runner.includePaths": [], + "C_Cpp_Runner.includeSearch": [ + "*", + "**/*" + ], + "C_Cpp_Runner.excludeSearch": [ + "**/build", + "**/build/**", + "**/.*", + "**/.*/**", + "**/.vscode", + "**/.vscode/**" + ], + "C_Cpp_Runner.useAddressSanitizer": false, + "C_Cpp_Runner.useUndefinedSanitizer": false, + "C_Cpp_Runner.useLeakSanitizer": false, + "C_Cpp_Runner.showCompilationTime": false, + "C_Cpp_Runner.useLinkTimeOptimization": false, + "C_Cpp_Runner.msvcSecureNoWarnings": false, + "ROS2.distro": "humble", + "python.autoComplete.extraPaths": [ + "/opt/ros/humble/lib/python3.10/site-packages", + "/opt/ros/humble/local/lib/python3.10/dist-packages" + ], + "python.analysis.extraPaths": [ + "/opt/ros/humble/lib/python3.10/site-packages", + "/opt/ros/humble/local/lib/python3.10/dist-packages" + ] +} diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 8511486cec..8b798f0d90 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -164,6 +164,15 @@ controller_interface::return_type DiffDriveController::update_and_write_commands const double right_wheel_radius = params_.right_wheel_radius_multiplier * params_.wheel_radius; bool odometry_updated = false; + double & last_linear = previous_two_commands_.back()[0]; + double & second_to_last_linear = previous_two_commands_.front()[0]; + double & last_angular = previous_two_commands_.back()[1]; + double & second_to_last_angular = previous_two_commands_.front()[1]; + + limiter_linear_->limit(linear_command, last_linear, second_to_last_linear, period.seconds()); + limiter_angular_->limit(angular_command, last_angular, second_to_last_angular, period.seconds()); + previous_two_commands_.pop(); + previous_two_commands_.push({{linear_command, angular_command}}); // check if odometry set or reset was requested by non-RT thread if (set_odom_requested_.load()) @@ -289,16 +298,6 @@ controller_interface::return_type DiffDriveController::update_and_write_commands } } - double & last_linear = previous_two_commands_.back()[0]; - double & second_to_last_linear = previous_two_commands_.front()[0]; - double & last_angular = previous_two_commands_.back()[1]; - double & second_to_last_angular = previous_two_commands_.front()[1]; - - limiter_linear_->limit(linear_command, last_linear, second_to_last_linear, period.seconds()); - limiter_angular_->limit(angular_command, last_angular, second_to_last_angular, period.seconds()); - previous_two_commands_.pop(); - previous_two_commands_.push({{linear_command, angular_command}}); - // Publish limited velocity if (publish_limited_velocity_ && realtime_limited_velocity_publisher_) { From 77397b8074215beab2ad14e289976db7423f8545 Mon Sep 17 00:00:00 2001 From: Sai Teja Bojja <128970853+Saitejabojja07@users.noreply.github.com> Date: Mon, 30 Mar 2026 09:19:29 +0530 Subject: [PATCH 2/3] Deleted .vscode directory --- .vscode/c_cpp_properties.json | 18 ---------- .vscode/launch.json | 24 ------------- .vscode/settings.json | 68 ----------------------------------- 3 files changed, 110 deletions(-) delete mode 100644 .vscode/c_cpp_properties.json delete mode 100644 .vscode/launch.json delete mode 100644 .vscode/settings.json diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json deleted file mode 100644 index ad8ab09bd3..0000000000 --- a/.vscode/c_cpp_properties.json +++ /dev/null @@ -1,18 +0,0 @@ -{ - "configurations": [ - { - "name": "linux-gcc-x64", - "includePath": [ - "${workspaceFolder}/**" - ], - "compilerPath": "/usr/bin/gcc", - "cStandard": "${default}", - "cppStandard": "${default}", - "intelliSenseMode": "linux-gcc-x64", - "compilerArgs": [ - "" - ] - } - ], - "version": 4 -} diff --git a/.vscode/launch.json b/.vscode/launch.json deleted file mode 100644 index d9e9b0d8ec..0000000000 --- a/.vscode/launch.json +++ /dev/null @@ -1,24 +0,0 @@ -{ - "version": "0.2.0", - "configurations": [ - { - "name": "C/C++ Runner: Debug Session", - "type": "cppdbg", - "request": "launch", - "args": [], - "stopAtEntry": false, - "externalConsole": false, - "cwd": "/home/sai/saiteja/ros2_controllers/diff_drive_controller/src", - "program": "/home/sai/saiteja/ros2_controllers/diff_drive_controller/src/build/Debug/outDebug", - "MIMode": "gdb", - "miDebuggerPath": "gdb", - "setupCommands": [ - { - "description": "Enable pretty-printing for gdb", - "text": "-enable-pretty-printing", - "ignoreFailures": true - } - ] - } - ] -} diff --git a/.vscode/settings.json b/.vscode/settings.json deleted file mode 100644 index ee1337ac5c..0000000000 --- a/.vscode/settings.json +++ /dev/null @@ -1,68 +0,0 @@ -{ - "C_Cpp_Runner.cCompilerPath": "gcc", - "C_Cpp_Runner.cppCompilerPath": "g++", - "C_Cpp_Runner.debuggerPath": "gdb", - "C_Cpp_Runner.cStandard": "", - "C_Cpp_Runner.cppStandard": "", - "C_Cpp_Runner.msvcBatchPath": "", - "C_Cpp_Runner.useMsvc": false, - "C_Cpp_Runner.warnings": [ - "-Wall", - "-Wextra", - "-Wpedantic", - "-Wshadow", - "-Wformat=2", - "-Wcast-align", - "-Wconversion", - "-Wsign-conversion", - "-Wnull-dereference" - ], - "C_Cpp_Runner.msvcWarnings": [ - "/W4", - "/permissive-", - "/w14242", - "/w14287", - "/w14296", - "/w14311", - "/w14826", - "/w44062", - "/w44242", - "/w14905", - "/w14906", - "/w14263", - "/w44265", - "/w14928" - ], - "C_Cpp_Runner.enableWarnings": true, - "C_Cpp_Runner.warningsAsError": false, - "C_Cpp_Runner.compilerArgs": [], - "C_Cpp_Runner.linkerArgs": [], - "C_Cpp_Runner.includePaths": [], - "C_Cpp_Runner.includeSearch": [ - "*", - "**/*" - ], - "C_Cpp_Runner.excludeSearch": [ - "**/build", - "**/build/**", - "**/.*", - "**/.*/**", - "**/.vscode", - "**/.vscode/**" - ], - "C_Cpp_Runner.useAddressSanitizer": false, - "C_Cpp_Runner.useUndefinedSanitizer": false, - "C_Cpp_Runner.useLeakSanitizer": false, - "C_Cpp_Runner.showCompilationTime": false, - "C_Cpp_Runner.useLinkTimeOptimization": false, - "C_Cpp_Runner.msvcSecureNoWarnings": false, - "ROS2.distro": "humble", - "python.autoComplete.extraPaths": [ - "/opt/ros/humble/lib/python3.10/site-packages", - "/opt/ros/humble/local/lib/python3.10/dist-packages" - ], - "python.analysis.extraPaths": [ - "/opt/ros/humble/lib/python3.10/site-packages", - "/opt/ros/humble/local/lib/python3.10/dist-packages" - ] -} From 472f8775106fe87f6f124c802e65c004b9c71458 Mon Sep 17 00:00:00 2001 From: Saitejabojja07 Date: Tue, 31 Mar 2026 09:09:38 +0530 Subject: [PATCH 3/3] Fix : Updated Speed limits before Odom --- .../src/diff_drive_controller.cpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 8b798f0d90..3a04b5f960 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -158,12 +158,7 @@ controller_interface::return_type DiffDriveController::update_and_write_commands return controller_interface::return_type::OK; } - // Apply (possibly new) multipliers: - const double wheel_separation = params_.wheel_separation_multiplier * params_.wheel_separation; - const double left_wheel_radius = params_.left_wheel_radius_multiplier * params_.wheel_radius; - const double right_wheel_radius = params_.right_wheel_radius_multiplier * params_.wheel_radius; - - bool odometry_updated = false; + // Apply speed limits and update buffers double & last_linear = previous_two_commands_.back()[0]; double & second_to_last_linear = previous_two_commands_.front()[0]; double & last_angular = previous_two_commands_.back()[1]; @@ -174,6 +169,13 @@ controller_interface::return_type DiffDriveController::update_and_write_commands previous_two_commands_.pop(); previous_two_commands_.push({{linear_command, angular_command}}); + // Apply (possibly new) multipliers: + const double wheel_separation = params_.wheel_separation_multiplier * params_.wheel_separation; + const double left_wheel_radius = params_.left_wheel_radius_multiplier * params_.wheel_radius; + const double right_wheel_radius = params_.right_wheel_radius_multiplier * params_.wheel_radius; + + bool odometry_updated = false; + // check if odometry set or reset was requested by non-RT thread if (set_odom_requested_.load()) {