Prefix DiffDriveController with PickNik

henningkayser · henningkayser · commit 339bf84d8056 · 2024-04-30T16:52:49.000Z
diff --git a/README.md b/README.md
@@ -3,9 +3,16 @@
 Package defining custom controllers that are generic and we want to contribute them into ros2_controllers.
 The controllers have been prefixed with `picknik_` so that when they eventually land upstream the prefix can be removed and the versions here will become deprecated.
 
+- `picknik_diff_drive_controller::DiffDriveController`
 - `picknik_twist_controller::PicknikTwistController`
 - `picknik_reset_fault_controller::PicknikResetFaultController`
 
+## DiffDriveController
+Generic ROS2 controller with chaining support for JTC path following.
+The implementation is currently specific to MoveIt (x/y/theta joints), we might eventually break out the velocity tracking into linear and rotational components to bring this upstream to ros2_controllers.
+
+### Usage
+See the [Stretch ROS](https://github.com/PickNikRobotics/stretch_ros) configuration for an example setup, running a JointTrajectoryController and the chained DiffDriveController for computing wheel commands and mocked odometry.
 
 ## PicknikTwistController
 Generic ROS2 controller to forward cartesian twist commands.
diff --git a/picknik_diff_drive_controller/CHANGELOG.rst b/picknik_diff_drive_controller/CHANGELOG.rst
diff --git a/picknik_diff_drive_controller/CMakeLists.txt b/picknik_diff_drive_controller/CMakeLists.txt
@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.16)
-project(diff_drive_controller LANGUAGES CXX)
+project(picknik_diff_drive_controller LANGUAGES CXX)
 
 if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
   add_compile_options(-Wall -Wextra)
@@ -80,7 +80,7 @@ endif()
 
 install(
   DIRECTORY include/
-  DESTINATION include/diff_drive_controller
+  DESTINATION include/picknik_diff_drive_controller
 )
 install(TARGETS diff_drive_controller diff_drive_controller_parameters
   EXPORT export_diff_drive_controller
diff --git a/picknik_diff_drive_controller/diff_drive_plugin.xml b/picknik_diff_drive_controller/diff_drive_plugin.xml
@@ -1,5 +1,5 @@
 <library path="diff_drive_controller">
-  <class name="diff_drive_controller/DiffDriveController" type="diff_drive_controller::DiffDriveController" base_class_type="controller_interface::ChainableControllerInterface">
+  <class name="picknik_diff_drive_controller/DiffDriveController" type="picknik_diff_drive_controller::DiffDriveController" base_class_type="controller_interface::ChainableControllerInterface">
   <description>
     The differential drive controller transforms linear and angular velocity messages into signals for each wheel(s) for a differential drive robot.
   </description>
diff --git a/picknik_diff_drive_controller/doc/parameters_context.yaml b/picknik_diff_drive_controller/doc/parameters_context.yaml
diff --git a/picknik_diff_drive_controller/doc/userdoc.rst b/picknik_diff_drive_controller/doc/userdoc.rst
diff --git a/picknik_diff_drive_controller/include/picknik_diff_drive_controller/diff_drive_controller.hpp b/picknik_diff_drive_controller/include/picknik_diff_drive_controller/diff_drive_controller.hpp
@@ -27,14 +27,14 @@
 #include <vector>
 
 #include "controller_interface/chainable_controller_interface.hpp"
-#include "diff_drive_controller/odometry.hpp"
-#include "diff_drive_controller/speed_limiter.hpp"
-#include "diff_drive_controller/visibility_control.h"
 #include "geometry_msgs/msg/twist.hpp"
 #include "geometry_msgs/msg/twist_stamped.hpp"
 #include "hardware_interface/handle.hpp"
 #include "nav_msgs/msg/odometry.hpp"
 #include "odometry.hpp"
+#include "picknik_diff_drive_controller/odometry.hpp"
+#include "picknik_diff_drive_controller/speed_limiter.hpp"
+#include "picknik_diff_drive_controller/visibility_control.h"
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp_lifecycle/state.hpp"
 #include "realtime_tools/realtime_buffer.h"
@@ -43,7 +43,7 @@
 
 #include "diff_drive_controller_parameters.hpp"
 
-namespace diff_drive_controller
+namespace picknik_diff_drive_controller
 {
 class DiffDriveController : public controller_interface::ChainableControllerInterface
 {
@@ -168,5 +168,5 @@ class DiffDriveController : public controller_interface::ChainableControllerInte
   void reference_callback(const std::shared_ptr<ControllerTwistReferenceMsg> msg);
   void reference_callback_unstamped(const std::shared_ptr<geometry_msgs::msg::Twist> msg);
 };
-}  // namespace diff_drive_controller
+}  // namespace picknik_diff_drive_controller
 #endif  // DIFF_DRIVE_CONTROLLER__DIFF_DRIVE_CONTROLLER_HPP_
diff --git a/picknik_diff_drive_controller/include/picknik_diff_drive_controller/odometry.hpp b/picknik_diff_drive_controller/include/picknik_diff_drive_controller/odometry.hpp
@@ -27,7 +27,7 @@
 #include "rclcpp/time.hpp"
 #include "rcppmath/rolling_mean_accumulator.hpp"
 
-namespace diff_drive_controller
+namespace picknik_diff_drive_controller
 {
 class Odometry
 {
@@ -83,6 +83,6 @@ class Odometry
   RollingMeanAccumulator angular_accumulator_;
 };
 
-}  // namespace diff_drive_controller
+}  // namespace picknik_diff_drive_controller
 
 #endif  // DIFF_DRIVE_CONTROLLER__ODOMETRY_HPP_
diff --git a/picknik_diff_drive_controller/include/picknik_diff_drive_controller/speed_limiter.hpp b/picknik_diff_drive_controller/include/picknik_diff_drive_controller/speed_limiter.hpp
@@ -21,7 +21,7 @@
 
 #include <cmath>
 
-namespace diff_drive_controller
+namespace picknik_diff_drive_controller
 {
 class SpeedLimiter
 {
@@ -100,6 +100,6 @@ class SpeedLimiter
   double max_jerk_;
 };
 
-}  // namespace diff_drive_controller
+}  // namespace picknik_diff_drive_controller
 
 #endif  // DIFF_DRIVE_CONTROLLER__SPEED_LIMITER_HPP_
diff --git a/picknik_diff_drive_controller/include/picknik_diff_drive_controller/visibility_control.h b/picknik_diff_drive_controller/include/picknik_diff_drive_controller/visibility_control.h
diff --git a/picknik_diff_drive_controller/package.xml b/picknik_diff_drive_controller/package.xml
@@ -1,6 +1,6 @@
 <?xml version="1.0"?>
 <package format="3">
-  <name>diff_drive_controller</name>
+  <name>picknik_diff_drive_controller</name>
   <version>2.33.0</version>
   <description>Controller for a differential drive mobile base.</description>
   <maintainer email="bence.magyar.robotics@gmail.com">Bence Magyar</maintainer>
diff --git a/picknik_diff_drive_controller/src/diff_drive_controller.cpp b/picknik_diff_drive_controller/src/diff_drive_controller.cpp
@@ -22,9 +22,9 @@
 #include <utility>
 #include <vector>
 
-#include "diff_drive_controller/diff_drive_controller.hpp"
 #include "hardware_interface/types/hardware_interface_type_values.hpp"
 #include "lifecycle_msgs/msg/state.hpp"
+#include "picknik_diff_drive_controller/diff_drive_controller.hpp"
 #include "rclcpp/logging.hpp"
 #include "tf2/LinearMath/Quaternion.h"
 
@@ -59,7 +59,7 @@ void reset_controller_reference_msg(
 
 }  // namespace
 
-namespace diff_drive_controller
+namespace picknik_diff_drive_controller
 {
 using namespace std::chrono_literals;
 using controller_interface::interface_configuration_type;
@@ -700,9 +700,10 @@ bool DiffDriveController::on_set_chained_mode(bool chained_mode)
   return true || chained_mode;
 }
 
-}  // namespace diff_drive_controller
+}  // namespace picknik_diff_drive_controller
 
 #include "class_loader/register_macro.hpp"
 
 CLASS_LOADER_REGISTER_CLASS(
-  diff_drive_controller::DiffDriveController, controller_interface::ChainableControllerInterface)
+  picknik_diff_drive_controller::DiffDriveController,
+  controller_interface::ChainableControllerInterface)
diff --git a/picknik_diff_drive_controller/src/diff_drive_controller_parameter.yaml b/picknik_diff_drive_controller/src/diff_drive_controller_parameter.yaml
@@ -1,4 +1,4 @@
-diff_drive_controller:
+picknik_diff_drive_controller:
   left_wheel_names: {
     type: string_array,
     default_value: [],
diff --git a/picknik_diff_drive_controller/src/odometry.cpp b/picknik_diff_drive_controller/src/odometry.cpp
@@ -16,9 +16,9 @@
  * Author: Enrique Fernández
  */
 
-#include "diff_drive_controller/odometry.hpp"
+#include "picknik_diff_drive_controller/odometry.hpp"
 
-namespace diff_drive_controller
+namespace picknik_diff_drive_controller
 {
 Odometry::Odometry(size_t velocity_rolling_window_size)
 : timestamp_(0.0),
@@ -162,4 +162,4 @@ void Odometry::resetAccumulators()
   angular_accumulator_ = RollingMeanAccumulator(velocity_rolling_window_size_);
 }
 
-}  // namespace diff_drive_controller
+}  // namespace picknik_diff_drive_controller
diff --git a/picknik_diff_drive_controller/src/speed_limiter.cpp b/picknik_diff_drive_controller/src/speed_limiter.cpp
@@ -19,10 +19,10 @@
 #include <algorithm>
 #include <stdexcept>
 
-#include "diff_drive_controller/speed_limiter.hpp"
+#include "picknik_diff_drive_controller/speed_limiter.hpp"
 #include "rcppmath/clamp.hpp"
 
-namespace diff_drive_controller
+namespace picknik_diff_drive_controller
 {
 SpeedLimiter::SpeedLimiter(
   bool has_velocity_limits, bool has_acceleration_limits, bool has_jerk_limits, double min_velocity,
@@ -137,4 +137,4 @@ double SpeedLimiter::limit_jerk(double & v, double v0, double v1, double dt)
   return tmp != 0.0 ? v / tmp : 1.0;
 }
 
-}  // namespace diff_drive_controller
+}  // namespace picknik_diff_drive_controller
diff --git a/picknik_diff_drive_controller/test/config/test_diff_drive_controller.yaml b/picknik_diff_drive_controller/test/config/test_diff_drive_controller.yaml
diff --git a/picknik_diff_drive_controller/test/test_diff_drive_controller.cpp b/picknik_diff_drive_controller/test/test_diff_drive_controller.cpp
@@ -21,11 +21,11 @@
 #include <utility>
 #include <vector>
 
-#include "diff_drive_controller/diff_drive_controller.hpp"
 #include "hardware_interface/loaned_command_interface.hpp"
 #include "hardware_interface/loaned_state_interface.hpp"
 #include "hardware_interface/types/hardware_interface_type_values.hpp"
 #include "lifecycle_msgs/msg/state.hpp"
+#include "picknik_diff_drive_controller/diff_drive_controller.hpp"
 #include "rclcpp/rclcpp.hpp"
 
 using CallbackReturn = controller_interface::CallbackReturn;
@@ -36,7 +36,7 @@ using hardware_interface::LoanedStateInterface;
 using lifecycle_msgs::msg::State;
 using testing::SizeIs;
 
-class TestableDiffDriveController : public diff_drive_controller::DiffDriveController
+class TestableDiffDriveController : public picknik_diff_drive_controller::DiffDriveController
 {
 public:
   using DiffDriveController::DiffDriveController;
diff --git a/picknik_diff_drive_controller/test/test_load_diff_drive_controller.cpp b/picknik_diff_drive_controller/test/test_load_diff_drive_controller.cpp
@@ -34,7 +34,8 @@ TEST(TestLoadDiffDriveController, load_controller)
     executor, "test_controller_manager");
 
   ASSERT_NE(
-    cm.load_controller("test_diff_drive_controller", "diff_drive_controller/DiffDriveController"),
+    cm.load_controller(
+      "test_diff_drive_controller", "picknik_diff_drive_controller/DiffDriveController"),
     nullptr);
 
   rclcpp::shutdown();

Original file line number	Diff line number	Diff line change
`@@ -27,7 +27,7 @@`
`27`	`27`	`#include "rclcpp/time.hpp"`
`28`	`28`	`#include "rcppmath/rolling_mean_accumulator.hpp"`
`29`	`29`
`30`		`-namespace diff_drive_controller`
	`30`	`+namespace picknik_diff_drive_controller`
`31`	`31`	`{`
`32`	`32`	`class Odometry`
`33`	`33`	`{`
`@@ -83,6 +83,6 @@ class Odometry`
`83`	`83`	`RollingMeanAccumulator angular_accumulator_;`
`84`	`84`	`};`
`85`	`85`
`86`		`-} // namespace diff_drive_controller`
	`86`	`+} // namespace picknik_diff_drive_controller`
`87`	`87`
`88`	`88`	`#endif // DIFF_DRIVE_CONTROLLER__ODOMETRY_HPP_`
Original file line number	Diff line number	Diff line change
`@@ -21,7 +21,7 @@`
`21`	`21`
`22`	`22`	`#include <cmath>`
`23`	`23`
`24`		`-namespace diff_drive_controller`
	`24`	`+namespace picknik_diff_drive_controller`
`25`	`25`	`{`
`26`	`26`	`class SpeedLimiter`
`27`	`27`	`{`
`@@ -100,6 +100,6 @@ class SpeedLimiter`
`100`	`100`	`double max_jerk_;`
`101`	`101`	`};`
`102`	`102`
`103`		`-} // namespace diff_drive_controller`
	`103`	`+} // namespace picknik_diff_drive_controller`
`104`	`104`
`105`	`105`	`#endif // DIFF_DRIVE_CONTROLLER__SPEED_LIMITER_HPP_`