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 all 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
49 changes: 33 additions & 16 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,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);
Copy link
Member

Choose a reason for hiding this comment

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

Why not use period instead of calculating it manually?

Suggested change
odometry_.updateFromVelocity(left_feedback_mean, right_feedback_mean, current_time);
odometry_.updateFromVelocity(left_feedback_mean, right_feedback_mean, period);

Copy link
Contributor Author

@roncapat roncapat Nov 10, 2021 •
edited
Loading

Choose a reason for hiding this comment

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

You mean calculating const double dt = time.seconds() - timestamp_.seconds();' outside updateFromVelocity() and update() instead of inside? This will remove redundancy but still allow (dt < 0.0001) check for update() call

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 have period available in the update method

Copy link

Choose a reason for hiding this comment

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

Shouldnt this be left_feedback_mean * dt and right_feedback_mean * dt since left_vel and right_vel used in integrateExact are supposed to be the position deltas and not the instantaneous velocities? In the position_feedback == true path, left_vel and right_vel are passed as position deltas. The fork that the feedback_param was based off of also multiplies by dt

Copy link
Contributor Author

Choose a reason for hiding this comment

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

Sorry, I'm from mobile now, but your line of thought seems reasonable at a first look. In these days I'm on vacation so I can't evaluate and fix, maybe it is worth pushing a new PR with the fixes you have in mind explaining there what's wrong with current code.

Copy link

Choose a reason for hiding this comment

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

It also doesn't really make sense to me why both pose and twist get computed from vel when this param is enabled. Doesn't it make more sense to compute odom pose using position deltas calculated from the position interface and then use instantaneous vel from the velocity interface to calculate the odom twist? This is the approach that the clearpath fork ended up using, allowing for two params to disable this behavior (originally implemented in clearpathrobotics/ros_controllers#18, but available in their latest indigo branch). Thoughts on this?

Copy link
Contributor Author

Choose a reason for hiding this comment

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

@wmmc88 do you opened any issue or PR related to this?

For your last message, I undestand that you are talking about using simultaneously the feedback position and velocity interfaces. Is is correct? In my undestanding, for now there is only support here for one of the two IF/s at time, but using both if available could be interesting (not to be seen as a bug, but as a feature enhancement).

Copy link

Choose a reason for hiding this comment

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

@wmmc88 do you opened any issue or PR related to this?

nope. havent opened an issue and haven't had any time to address it myself in a pr.

For your last message, I undestand that you are talking about using simultaneously the feedback position and velocity interfaces. Is is correct? In my undestanding, for now there is only support here for one of the two IF/s at time, but using both if available could be interesting (not to be seen as a bug, but as a feature enhancement).

yeah this wasn't meant to be an issue with this pr. I'm more just suggesting as an enhancement that the pose be calculated by the position interface data and the twist be calculated from the velocity interface data. I believe that is a more optimal solution and that is what the linked clearpath fork did(they reverted a change similar to this pr and went with the pose from position/twist from velocity approach).

Copy link
Contributor Author

Choose a reason for hiding this comment

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

I reviewed now, and you are true. Since the code calls "vel" the position delta (misleadingly), I made confusion. A multiplication with dt will convert back instantaneous velocity to deltas. Might now also consider to do the multiplication inside updateFromVelocity() to hide this detail at interface level. Will open PR now.

}
}

tf2::Quaternion orientation;
Expand Down Expand Up @@ -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{
Expand Down Expand Up @@ -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())
Expand Down
13 changes: 11 additions & 2 deletions diff_drive_controller/src/odometry.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
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