Add velocity feedback option for diff_drive_controller (ros-controls#260

)

diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp

@@ -88,10 +88,11 @@ class DiffDriveController : public controller_interface::ControllerInterface
 protected:
   struct WheelHandle
   {
-    std::reference_wrapper<const hardware_interface::LoanedStateInterface> position;
+    std::reference_wrapper<const hardware_interface::LoanedStateInterface> feedback;
     std::reference_wrapper<hardware_interface::LoanedCommandInterface> velocity;
   };
 
+  const char * feedback_type() const;
   CallbackReturn configure_side(
     const std::string & side, const std::vector<std::string> & wheel_names,
     std::vector<WheelHandle> & registered_handles);
@@ -115,6 +116,7 @@ class DiffDriveController : public controller_interface::ControllerInterface
   struct OdometryParams
   {
     bool open_loop = false;
+    bool position_feedback = true;
     bool enable_odom_tf = true;
     std::string base_frame_id = "base_link";
     std::string odom_frame_id = "odom";

diff_drive_controller/include/diff_drive_controller/odometry.hpp

@@ -36,6 +36,7 @@ class Odometry
 
   void init(const rclcpp::Time & time);
   bool update(double left_pos, double right_pos, const rclcpp::Time & time);
+  bool updateFromVelocity(double left_vel, double right_vel, const rclcpp::Time & time);
   void updateOpenLoop(double linear, double angular, const rclcpp::Time & time);
   void resetOdometry();
 

diff_drive_controller/src/diff_drive_controller.cpp

@@ -48,6 +48,11 @@ using lifecycle_msgs::msg::State;
 
 DiffDriveController::DiffDriveController() : controller_interface::ControllerInterface() {}
 
+const char * DiffDriveController::feedback_type() const
+{
+  return odom_params_.position_feedback ? HW_IF_POSITION : HW_IF_VELOCITY;
+}
+
 CallbackReturn DiffDriveController::on_init()
 {
   try
@@ -68,6 +73,7 @@ CallbackReturn DiffDriveController::on_init()
     auto_declare<std::vector<double>>("pose_covariance_diagonal", std::vector<double>());
     auto_declare<std::vector<double>>("twist_covariance_diagonal", std::vector<double>());
     auto_declare<bool>("open_loop", odom_params_.open_loop);
+    auto_declare<bool>("position_feedback", odom_params_.position_feedback);
     auto_declare<bool>("enable_odom_tf", odom_params_.enable_odom_tf);
 
     auto_declare<double>("cmd_vel_timeout", cmd_vel_timeout_.count() / 1000.0);
@@ -123,11 +129,11 @@ InterfaceConfiguration DiffDriveController::state_interface_configuration() cons
   std::vector<std::string> conf_names;
   for (const auto & joint_name : left_wheel_names_)
   {
-    conf_names.push_back(joint_name + "/" + HW_IF_POSITION);
+    conf_names.push_back(joint_name + "/" + feedback_type());
   }
   for (const auto & joint_name : right_wheel_names_)
   {
-    conf_names.push_back(joint_name + "/" + HW_IF_POSITION);
+    conf_names.push_back(joint_name + "/" + feedback_type());
   }
   return {interface_configuration_type::INDIVIDUAL, conf_names};
 }
@@ -183,28 +189,36 @@ controller_interface::return_type DiffDriveController::update(
   }
   else
   {
-    double left_position_mean = 0.0;
-    double right_position_mean = 0.0;
+    double left_feedback_mean = 0.0;
+    double right_feedback_mean = 0.0;
     for (size_t index = 0; index < wheels.wheels_per_side; ++index)
     {
-      const double left_position = registered_left_wheel_handles_[index].position.get().get_value();
-      const double right_position =
-        registered_right_wheel_handles_[index].position.get().get_value();
+      const double left_feedback = registered_left_wheel_handles_[index].feedback.get().get_value();
+      const double right_feedback =
+        registered_right_wheel_handles_[index].feedback.get().get_value();
 
-      if (std::isnan(left_position) || std::isnan(right_position))
+      if (std::isnan(left_feedback) || std::isnan(right_feedback))
       {
         RCLCPP_ERROR(
-          logger, "Either the left or right wheel position is invalid for index [%zu]", index);
+          logger, "Either the left or right wheel %s is invalid for index [%zu]", feedback_type(),
+          index);
         return controller_interface::return_type::ERROR;
       }
 
-      left_position_mean += left_position;
-      right_position_mean += right_position;
+      left_feedback_mean += left_feedback;
+      right_feedback_mean += right_feedback;
     }
-    left_position_mean /= wheels.wheels_per_side;
-    right_position_mean /= wheels.wheels_per_side;
+    left_feedback_mean /= wheels.wheels_per_side;
+    right_feedback_mean /= wheels.wheels_per_side;
 
-    odometry_.update(left_position_mean, right_position_mean, current_time);
+    if (odom_params_.position_feedback)
+    {
+      odometry_.update(left_feedback_mean, right_feedback_mean, current_time);
+    }
+    else
+    {
+      odometry_.updateFromVelocity(left_feedback_mean, right_feedback_mean, current_time);
+    }
   }
 
   tf2::Quaternion orientation;
@@ -331,6 +345,7 @@ CallbackReturn DiffDriveController::on_configure(const rclcpp_lifecycle::State &
     twist_diagonal.begin(), twist_diagonal.end(), odom_params_.twist_covariance_diagonal.begin());
 
   odom_params_.open_loop = node_->get_parameter("open_loop").as_bool();
+  odom_params_.position_feedback = node_->get_parameter("position_feedback").as_bool();
   odom_params_.enable_odom_tf = node_->get_parameter("enable_odom_tf").as_bool();
 
   cmd_vel_timeout_ = std::chrono::milliseconds{
@@ -586,10 +601,12 @@ 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](const auto & interface) {
+      state_interfaces_.cbegin(), state_interfaces_.cend(),
+      [&wheel_name, &interface_name](const auto & interface) {
         return interface.get_name() == wheel_name &&
-               interface.get_interface_name() == HW_IF_POSITION;
+               interface.get_interface_name() == interface_name;
       });
 
     if (state_handle == state_interfaces_.cend())

diff_drive_controller/src/odometry.cpp

@@ -66,10 +66,19 @@ bool Odometry::update(double left_pos, double right_pos, const rclcpp::Time & ti
   left_wheel_old_pos_ = left_wheel_cur_pos;
   right_wheel_old_pos_ = right_wheel_cur_pos;
 
+  updateFromVelocity(left_wheel_est_vel, right_wheel_est_vel, time);
+
+  return true;
+}
+
+bool Odometry::updateFromVelocity(double left_vel, double right_vel, const rclcpp::Time & time)
+{
+  const double dt = time.seconds() - timestamp_.seconds();
+
   // Compute linear and angular diff:
-  const double linear = (right_wheel_est_vel + left_wheel_est_vel) * 0.5;
+  const double linear = (left_vel + right_vel) * 0.5;
   // Now there is a bug about scout angular velocity
-  const double angular = (right_wheel_est_vel - left_wheel_est_vel) / wheel_separation_;
+  const double angular = (left_vel - right_vel) / wheel_separation_;
 
   // Integrate odometry:
   integrateExact(linear, angular);

diff_drive_controller/test/config/test_diff_drive_controller.yaml

@@ -1,4 +1,4 @@
-diff_drive_controller:
+test_diff_drive_controller:
   ros__parameters:
     left_wheel_names: ["left_wheels"]
     right_wheel_names: ["right_wheels"]
@@ -17,6 +17,7 @@ diff_drive_controller:
     pose_covariance_diagonal: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
     twist_covariance_diagonal: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
 
+    position_feedback: false
     open_loop: true
     enable_odom_tf: true
 

diff_drive_controller/test/test_diff_drive_controller.cpp

@@ -129,7 +129,7 @@ class TestDiffDriveController : public ::testing::Test
     }
   }
 
-  void assignResources()
+  void assignResourcesPosFeedback()
   {
     std::vector<LoanedStateInterface> state_ifs;
     state_ifs.emplace_back(left_wheel_pos_state_);
@@ -142,6 +142,19 @@ class TestDiffDriveController : public ::testing::Test
     controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs));
   }
 
+  void assignResourcesVelFeedback()
+  {
+    std::vector<LoanedStateInterface> state_ifs;
+    state_ifs.emplace_back(left_wheel_vel_state_);
+    state_ifs.emplace_back(right_wheel_vel_state_);
+
+    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), std::move(state_ifs));
+  }
+
   const std::string controller_name = "test_diff_drive_controller";
   std::unique_ptr<TestableDiffDriveController> controller_;
 
@@ -154,6 +167,10 @@ class TestDiffDriveController : public ::testing::Test
     left_wheel_names[0], HW_IF_POSITION, &position_values_[0]};
   hardware_interface::StateInterface right_wheel_pos_state_{
     right_wheel_names[0], HW_IF_POSITION, &position_values_[1]};
+  hardware_interface::StateInterface left_wheel_vel_state_{
+    left_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[0]};
+  hardware_interface::StateInterface right_wheel_vel_state_{
+    right_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[1]};
   hardware_interface::CommandInterface left_wheel_vel_cmd_{
     left_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[0]};
   hardware_interface::CommandInterface right_wheel_vel_cmd_{
@@ -241,21 +258,73 @@ TEST_F(TestDiffDriveController, activate_fails_without_resources_assigned)
   ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR);
 }
 
-TEST_F(TestDiffDriveController, activate_succeeds_with_resources_assigned)
+TEST_F(TestDiffDriveController, activate_succeeds_with_pos_resources_assigned)
 {
   const auto ret = controller_->init(controller_name);
   ASSERT_EQ(ret, controller_interface::return_type::OK);
 
+  // We implicitly test that by default position feedback is required
   controller_->get_node()->set_parameter(
     rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names)));
   controller_->get_node()->set_parameter(
     rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names)));
 
   ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
-  assignResources();
+  assignResourcesPosFeedback();
   ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
 }
 
+TEST_F(TestDiffDriveController, activate_succeeds_with_vel_resources_assigned)
+{
+  const auto ret = controller_->init(controller_name);
+  ASSERT_EQ(ret, controller_interface::return_type::OK);
+
+  controller_->get_node()->set_parameter(
+    rclcpp::Parameter("position_feedback", rclcpp::ParameterValue(false)));
+  controller_->get_node()->set_parameter(
+    rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names)));
+  controller_->get_node()->set_parameter(
+    rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names)));
+
+  ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
+  assignResourcesVelFeedback();
+  ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
+}
+
+TEST_F(TestDiffDriveController, activate_fails_with_wrong_resources_assigned_1)
+{
+  const auto ret = controller_->init(controller_name);
+  ASSERT_EQ(ret, controller_interface::return_type::OK);
+
+  controller_->get_node()->set_parameter(
+    rclcpp::Parameter("position_feedback", rclcpp::ParameterValue(false)));
+  controller_->get_node()->set_parameter(
+    rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names)));
+  controller_->get_node()->set_parameter(
+    rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names)));
+
+  ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
+  assignResourcesPosFeedback();
+  ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR);
+}
+
+TEST_F(TestDiffDriveController, activate_fails_with_wrong_resources_assigned_2)
+{
+  const auto ret = controller_->init(controller_name);
+  ASSERT_EQ(ret, controller_interface::return_type::OK);
+
+  controller_->get_node()->set_parameter(
+    rclcpp::Parameter("position_feedback", rclcpp::ParameterValue(true)));
+  controller_->get_node()->set_parameter(
+    rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names)));
+  controller_->get_node()->set_parameter(
+    rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names)));
+
+  ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
+  assignResourcesVelFeedback();
+  ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR);
+}
+
 TEST_F(TestDiffDriveController, cleanup)
 {
   const auto ret = controller_->init(controller_name);
@@ -272,7 +341,7 @@ TEST_F(TestDiffDriveController, cleanup)
   executor.add_node(controller_->get_node()->get_node_base_interface());
   auto state = controller_->configure();
   ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id());
-  assignResources();
+  assignResourcesPosFeedback();
 
   state = controller_->activate();
   ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id());
@@ -321,7 +390,7 @@ TEST_F(TestDiffDriveController, correct_initialization_using_parameters)
   executor.add_node(controller_->get_node()->get_node_base_interface());
 
   auto state = controller_->configure();
-  assignResources();
+  assignResourcesPosFeedback();
 
   ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id());
   EXPECT_EQ(0.01, left_wheel_vel_cmd_.get_value());