Fix DiffDrive claiming state when open_loop is set (#1730)

SuperJappie08 · web-flow · commit e188bf9fff61 · 2025-06-03T08:04:38.000+02:00
diff --git a/diff_drive_controller/doc/userdoc.rst b/diff_drive_controller/doc/userdoc.rst
@@ -41,6 +41,7 @@ Feedback
 ,,,,,,,,,,,,,,
 
 As feedback interface type the joints' position (``hardware_interface::HW_IF_POSITION``) or velocity (``hardware_interface::HW_IF_VELOCITY``,if parameter ``position_feedback=false``) are used.
+Unless the parameter ``open_loop=true`` is set, then no external state interfaces are used (the commanded velocity is used for odometry instead).
 
 Output
 ,,,,,,,,,
diff --git a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp
@@ -83,7 +83,7 @@ class DiffDriveController : public controller_interface::ChainableControllerInte
 
   struct WheelHandle
   {
-    std::reference_wrapper<const hardware_interface::LoanedStateInterface> feedback;
+    std::optional<std::reference_wrapper<const hardware_interface::LoanedStateInterface>> feedback;
     std::reference_wrapper<hardware_interface::LoanedCommandInterface> velocity;
   };
 
diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp
@@ -85,6 +85,11 @@ InterfaceConfiguration DiffDriveController::command_interface_configuration() co
 
 InterfaceConfiguration DiffDriveController::state_interface_configuration() const
 {
+  if (params_.open_loop)
+  {
+    return {interface_configuration_type::NONE, {}};
+  }
+
   std::vector<std::string> conf_names;
   for (const auto & joint_name : params_.left_wheel_names)
   {
@@ -168,9 +173,9 @@ controller_interface::return_type DiffDriveController::update_and_write_commands
     for (size_t index = 0; index < static_cast<size_t>(wheels_per_side_); ++index)
     {
       const auto left_feedback_op =
-        registered_left_wheel_handles_[index].feedback.get().get_optional();
+        registered_left_wheel_handles_[index].feedback.value().get().get_optional();
       const auto right_feedback_op =
-        registered_right_wheel_handles_[index].feedback.get().get_optional();
+        registered_right_wheel_handles_[index].feedback.value().get().get_optional();
 
       if (!left_feedback_op.has_value() || !right_feedback_op.has_value())
       {
@@ -667,21 +672,6 @@ controller_interface::CallbackReturn DiffDriveController::configure_side(
   registered_handles.reserve(wheel_names.size());
   for (const auto & wheel_name : wheel_names)
   {
-    const auto interface_name = feedback_type();
-    const auto state_handle = std::find_if(
-      state_interfaces_.cbegin(), state_interfaces_.cend(),
-      [&wheel_name, &interface_name](const auto & interface)
-      {
-        return interface.get_prefix_name() == wheel_name &&
-               interface.get_interface_name() == interface_name;
-      });
-
-    if (state_handle == state_interfaces_.cend())
-    {
-      RCLCPP_ERROR(logger, "Unable to obtain joint state handle for %s", wheel_name.c_str());
-      return controller_interface::CallbackReturn::ERROR;
-    }
-
     const auto command_handle = std::find_if(
       command_interfaces_.begin(), command_interfaces_.end(),
       [&wheel_name](const auto & interface)
@@ -696,8 +686,30 @@ controller_interface::CallbackReturn DiffDriveController::configure_side(
       return controller_interface::CallbackReturn::ERROR;
     }
 
-    registered_handles.emplace_back(
-      WheelHandle{std::ref(*state_handle), std::ref(*command_handle)});
+    if (params_.open_loop)
+    {
+      registered_handles.emplace_back(WheelHandle{std::nullopt, std::ref(*command_handle)});
+    }
+    else
+    {
+      const auto interface_name = feedback_type();
+      const auto state_handle = std::find_if(
+        state_interfaces_.cbegin(), state_interfaces_.cend(),
+        [&wheel_name, &interface_name](const auto & interface)
+        {
+          return interface.get_prefix_name() == wheel_name &&
+                 interface.get_interface_name() == interface_name;
+        });
+
+      if (state_handle == state_interfaces_.cend())
+      {
+        RCLCPP_ERROR(logger, "Unable to obtain joint state handle for %s", wheel_name.c_str());
+        return controller_interface::CallbackReturn::ERROR;
+      }
+
+      registered_handles.emplace_back(
+        WheelHandle{{std::ref(*state_handle)}, std::ref(*command_handle)});
+    }
   }
 
   return controller_interface::CallbackReturn::SUCCESS;
diff --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp
@@ -175,6 +175,15 @@ class TestDiffDriveController : public ::testing::Test
     controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs));
   }
 
+  void assignResourcesNoFeedback()
+  {
+    std::vector<LoanedCommandInterface> command_ifs;
+    command_ifs.emplace_back(left_wheel_vel_cmd_);
+    command_ifs.emplace_back(right_wheel_vel_cmd_);
+
+    controller_->assign_interfaces(std::move(command_ifs), {});
+  }
+
   controller_interface::return_type InitController(
     const std::vector<std::string> left_wheel_joints_init = left_wheel_names,
     const std::vector<std::string> right_wheel_joints_init = right_wheel_names,
@@ -262,6 +271,26 @@ TEST_F(
   EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL);
 }
 
+TEST_F(
+  TestDiffDriveController,
+  command_and_state_interface_configuration_succeeds_when_wheels_and_open_loop_are_specified)
+{
+  ASSERT_EQ(
+    InitController(
+      left_wheel_names, right_wheel_names,
+      {rclcpp::Parameter("open_loop", rclcpp::ParameterValue(true))}),
+    controller_interface::return_type::OK);
+
+  ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
+
+  auto state_if_conf = controller_->state_interface_configuration();
+  ASSERT_THAT(state_if_conf.names, SizeIs(0));
+  EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::NONE);
+  auto cmd_if_conf = controller_->command_interface_configuration();
+  ASSERT_THAT(cmd_if_conf.names, SizeIs(left_wheel_names.size() + right_wheel_names.size()));
+  EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL);
+}
+
 TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_no_namespace)
 {
   std::string odom_id = "odom";
@@ -458,6 +487,19 @@ TEST_F(TestDiffDriveController, activate_succeeds_with_vel_resources_assigned)
   ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
 }
 
+TEST_F(TestDiffDriveController, activate_succeeds_with_open_loop_assigned)
+{
+  ASSERT_EQ(
+    InitController(
+      left_wheel_names, right_wheel_names,
+      {rclcpp::Parameter("open_loop", rclcpp::ParameterValue(true))}),
+    controller_interface::return_type::OK);
+
+  ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
+  assignResourcesNoFeedback();
+  ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
+}
+
 TEST_F(TestDiffDriveController, test_speed_limiter)
 {
   // If you send a linear velocity command without acceleration limits,
@@ -690,6 +732,34 @@ TEST_F(TestDiffDriveController, activate_fails_with_wrong_resources_assigned_2)
   ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR);
 }
 
+TEST_F(TestDiffDriveController, activate_silently_ignores_with_unnecessary_resources_assigned_1)
+{
+  ASSERT_EQ(
+    InitController(
+      left_wheel_names, right_wheel_names,
+      {rclcpp::Parameter("open_loop", rclcpp::ParameterValue(true)),
+       rclcpp::Parameter("position_feedback", rclcpp::ParameterValue(false))}),
+    controller_interface::return_type::OK);
+
+  ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
+  assignResourcesPosFeedback();
+  ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
+}
+
+TEST_F(TestDiffDriveController, activate_silently_ignores_with_unnecessary_resources_assigned_2)
+{
+  ASSERT_EQ(
+    InitController(
+      left_wheel_names, right_wheel_names,
+      {rclcpp::Parameter("open_loop", rclcpp::ParameterValue(true)),
+       rclcpp::Parameter("position_feedback", rclcpp::ParameterValue(true))}),
+    controller_interface::return_type::OK);
+
+  ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
+  assignResourcesVelFeedback();
+  ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
+}
+
 TEST_F(TestDiffDriveController, cleanup)
 {
   ASSERT_EQ(

Original file line number	Diff line number	Diff line change
`@@ -83,7 +83,7 @@ class DiffDriveController : public controller_interface::ChainableControllerInte`
`83`	`83`
`84`	`84`	`struct WheelHandle`
`85`	`85`	`{`
`86`		`- std::reference_wrapper<const hardware_interface::LoanedStateInterface> feedback;`
	`86`	`+ std::optional<std::reference_wrapper<const hardware_interface::LoanedStateInterface>> feedback;`
`87`	`87`	`std::reference_wrapper<hardware_interface::LoanedCommandInterface> velocity;`
`88`	`88`	`};`
`89`	`89`