Cartesian twist controller #300

livanov93 · 2022-02-23T12:09:49Z

PR info:

add twist controller (cartesian controller) using input type message geometry_msgs/TwistStamped

TODOs:

add tests

gavanderhoorn · 2022-02-23T12:32:12Z

cartesian_controllers/src/twist_controller.cpp

+  if (command_interfaces_.size() != 6)
+  {
+    RCLCPP_ERROR_THROTTLE(
+      get_node()->get_logger(), *node_->get_clock(), 1000,
+      "Twist controller needs does not match number of interfaces needed 6, given (%zu) interfaces",
+      command_interfaces_.size());
+    return controller_interface::return_type::ERROR;
+  }


(edit) should this should be checked in on_configure(..)?

malapatiravi

looks good to me. someone else must approve.

aprotyas

Looks mostly fine to me, but I've left a few nits in-line.

I think you need to rebase your branch on ros-controls:master for CI to pass, since your branch doesn't have ros2_controllers_test_nodes (e7d0517) yet.

Also, looks like the linter is unhappy.

aprotyas · 2022-02-27T05:26:46Z

cartesian_controllers/package.xml

+  <maintainer email="lovro.ivanov@gmail.com">Lovro Ivanov</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>


Since find_package(ament_cmake_ros REQUIRED) was introduced in 1f1fefc:

Suggested change

<buildtool_depend>ament_cmake</buildtool_depend>

<buildtool_depend>ament_cmake</buildtool_depend>

<buildtool_depend>ament_cmake_ros</buildtool_depend>

It doesn't like this pacakge needs ament_cmake_ros I'll remove from the CMakeLists.txt

Don't add it, remove it from CMakeLists.txt

aprotyas · 2022-02-27T08:18:45Z

cartesian_controllers/src/twist_controller.cpp

+  }
+  catch (const std::exception & e)
+  {
+    fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what());


Pardon my ignorance here, but could this not be logged with RCLCPP_ERROR_* instead?

This is actually quite often a pattern in these use-cases.

aprotyas · 2022-02-27T08:23:09Z

cartesian_controllers/src/twist_controller.cpp

+  command_interfaces_[0].set_value((*twist_commands)->twist.linear.x);
+  command_interfaces_[1].set_value((*twist_commands)->twist.linear.y);
+  command_interfaces_[2].set_value((*twist_commands)->twist.linear.z);
+  command_interfaces_[3].set_value((*twist_commands)->twist.angular.x);
+  command_interfaces_[4].set_value((*twist_commands)->twist.angular.y);
+  command_interfaces_[5].set_value((*twist_commands)->twist.angular.z);


Nit. Not that it matters, but purely for brevity. Feel free to ignore:

Suggested change

command_interfaces_[0].set_value((*twist_commands)->twist.linear.x);

command_interfaces_[1].set_value((*twist_commands)->twist.linear.y);

command_interfaces_[2].set_value((*twist_commands)->twist.linear.z);

command_interfaces_[3].set_value((*twist_commands)->twist.angular.x);

command_interfaces_[4].set_value((*twist_commands)->twist.angular.y);

command_interfaces_[5].set_value((*twist_commands)->twist.angular.z);

auto command = (*twist_commands)->twist;

command_interfaces_[0].set_value(command.linear.x);

command_interfaces_[1].set_value(command.linear.y);

command_interfaces_[2].set_value(command.linear.z);

command_interfaces_[3].set_value(command.angular.x);

command_interfaces_[4].set_value(command.angular.y);

command_interfaces_[5].set_value(command.angular.z);

@livanov93

This Commit builds on a rebased PR ros-controls#300 The Original author @livanov93 was in 2022 open sourcing part of a PickNik package form 2021. This commit brings the internal picknik and previously open PR ros-controls#300 back in sync and rebases the open PR onto humble.

moriarty · 2023-05-01T18:46:48Z

@livanov93 I've rebased this branch onto Humble and brought it in sync with the internal version from PickNik for open sourcing.
livanov93@19a6068

Should we force push onto this PR or close this one out and open a new one?

livanov93 · 2023-05-01T19:07:33Z

@moriarty Feel free to force-push here.

mergify · 2023-05-02T14:11:17Z

This pull request is in conflict. Could you fix it @livanov93?

moriarty · 2023-05-02T14:15:08Z

@livanov93 I've rebased this branch onto Humble and brought it in sync with the internal version from PickNik for open sourcing. livanov93@19a6068

Should we force push onto this PR or close this one out and open a new one?

I've forced pushed but the internal version was targeting humble so that's what I'd rebased on.
To bring it up to date with master I'll need to first do a bit more testing.

@livanov93

This Commit builds on a rebased PR ros-controls#300 The Original author @livanov93 was in 2022 open sourcing part of a PickNik package form 2021. This commit brings the internal picknik and previously open PR ros-controls#300 back in sync and rebases the open PR onto humble.

Signed-off-by: Alex Moriarty <alex.moriarty@picknik.ai>

Unclear what the state of these tests were from original PR and if they were finished because it was marked as Work in Progress. This change makes the tests pass Signed-off-by: Alex Moriarty <alex.moriarty@picknik.ai>

moriarty · 2023-05-03T13:18:37Z

cartesian_controllers/test/test_twist_controller.cpp

@@ -43,7 +43,7 @@ TEST_F(TwistControllerTest, joint_names_parameter_not_set)
  ASSERT_TRUE(controller_->joint_name_.empty());
  ASSERT_TRUE(controller_->interface_names_.empty());

-  controller_->get_node()->set_parameter({"joint", joint_name_});
+  controller_->get_node()->set_parameter({"interface_names", interface_names_});


This test was called joint_names_parameter_not_set but was setting the joint param.
The following test was called interface_parameter_not_set but was setting interface_name parameter.

These two tests just check to it isn't possible to configure the controller with missing mandatory parameters.

That is correct, could be that additional work was required.

We have to swith to generate_parameter_library anyway and then those tests get partially obsolete.

moriarty · 2023-05-03T13:23:25Z

cartesian_controllers/test/test_twist_controller.cpp

-    ASSERT_EQ(wait_for(controller_->twist_command_subscriber_), rclcpp::WaitResultKind::Ready);
-
-    // process callbacks
-    rclcpp::spin_some(controller_->get_node()->get_node_base_interface());
+    ASSERT_TRUE(controller_->wait_for_commands(executor));


This wait_for resulted in rclcpp throwing an exception, there was a wait_for_commands which looks like it was intended for this. A similar pattern exists in the other controller tests.

ros2_controllers/admittance_controller/test/test_admittance_controller.cpp

Lines 268 to 269 in 6793e02

publish_commands();

ASSERT_TRUE(controller_->wait_for_commands(executor));

moriarty · 2023-05-03T13:28:47Z

looks good to me. someone else must approve.

@malapatiravi

Looks mostly fine to me, but I've left a few nits in-line.

I think you need to rebase your branch on ros-controls:master for CI to pass, since your branch doesn't have ros2_controllers_test_nodes (e7d0517) yet.

Also, looks like the linter is unhappy.

@aprotyas

I've taken over this Branch & PR from @livanov93 and it's ready for another review.
I'm unable to re-request reviews or remove the [WIP] because I didn't open the PR.
The tests should be passing, but if someone could trigger CI that would be great then I can take a look at any failures

Signed-off-by: Alex Moriarty <alex.moriarty@picknik.ai>

livanov93

@moriarty Looks fine, please take a look on the suggestions. Although I can't approve as PR owner

livanov93 · 2023-05-03T13:46:06Z

cartesian_controllers/CMakeLists.txt

+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(ament_cmake_ros REQUIRED)


This is not needed I think

yes I removed in 87717e7

livanov93 · 2023-05-03T13:46:25Z

cartesian_controllers/CMakeLists.txt

+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(ament_cmake_ros REQUIRED)


Suggested change

find_package(ament_cmake_ros REQUIRED)

livanov93 · 2023-05-03T13:47:35Z

cartesian_controllers/package.xml

+  <maintainer email="lovro.ivanov@gmail.com">Lovro Ivanov</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>


Don't add it, remove it from CMakeLists.txt

livanov93 · 2023-05-03T13:49:50Z

cartesian_controllers/test/test_twist_controller.cpp

@@ -43,7 +43,7 @@ TEST_F(TwistControllerTest, joint_names_parameter_not_set)
  ASSERT_TRUE(controller_->joint_name_.empty());
  ASSERT_TRUE(controller_->interface_names_.empty());

-  controller_->get_node()->set_parameter({"joint", joint_name_});
+  controller_->get_node()->set_parameter({"interface_names", interface_names_});


That is correct, could be that additional work was required.

codecov-commenter · 2023-05-03T14:21:56Z

Codecov Report

Merging #300 (87717e7) into master (e7f9962) will decrease coverage by 3.36%.
The diff coverage is 26.44%.

📣 This organization is not using Codecov’s GitHub App Integration. We recommend you install it so Codecov can continue to function properly for your repositories. Learn more

@@            Coverage Diff             @@
##           master     #300      +/-   ##
==========================================
- Coverage   35.78%   32.43%   -3.36%     
==========================================
  Files         189        7     -182     
  Lines       17570      666   -16904     
  Branches    11592      358   -11234     
==========================================
- Hits         6287      216    -6071     
+ Misses        994      157     -837     
+ Partials    10289      293    -9996

Flag	Coverage Δ
unittests	`32.43% <26.44%> (-3.36%)`	⬇️

Flags with carried forward coverage won't be shown. Click here to find out more.

Impacted Files	Coverage Δ
...ontroller/test/test_load_diff_drive_controller.cpp	`11.11% <0.00%> (ø)`
diff_drive_controller/src/odometry.cpp	`42.16% <11.11%> (ø)`
diff_drive_controller/src/speed_limiter.cpp	`46.55% <11.11%> (ø)`
...ive_controller/test/test_diff_drive_controller.cpp	`17.62% <12.08%> (ø)`
...troller/include/diff_drive_controller/odometry.hpp	`20.00% <20.00%> (ø)`
...iff_drive_controller/src/diff_drive_controller.cpp	`39.22% <35.50%> (ø)`
...de/diff_drive_controller/diff_drive_controller.hpp	`100.00% <100.00%> (ø)`

... and 189 files with indirect coverage changes

moriarty · 2023-05-03T18:03:58Z

@moriarty Looks fine, please take a look on the suggestions. Although I can't approve as PR owner

👍 and as I am not PR owner I can't mark any comments as resolved 😆

I will run the fixup the formatting. The Rolling - ABI compatibility is known to be broken. I noticed it when testing locally and had to clone control_msgs to get ros-controls/control_msgs#86 and it is causing a lot of CI jobs to fail #565 (comment)

Signed-off-by: Alex Moriarty <alex.moriarty@picknik.ai>

destogl

Recommend renaming the controller to ForwardTwistController

@bmagyar should we then add those into forwarding_controllers package? We should probably create one then. Then we can also clean a bit of a mess with position_controllers, velocity_controllers and so on. Which are mostly forwarding controllers anyway. IMHO we have too many packages, and can reduce the amount of code by large putting all those controllers together.

destogl · 2023-05-04T18:53:21Z

cartesian_controllers/CMakeLists.txt

Please update file according to current standard of other controllers

destogl · 2023-05-04T18:53:36Z

cartesian_controllers/include/cartesian_controllers/twist_controller.hpp

@@ -0,0 +1,72 @@
+// Copyright 2021, PickNik Inc.


Suggested change

// Copyright 2021, PickNik Inc.

// Copyright 2023, PickNik Inc.

2021 is correct, its the date this file was created.

destogl · 2023-05-04T18:54:45Z

cartesian_controllers/include/cartesian_controllers/twist_controller.hpp

+  realtime_tools::RealtimeBuffer<std::shared_ptr<CmdType>> rt_command_ptr_;
+  rclcpp::Subscription<CmdType>::SharedPtr twist_command_subscriber_;
+
+  std::string logger_name_;


We don't use this anymore. We rather use get_node()->get_logger() to get an appropriate logger name. This is a pattern in other controllers.

destogl · 2023-05-04T18:54:59Z

cartesian_controllers/include/cartesian_controllers/visibility_control.h

@@ -0,0 +1,49 @@
+// Copyright 2021, PickNik Inc.


Suggested change

// Copyright 2021, PickNik Inc.

// Copyright 2023, PickNik Inc.

destogl · 2023-05-04T18:56:04Z

cartesian_controllers/src/twist_controller.cpp

@@ -0,0 +1,149 @@
+// Copyright 2021, PickNik Inc.


Suggested change

// Copyright 2021, PickNik Inc.

// Copyright 2023, PickNik Inc.

destogl · 2023-05-04T18:56:57Z

cartesian_controllers/src/twist_controller.cpp

+{
+  try
+  {
+    auto_declare<std::vector<std::string>>("interface_names", std::vector<std::string>());


Please convert this to use generate_parameter_library

destogl · 2023-05-04T18:57:48Z

cartesian_controllers/src/twist_controller.cpp

+  }
+  catch (const std::exception & e)
+  {
+    fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what());


This is actually quite often a pattern in these use-cases.

destogl · 2023-05-04T19:00:23Z

cartesian_controllers/src/twist_controller.cpp

+  }
+
+  twist_command_subscriber_ = get_node()->create_subscription<CmdType>(
+    "~/commands", rclcpp::SystemDefaultsQoS(),


We use reference now. See this file

Suggested change

"~/commands", rclcpp::SystemDefaultsQoS(),

"~/reference", rclcpp::SystemDefaultsQoS(),

destogl · 2023-05-04T19:01:54Z

cartesian_controllers/test/test_twist_controller.cpp

@@ -43,7 +43,7 @@ TEST_F(TwistControllerTest, joint_names_parameter_not_set)
  ASSERT_TRUE(controller_->joint_name_.empty());
  ASSERT_TRUE(controller_->interface_names_.empty());

-  controller_->get_node()->set_parameter({"joint", joint_name_});
+  controller_->get_node()->set_parameter({"interface_names", interface_names_});


We have to swith to generate_parameter_library anyway and then those tests get partially obsolete.

* Add picknik_controllers The purpose of picknik_controllers repository is hold controllers that we are in the process of open sourcing and upstreaming. The controllers in this repository have been prefixed with `picknik_` so that if they are accepted upstream we will drop the prefix and release the prefixed versions with deprication warnings. - picknik_reset_fault_controller has been discussed here https://roscontrol.slack.com/archives/C01HA09KDLH/p1686332900429009 - picknik_twist_controller ros-controls/ros2_controllers#300 These do not have a gbp-release package and have not been release they are currently source only. Following the steps outlined here: https://docs.ros.org/en/rolling/How-To-Guides/Releasing/First-Time-Release.html This PR is for step 2 1. Be part of a release team: ros2-gbp/ros2-gbp-github-org#281 2. https://docs.ros.org/en/rolling/How-To-Guides/Releasing/Release-Team-Repository.html#create-a-new-release-repository Signed-off-by: Alex Moriarty <alex.moriarty@picknik.ai> * fixup: remove the release Signed-off-by: Alex Moriarty <alex.moriarty@picknik.ai> * fixup repo typo - PickNikRobots -> PickNikRobotics Signed-off-by: Alex Moriarty <alex.moriarty@picknik.ai> --------- Signed-off-by: Alex Moriarty <alex.moriarty@picknik.ai>

@moriarty

Increasing version of package(s) in repository `picknik_controllers` to `0.0.1-1`: - upstream repository: https://github.com/PickNikRobotics/picknik_controllers.git - release repository: https://github.com/ros2-gbp/picknik_controllers-release.git - distro file: `rolling/distribution.yaml` - bloom version: `0.11.2` - previous version for package: `null` ``` * Initial Release of picknik_reset_fault_controller * Originally this was used internally and there was an attempt to release it to ros2_controllers * After discussion with the ros2 controllers WG over slack we have decided to open source it here first * The goal is to still move this upstream but it can be worked on here first and moved in the future * fault_controller -> picknik_reset_fault_controller (ros#2 <PickNikRobotics/picknik_controllers#2>) * fault_controller -> picknik_reset_fault_controller This commit does two things: 1) renames fault_controller to reset_fault_controller 2) prefixes with picknik_ The first change, is to be more specific what this controller is used for. The second change is because we want to move this controller into ros2_controllers and when that is complete we can drop the picknik_ and depricate this version allowing for a transition period. --------- * Contributors: Alexander Moriarty @moriarty, Anthony Baker @abake48, @livanov93, @destogl, @MarqRazz, @Abishalini, @JafarAbdi ``` ``` * Initial Release of picknik_twist_controller * Originally this was used internally and there was an attempt to release it to ros2_controllers here: ros-controls/ros2_controllers#300 * The goal is to still move this upstream but it needs to be refactored before going upstream. * twist_controller -> picknik_twist_controller (ros#3 <PickNikRobotics/picknik_controllers#3>) * twist_controller -> picknik_twist_controller 1. prefix twist_controller with picknik_twist_controller When we merge twist_controller into ros2_controllers we can depricate this one and not have naming conflicts as users migrate * cmake: 3.8 -> 3.16 bump to oldest version used on Ubuntu Focal + ROS 2 Humble https://www.ros.org/reps/rep-2000.html#humble-hawksbill-may-2022-may-2027 --------- * Contributors: Alexander Moriarty @moriarty, Anthony Baker @abake48, @livanov93, @destogl, @MarqRazz, @Abishalini, @JafarAbdi ``` Signed-off-by: Alexander Moriarty <alex.moriarty@picknik.ai>

github-actions bot requested review from aprotyas, bmagyar, destogl, kasiceo, kellyprankin, malapatiravi and TomoyaFujita2016 February 23, 2022 12:10

gavanderhoorn reviewed Feb 23, 2022

View reviewed changes

malapatiravi reviewed Feb 27, 2022

View reviewed changes

aprotyas suggested changes Feb 27, 2022

View reviewed changes

moriarty force-pushed the twist-controller branch from 2aec5fc to 19a6068 Compare May 2, 2023 14:10

livanov93 and others added 6 commits May 2, 2023 11:18

Introduce cartesian twist controller.

1d5d4dc

Remove unused-parameter and return-type warnings.

9c6d2c1

Add pluginlib export based on ControllerInterface.

b7187c9

Add cartesian controllers test.

4bf2cc5

Correct package.xml/=.

8f31cf7

moriarty force-pushed the twist-controller branch from 19a6068 to 5bda0a8 Compare May 2, 2023 14:20

moriarty added 2 commits May 2, 2023 15:00

fix typo in Twist Controller Tests

c312b69

Signed-off-by: Alex Moriarty <alex.moriarty@picknik.ai>

Twist Controller fixup old tests

5c39225

Unclear what the state of these tests were from original PR and if they were finished because it was marked as Work in Progress. This change makes the tests pass Signed-off-by: Alex Moriarty <alex.moriarty@picknik.ai>

moriarty reviewed May 3, 2023

View reviewed changes

livanov93 changed the title ~~[WIP] Cartesian twist controller~~ Cartesian twist controller May 3, 2023

livanov93 requested a review from malapatiravi May 3, 2023 13:41

livanov93 requested review from aprotyas, gavanderhoorn and moriarty May 3, 2023 13:41

remove unused ament_cmake_ros

87717e7

Signed-off-by: Alex Moriarty <alex.moriarty@picknik.ai>

livanov93 commented May 3, 2023

View reviewed changes

run pre-commit to fixup formatting

d99b3af

Signed-off-by: Alex Moriarty <alex.moriarty@picknik.ai>

destogl requested changes May 4, 2023

View reviewed changes

abake48 mentioned this pull request May 10, 2023

Move twist_controller to picknik_controllers Kinovarobotics/ros2_kortex#83

Closed

moriarty mentioned this pull request Jun 20, 2023

Add picknik_controllers ros/rosdistro#37736

Merged

3 tasks

	<buildtool_depend>ament_cmake</buildtool_depend>
	<buildtool_depend>ament_cmake</buildtool_depend>
	<buildtool_depend>ament_cmake_ros</buildtool_depend>

-  command_interfaces_[0].set_value((*twist_commands)->twist.linear.x);
-  command_interfaces_[1].set_value((*twist_commands)->twist.linear.y);
-  command_interfaces_[2].set_value((*twist_commands)->twist.linear.z);
-  command_interfaces_[3].set_value((*twist_commands)->twist.angular.x);
-  command_interfaces_[4].set_value((*twist_commands)->twist.angular.y);
-  command_interfaces_[5].set_value((*twist_commands)->twist.angular.z);
+  auto command = (*twist_commands)->twist;
+  command_interfaces_[0].set_value(command.linear.x);
+  command_interfaces_[1].set_value(command.linear.y);
+  command_interfaces_[2].set_value(command.linear.z);
+  command_interfaces_[3].set_value(command.angular.x);
+  command_interfaces_[4].set_value(command.angular.y);
+  command_interfaces_[5].set_value(command.angular.z);

	publish_commands();
	ASSERT_TRUE(controller_->wait_for_commands(executor));

	// Copyright 2021, PickNik Inc.
	// Copyright 2023, PickNik Inc.

	"~/commands", rclcpp::SystemDefaultsQoS(),
	"~/reference", rclcpp::SystemDefaultsQoS(),

Cartesian twist controller #300

Are you sure you want to change the base?

Cartesian twist controller #300

Conversation

livanov93 commented Feb 23, 2022

gavanderhoorn Feb 23, 2022 • edited

Choose a reason for hiding this comment

malapatiravi left a comment

Choose a reason for hiding this comment

aprotyas 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

moriarty commented May 1, 2023

livanov93 commented May 1, 2023

mergify bot commented May 2, 2023

moriarty commented May 2, 2023

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

moriarty commented May 3, 2023

livanov93 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

codecov-commenter commented May 3, 2023

Codecov Report

moriarty commented May 3, 2023

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

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

gavanderhoorn Feb 23, 2022 •

edited