Add velocity feedback option for diff_drive_controller #260

roncapat · 2021-10-22T19:27:10Z

Implements #229.
Builds on a fresh Ubuntu 20.04 ROS2 Dashing install alongside the master branch of ros2-controls.

roncapat · 2021-11-03T07:50:40Z

I hope today to already fix the linter errors :)

roncapat · 2021-11-03T08:43:08Z

Only sync to master for now, no need to re-run workflows till I fix the other errors :)

roncapat · 2021-11-03T09:43:19Z

I enabled workflows on my fork. Actually, the only one failing is this one. Can someone help me with the log? It seems not direclty connected to my code, that seems to pass all tests.

roncapat · 2021-11-04T12:05:14Z

The issue on my fork workflows is related with test_force_torque_sensor_broadcaster. All other tests pass. Is there a fix?

bmagyar · 2021-11-05T07:45:32Z

Sorry for the delay on this:
First comment that will keep you busy for a bit :D can you please add a test for this?

roncapat · 2021-11-05T07:51:44Z

I may try... Never done. What behaviour I should test?

roncapat · 2021-11-05T15:15:15Z

In the meantime I try to learn about writing test, I paste here the log of the failing job:

   2: [ RUN      ] ForceTorqueSensorBroadcasterTest.SensorName_Publish_Success
  2: 1636098896.664650 [0] test_force: using network interface eth0 (udp/10.1.1.107) selected arbitrarily from: eth0, docker0
  2: /home/runner/work/ros2_controllers/ros2_controllers/ros_ws/src/cav7w0ygslo/ros2_controllers/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp:96: Failure
  2: Expected equality of these values:
  2:   wait_for(subscription)
  2:     Which is: 1
  2:   rclcpp::WaitResultKind::Ready
  2:     Which is: 0
  2: /home/runner/work/ros2_controllers/ros2_controllers/ros_ws/src/cav7w0ygslo/ros2_controllers/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp:260: Failure
  2: Expected equality of these values:
  2:   wrench_msg.header.frame_id
  2:     Which is: ""
  2:   frame_id_
  2:     Which is: "fts_sensor_frame"
  2: [  FAILED  ] ForceTorqueSensorBroadcasterTest.SensorName_Publish_Success (20168 ms)
  2: [ RUN      ] ForceTorqueSensorBroadcasterTest.InterfaceNames_Publish_Success
  2: 1636098916.833366 [0] test_force: using network interface eth0 (udp/10.1.1.107) selected arbitrarily from: eth0, docker0
  2: /home/runner/work/ros2_controllers/ros2_controllers/ros_ws/src/cav7w0ygslo/ros2_controllers/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp:96: Failure
  2: Expected equality of these values:
  2:   wait_for(subscription)
  2:     Which is: 1
  2:   rclcpp::WaitResultKind::Ready
  2:     Which is: 0
  2: /home/runner/work/ros2_controllers/ros2_controllers/ros_ws/src/cav7w0ygslo/ros2_controllers/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp:284: Failure
  2: Expected equality of these values:
  2:   wrench_msg.header.frame_id
  2:     Which is: ""
  2:   frame_id_
  2:     Which is: "fts_sensor_frame"
  2: [  FAILED  ] ForceTorqueSensorBroadcasterTest.InterfaceNames_Publish_Success (20211 ms)

From the source code, seems that the subscription times out. FYI

roncapat · 2021-11-06T17:00:04Z

@bmagyar I ran locally the existing tests, but noticed that the test YAML file is not taken into account! (I vary values, recompile, run test and print some parameters and nothing changes).

In CMake:

ament_add_gmock(test_diff_drive_controller
    test/test_diff_drive_controller.cpp 
    ENV config_file=${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_diff_drive_controller.yaml)

but parameters passing in this way is documented nowhere and seems to me not to work correctly. Are we sure that up to now it has always worked well?

Moreover, the code gives the controller the name test_diff_drive_controller during init but the YAML is for diff_drive_controller.

roncapat · 2021-11-06T17:41:04Z

I learned to write basic tests. In particular, now there are test that validate the effectiveness of the new parameter I introduced on the loading of other position or velocity feedback interfaces.

diff_drive_controller/src/odometry.cpp

diff_drive_controller/src/diff_drive_controller.cpp

bmagyar · 2021-11-08T08:56:16Z

Thanks for the work on this @roncapat and indeed probably that way of configuring tests may not work anymore as we moved from one distro to another. I'll create a separate issue to look into that.

roncapat · 2021-11-08T09:03:45Z

@bmagyar Thank you for the review, I'll implement your comments as soon as I can :)

roncapat · 2021-11-08T11:50:42Z

I addressed all comments. Now I run the format workflow on my branch and push format fixes if needed.

roncapat · 2021-11-08T11:58:12Z

@bmagyar done :)

bmagyar

Thanks very much for this!

destogl

@roncapat, @bmagyar please check/address my comments

destogl · 2021-11-09T17:25:19Z

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;


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

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

I think it's ok for now

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

destogl · 2021-11-09T17:41:23Z

diff_drive_controller/src/diff_drive_controller.cpp

+    }
+    else
+    {
+      odometry_.updateFromVelocity(left_feedback_mean, right_feedback_mean, current_time);


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);

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

we should have period available in the update method

destogl · 2021-11-09T17:44:56Z

diff_drive_controller/test/config/test_diff_drive_controller.yaml

@@ -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


is there other option missing: position_feedback: true

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.

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

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

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

roncapat · 2021-11-15T11:59:44Z

@destogl any feedbacks on the review answers? I may have some time this week to fix/implement/polish so that we can eventually merge.

destogl

OK, let's move with it and address my other comments in the follow-up PRs.

wmmc88 · 2021-12-30T18:59:28Z

diff_drive_controller/src/diff_drive_controller.cpp

+    }
+    else
+    {
+      odometry_.updateFromVelocity(left_feedback_mean, right_feedback_mean, current_time);


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

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.

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?

@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).

@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).

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.

Add velocity feedback option for diff_drive_controller

bc62802

roncapat mentioned this pull request Oct 26, 2021

diff_drive_controller shall accept velocity feedback from motors #229

Closed

Merge branch 'ros-controls:master' into master

81f35b0

roncapat added 6 commits November 3, 2021 10:14

Fix some >100 char lines for linter

3a14e7b

Fix for linter

cdec993

Fix for linters

8a005bb

Update diff_drive_controller.cpp

93c77b7

Update odometry.cpp

2248fe3

Update diff_drive_controller.cpp

02ce596

bmagyar requested review from bmagyar and destogl November 5, 2021 07:44

Added configuration tests

1a442fe

fix format

4f79852

roncapat mentioned this pull request Nov 6, 2021

force_torque_sensor_broadcaster tests fail on Galactic Source Job in GitHub Workflows #263

Closed

bmagyar reviewed Nov 8, 2021

View reviewed changes

diff_drive_controller/src/odometry.cpp Show resolved Hide resolved

bmagyar reviewed Nov 8, 2021

View reviewed changes

diff_drive_controller/src/diff_drive_controller.cpp Outdated Show resolved Hide resolved

bmagyar reviewed Nov 8, 2021

View reviewed changes

diff_drive_controller/src/diff_drive_controller.cpp Outdated Show resolved Hide resolved

Implement review comments

1fdb105

roncapat added 4 commits November 8, 2021 12:53

Format

dc58149

Format

352ec3a

Format

d31115a

Format

a45fd43

bmagyar approved these changes Nov 9, 2021

View reviewed changes

destogl reviewed Nov 9, 2021

View reviewed changes

destogl approved these changes Nov 18, 2021

View reviewed changes

destogl merged commit d86cffa into ros-controls:master Nov 18, 2021

This was referenced Nov 18, 2021

[DiffDriveController] Rename "velocity" to "command" or even better "velocity_command" #270

Closed

[DiffDriveController] use "period" argument in the update method insead of calculating "dt" #271

Open

wmmc88 reviewed Dec 30, 2021

View reviewed changes

PaulVerhoeckx mentioned this pull request Jan 5, 2022

Diff_drive_controller's odometry rotates in wrong direction #280

Closed

roncapat mentioned this pull request Jan 19, 2022

Wrong integration of velocity feedback in diff_drive_controller #286

Closed

roncapat mentioned this pull request Apr 16, 2022

fix odometry issue in diff_drive_controller #331

Merged

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

Add velocity feedback option for diff_drive_controller #260

Add velocity feedback option for diff_drive_controller #260

Conversation

roncapat commented Oct 22, 2021

roncapat commented Nov 3, 2021

roncapat commented Nov 3, 2021

roncapat commented Nov 3, 2021 • edited Loading

roncapat commented Nov 4, 2021

bmagyar commented Nov 5, 2021 • edited Loading

roncapat commented Nov 5, 2021

roncapat commented Nov 5, 2021

roncapat commented Nov 6, 2021 • edited Loading

roncapat commented Nov 6, 2021

bmagyar commented Nov 8, 2021

roncapat commented Nov 8, 2021

roncapat commented Nov 8, 2021

roncapat commented Nov 8, 2021

bmagyar left a comment

Choose a reason for hiding this comment

destogl left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

roncapat Nov 10, 2021 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

roncapat commented Nov 15, 2021

destogl left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

roncapat commented Nov 3, 2021 •

edited

Loading

bmagyar commented Nov 5, 2021 •

edited

Loading

roncapat commented Nov 6, 2021 •

edited

Loading

roncapat Nov 10, 2021 •

edited

Loading