From d86cffa89fdc05f5b59c5af5e8a836511468665b Mon Sep 17 00:00:00 2001 From: Patrick Roncagliolo Date: Thu, 18 Nov 2021 14:14:41 +0100 Subject: [PATCH] Add velocity feedback option for diff_drive_controller (#260) --- .../diff_drive_controller.hpp | 4 +- .../diff_drive_controller/odometry.hpp | 1 + .../src/diff_drive_controller.cpp | 49 ++++++++---- diff_drive_controller/src/odometry.cpp | 13 ++- .../config/test_diff_drive_controller.yaml | 3 +- .../test/test_diff_drive_controller.cpp | 79 +++++++++++++++++-- 6 files changed, 124 insertions(+), 25 deletions(-) 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 index 38f767dd3b..026d4edccc 100644 --- a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp +++ b/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 position; + std::reference_wrapper feedback; std::reference_wrapper velocity; }; + const char * feedback_type() const; CallbackReturn configure_side( const std::string & side, const std::vector & wheel_names, std::vector & 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 --git a/diff_drive_controller/include/diff_drive_controller/odometry.hpp b/diff_drive_controller/include/diff_drive_controller/odometry.hpp index 57af643bf0..eb3bee9ab6 100644 --- a/diff_drive_controller/include/diff_drive_controller/odometry.hpp +++ b/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 --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 5e8c080f0d..77c9a61a85 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/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>("pose_covariance_diagonal", std::vector()); auto_declare>("twist_covariance_diagonal", std::vector()); auto_declare("open_loop", odom_params_.open_loop); + auto_declare("position_feedback", odom_params_.position_feedback); auto_declare("enable_odom_tf", odom_params_.enable_odom_tf); auto_declare("cmd_vel_timeout", cmd_vel_timeout_.count() / 1000.0); @@ -123,11 +129,11 @@ InterfaceConfiguration DiffDriveController::state_interface_configuration() cons std::vector 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 --git a/diff_drive_controller/src/odometry.cpp b/diff_drive_controller/src/odometry.cpp index 34168fb85e..bd718718fe 100644 --- a/diff_drive_controller/src/odometry.cpp +++ b/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 --git a/diff_drive_controller/test/config/test_diff_drive_controller.yaml b/diff_drive_controller/test/config/test_diff_drive_controller.yaml index 30e57361bb..a2149eb6bc 100644 --- a/diff_drive_controller/test/config/test_diff_drive_controller.yaml +++ b/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 --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index ca3da08803..ab007759ef 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -129,7 +129,7 @@ class TestDiffDriveController : public ::testing::Test } } - void assignResources() + void assignResourcesPosFeedback() { std::vector 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 state_ifs; + state_ifs.emplace_back(left_wheel_vel_state_); + state_ifs.emplace_back(right_wheel_vel_state_); + + std::vector 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 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());