Skip to content

Add velocity feedback option for diff_drive_controller #260

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 15 commits into from
Nov 18, 2021
Merged

Add velocity feedback option for diff_drive_controller #260

Show file tree
Hide file tree
Changes from 10 commits
Commits
Show all changes
15 commits
Select commit Hold shift + click to select a range
bc62802
Add velocity feedback option for diff_drive_controller
roncapat Oct 22, 2021
81f35b0
Merge branch 'ros-controls:master' into master
roncapat Nov 3, 2021
3a14e7b
Fix some >100 char lines for linter
roncapat Nov 3, 2021
cdec993
Fix for linter
roncapat Nov 3, 2021
8a005bb
Fix for linters
roncapat Nov 3, 2021
93c77b7
Update diff_drive_controller.cpp
roncapat Nov 3, 2021
2248fe3
Update odometry.cpp
roncapat Nov 3, 2021
02ce596
Update diff_drive_controller.cpp
roncapat Nov 3, 2021
1a442fe
Added configuration tests
roncapat Nov 6, 2021
4f79852
fix format
roncapat Nov 6, 2021
1fdb105
Implement review comments
roncapat Nov 8, 2021
dc58149
Format
roncapat Nov 8, 2021
352ec3a
Format
roncapat Nov 8, 2021
d31115a
Format
roncapat Nov 8, 2021
a45fd43
Format
roncapat Nov 8, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 3 additions & 1 deletion diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

We should probably also rename this in the follow-up PR to make the variable name clearer.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

You mean choosing a different name than "feedback"? Suggestions?

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think it's ok for now

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I am actually proposing to rename the "velocity" to "command" or something like this.

};

const char * feedback_type() const;
CallbackReturn configure_side(
const std::string & side, const std::vector<std::string> & wheel_names,
std::vector<WheelHandle> & registered_handles);
Expand All @@ -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";
Expand Down
1 change: 1 addition & 0 deletions diff_drive_controller/include/diff_drive_controller/odometry.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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();

Expand Down
80 changes: 60 additions & 20 deletions diff_drive_controller/src/diff_drive_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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);
Expand Down Expand Up @@ -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};
}
Expand Down Expand Up @@ -183,28 +189,59 @@ controller_interface::return_type DiffDriveController::update(
}
else
{
double left_position_mean = 0.0;
double right_position_mean = 0.0;
for (size_t index = 0; index < wheels.wheels_per_side; ++index)
if (odom_params_.position_feedback)
{
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();

if (std::isnan(left_position) || std::isnan(right_position))
double left_position_mean = 0.0;
double right_position_mean = 0.0;
for (size_t index = 0; index < wheels.wheels_per_side; ++index)
{
RCLCPP_ERROR(
logger, "Either the left or right wheel position is invalid for index [%zu]", index);
return controller_interface::return_type::ERROR;
const double left_position =
registered_left_wheel_handles_[index].feedback.get().get_value();
const double right_position =
registered_right_wheel_handles_[index].feedback.get().get_value();

if (std::isnan(left_position) || std::isnan(right_position))
{
RCLCPP_ERROR(
logger, "Either the left or right wheel position is invalid for index [%zu]", index);
return controller_interface::return_type::ERROR;
}

left_position_mean += left_position;
right_position_mean += right_position;
}
left_position_mean /= wheels.wheels_per_side;
right_position_mean /= wheels.wheels_per_side;

left_position_mean += left_position;
right_position_mean += right_position;
odometry_.update(left_position_mean, right_position_mean, current_time);
}
left_position_mean /= wheels.wheels_per_side;
right_position_mean /= wheels.wheels_per_side;
else
{
double left_velocity_mean = 0.0;
double right_velocity_mean = 0.0;
for (size_t index = 0; index < wheels.wheels_per_side; ++index)
{
const double left_velocity =
registered_left_wheel_handles_[index].feedback.get().get_value();
const double right_velocity =
registered_right_wheel_handles_[index].feedback.get().get_value();

if (std::isnan(left_velocity) || std::isnan(right_velocity))
{
RCLCPP_ERROR(
logger, "Either the left or right wheel velocity is invalid for index [%zu]", index);
return controller_interface::return_type::ERROR;
}

left_velocity_mean += left_velocity;
right_velocity_mean += right_velocity;
}
left_velocity_mean /= wheels.wheels_per_side;
right_velocity_mean /= wheels.wheels_per_side;

odometry_.update(left_position_mean, right_position_mean, current_time);
// Estimate linear and angular velocity using joint velocity information
odometry_.updateFromVelocity(left_velocity_mean, right_velocity_mean, current_time);
}
}

tf2::Quaternion orientation;
Expand Down Expand Up @@ -331,6 +368,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{
Expand Down Expand Up @@ -586,10 +624,12 @@ CallbackReturn DiffDriveController::configure_side(
registered_handles.reserve(wheel_names.size());
for (const auto & wheel_name : wheel_names)
{
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())
Expand Down
24 changes: 24 additions & 0 deletions diff_drive_controller/src/odometry.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -86,6 +86,30 @@ bool Odometry::update(double left_pos, double right_pos, const rclcpp::Time & ti
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 = (left_vel + right_vel) * 0.5;
// Now there is a bug about scout angular velocity
const double angular = (left_vel - right_vel) / wheel_separation_;

// Integrate odometry:
integrateExact(linear, angular);

timestamp_ = time;

// Estimate speeds using a rolling mean to filter them out:
linear_accumulator_.accumulate(linear / dt);
angular_accumulator_.accumulate(angular / dt);

linear_ = linear_accumulator_.getRollingMean();
angular_ = angular_accumulator_.getRollingMean();

return true;
}

void Odometry::updateOpenLoop(double linear, double angular, const rclcpp::Time & time)
{
/// Save last linear and angular velocity:
Expand Down
3 changes: 2 additions & 1 deletion diff_drive_controller/test/config/test_diff_drive_controller.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
diff_drive_controller:
test_diff_drive_controller:
ros__parameters:
left_wheel_names: ["left_wheels"]
right_wheel_names: ["right_wheels"]
Expand All @@ -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
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

is there other option missing: position_feedback: true

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

No, in my previous comments I also observed that this file is not being loaded correctly by the tests. So I just added the line as an example of configuration and reference, but it is not being used by tests.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Long story short, we need to address this ticket: #48

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@bmagyar is your reply to me or @destogl? To me it seems #48 is not a blocking PR for this one.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I was not meaning to block this PR with that comment, just cross referencing

open_loop: true
enable_odom_tf: true

Expand Down
79 changes: 74 additions & 5 deletions diff_drive_controller/test/test_diff_drive_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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_);
Expand All @@ -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_;

Expand All @@ -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_{
Expand Down Expand Up @@ -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);
Expand All @@ -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());
Expand Down Expand Up @@ -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());
Expand Down