Enable moveit2 kinematics framework within ros2 control loop #1329

pac48 · 2022-06-09T16:04:00Z

Description

Previously, the RDFLoader and RobotModelLoader classes required a rclcpp::Node node in order to load a robot from the ROS network. Since the ros2_control controllers now run within a rclcpp_lifecycle/LifecycleNode node, there is no way to use Moveit2's kinematic framework.

However, the rclcpp::Node and LifecycleNode nodes do provide getter methods for common interfaces, e.g. get_node_base_interface(), get_node_parameters_interface(), and get_node_topics_interface(). My approach to solve the issue was to refactor the RobotModelLoader and RDFLoader classes to use the provided common interfaces, rather than a rclcpp::Node. Since these classes pass a rclcpp::Node reference to KinematicsPluginLoader, which depends on KinematicsBase, I has to modify both of those classes as well.

Checklist

Required by CI: Code is auto formatted using clang-format
Extend the tutorials / documentation reference
Document API changes relevant to the user in the MIGRATION.md notes
Create tests, which fail without this PR reference
Include a screenshot if changing a GUI
While waiting for someone to review your request, please help review another open pull request to support the maintainers

codecov · 2022-06-09T17:31:06Z

Codecov Report

Merging #1329 (77e8020) into main (4a16b05) will increase coverage by 0.01%.
The diff coverage is 77.39%.

@@            Coverage Diff             @@
##             main    #1329      +/-   ##
==========================================
+ Coverage   61.56%   61.57%   +0.01%     
==========================================
  Files         274      275       +1     
  Lines       24965    25064      +99     
==========================================
+ Hits        15368    15431      +63     
- Misses       9597     9633      +36

Impacted Files	Coverage Δ
...e/include/moveit/kinematics_base/kinematics_base.h	`75.76% <ø> (ø)`
.../rdf_loader/include/moveit/rdf_loader/rdf_loader.h	`100.00% <ø> (ø)`
...ude/moveit/robot_model_loader/robot_model_loader.h	`100.00% <ø> (ø)`
...oveit_node_interface/src/moveit_node_interface.cpp	`53.34% <53.34%> (ø)`
...ning/robot_model_loader/src/robot_model_loader.cpp	`76.05% <74.20%> (-1.72%)`	⬇️
...oveit_core/kinematics_base/src/kinematics_base.cpp	`28.58% <80.00%> (+3.26%)`	⬆️
...g/rdf_loader/src/synchronized_string_parameter.cpp	`82.90% <87.10%> (-1.48%)`	⬇️
moveit_ros/planning/rdf_loader/src/rdf_loader.cpp	`57.66% <88.24%> (+4.20%)`	⬆️
...ics_plugin_loader/src/kinematics_plugin_loader.cpp	`69.90% <94.12%> (+0.16%)`	⬆️
...kinematic_constraints/src/kinematic_constraint.cpp	`74.20% <100.00%> (ø)`
... and 6 more

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update 4a16b05...77e8020. Read the comment docs.

moveit_core/package.xml

moveit_core/kinematic_constraints/src/kinematic_constraint.cpp

henningkayser · 2022-06-09T16:23:46Z

moveit_core/kinematics_base/src/kinematics_base.cpp

+                                double /*search_discretization*/)
+{
+  RCLCPP_ERROR(LOGGER,
+               "IK plugin for group '%s' relies on deprecated API. "


deprecated API should be annotated with [[deprecated("reason")]]. But this function is new. Could you clarify this?

For some reason kinematics_plugin_loader.cpp is calling this deprecated code in on of its methods. I don't see why at the moment. The reason I added the method is because it the existing KinematicsBase::initialize needed a node as an input. Now that you point it out, I can just remove the node input instead of overloading it.

Actually I take that back. KinematicsBase::initialize is a virtual method. I'm guessing it needs a default implementation because it is a plugin. The error message will occur if the user forgets to define the initialize message. So its is saying that their code is deprecated, not ours.

henningkayser · 2022-06-09T17:31:05Z

.../kinematics_plugin_loader/include/moveit/kinematics_plugin_loader/kinematics_plugin_loader.h

+  {
+  }
+
+  KinematicsPluginLoader(const NodeInterfaceSharedPtr& node_interface, const TopicsInterfaceSharedPtr& topics_interface,


It's really not pretty that we have to pass all these interface types here. I'd prefer something leaner, maybe a struct that wraps these interfaces, or a virtual class that provides these getter functions (which Node should have imo).

Where should that struct be located? Since these changes are across multiple packages, would it make sense to create a utility package in /moveit_ros that define a wrapper struct?

That's the question... We've discussed abstracting from the RCLCPP interfaces in the past, and came up with some DI approach for increased testability. #562 and #569 are examples for classes that provide ROS features without direct API usage. However, these interfaces were very specific to their classes. It would be great, if we could come up with a more generic approach for the node interfaces, which technically could be used all over the codebase. I'm not saying that this needs to be done with this PR, but if we could reduce it to one type, refactoring would be much easier in the future.

Since these changes are across multiple packages, would it make sense to create a utility package in /moveit_ros that define a wrapper struct?

Yes, that would make sense to me in this case.

Tagging @tylerjw @griswaldbrooks for their opinion.

I made a package called moveit_node_interface that wraps the lifecycle and regular node.

...anning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h

moveit_ros/planning/rdf_loader/include/moveit/rdf_loader/rdf_loader.h

moveit_ros/planning/rdf_loader/include/moveit/rdf_loader/synchronized_string_parameter.h

moveit_ros/planning/rdf_loader/src/rdf_loader.cpp

moveit_ros/planning/rdf_loader/src/synchronized_string_parameter.cpp

moveit_setup_assistant/src/tools/compute_default_collisions.cpp

moveit_ros/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp

tylerjw

I really like the approach of this. This seems like a really straightforward way to abstract the two types of Nodes in ros2 for the IK interface.

moveit_ros/moveit_node_interface/src/node_interface.cpp

moveit_ros/planning/CHANGELOG.rst

.../kinematics_plugin_loader/include/moveit/kinematics_plugin_loader/kinematics_plugin_loader.h

moveit_ros/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp

moveit_ros/moveit_node_interface/include/node_interface/node_interface.h

tylerjw · 2022-06-12T05:41:59Z

moveit_core/kinematics_base/src/kinematics_base.cpp

+        return initialize(node->get_rcl_node(), robot_model, group_name, base_frame, tip_frames,
+                          search_discretization);


I'm confused by this, if the node interface is the lifecycle one, it won't initalize?

Because I changed KinematicsPluginLoader::KinematicsLoaderImpl to call KinematicsBase::initialize with NodeInterfaceSharedPtr instead of rclcpp::Node::SharedPtr, IK plugins that use rclcpp::Node::SharedPt would now never get loaded. If this code is hit, it means the user did not load a plugin with the NodeInterfaceSharedPtr interface. So I need to call KinematicsBase::initialize with rclcpp::Node::SharedPtr in case that interface is being used.

sjahr

I like this improvement of the API

moveit_core/CMakeLists.txt

moveit_core/CHANGELOG.rst

moveit_core/kinematic_constraints/src/kinematic_constraint.cpp

moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h

moveit_core/kinematics_base/src/kinematics_base.cpp

moveit_ros/moveit_node_interface/include/node_interface/node_interface.h

sjahr · 2022-06-13T08:28:47Z

moveit_ros/moveit_node_interface/include/node_interface/node_interface.h

+
+#pragma once
+
+#include <rclcpp/rclcpp.hpp>


Can we replace this with more specific headers? Apparently, including the whole rclcpp header increases the build time a lot --> #1333

Replaced with #include <rclcpp/node.hpp>

How about forward-declaring the node types and only including the interface headers here? This should speed up build times even more.

Do you know if I can forward declare node like this:
class Node : public std::enable_shared_from_this<Node>

Unless build times are a problem, I don't want us to start forward declaring all over the place in the name of compile times. I'd rather we first aggressively seek out more granularly headers as #1333 does before we start resorting to clever tricks.

I saw this in my email and got worried. I see you aren't doing it here, that was just a trick for forward declaration.

Some of the most confusing interface designs I've seen started with enable_shared_from_this. Please avoid putting that on any class you create.

Compile time is surely an issue with MoveIt, and this header might have a big impact if we use it all over the codebase at some point. I agree that we shouldn't use shared_from_this, though.

.../kinematics_plugin_loader/include/moveit/kinematics_plugin_loader/kinematics_plugin_loader.h

moveit_ros/planning/package.xml

moveit_ros/planning/rdf_loader/include/moveit/rdf_loader/rdf_loader.h

mergify · 2022-06-14T15:00:17Z

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

…ssue

…aces

mergify · 2022-06-15T22:20:16Z

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

…ol-loop

henningkayser · 2022-06-16T14:19:45Z

moveit_ros/moveit_node_interface/include/moveit_node_interface/moveit_node_interface.hpp

+
+    rclcpp::node_interfaces::NodeParametersInterface::SharedPtr get_node_parameters_interface() override;
+
+    std::optional<std::shared_ptr<rclcpp::Node>> get_rcl_node() const override;


Why would you expose the nodes at all? Also, since this is already a template, I would say you could make this a single function get_node() and return T (with T being the node type, not the shared_ptr type)

This is to avoid ros2_control depending on moveit2. We can't have ros2_control take the new moveit_node_interface, so the best we can do is pass ros2_control a lifecycle node. I agree on the template part.

… github.com:pac48/moveit2 into pr-enable-moveit-kinimatic-within-ros2-control-loop

mergify · 2022-07-18T16:56:36Z

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

mergify · 2022-09-27T22:33:48Z

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

tylerjw · 2022-11-18T03:09:18Z

@destogl do you know if this change is still needed for something in ros2_control or should I close this?

@pac48 do you remember where we ended up with this change?

pac48 · 2022-11-18T03:19:57Z

@destogl do you know if this change is still needed for something in ros2_control or should I close this?

@pac48 do you remember where we ended up with this change?

This change originally was meant to allow MoveIt's kinematics plugins to be used by lifecycle nodes. This was a requirement for ros2_control because controller are implemented using lifecycle nodes. Since ros2_control now has its own bare bones kinematics interface, there is currently no need to introduce MoveIt's kinematics to ros2_control.

However, this change provides a consistent node interface for MoveIt. It has some value on its own. I don't have a strong opinion either way.

tylerjw · 2022-11-18T03:29:29Z

@pac48 thank you for responding. I'm going to close it for now and open an issue referencing this as a possible implementation for when someone gets time to revisit the issue of having a generic interface to the two types of nodes.

pac48 requested review from sjahr and tylerjw June 9, 2022 16:05

henningkayser reviewed Jun 9, 2022

View reviewed changes

tylerjw reviewed Jun 10, 2022

View reviewed changes

moveit_ros/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp Outdated Show resolved Hide resolved

tylerjw reviewed Jun 12, 2022

View reviewed changes

sjahr reviewed Jun 13, 2022

View reviewed changes

pac48 force-pushed the pr-enable-moveit-kinimatic-within-ros2-control-loop branch 2 times, most recently from 3258c97 to c4c4bb8 Compare June 13, 2022 21:40

pac48 added 20 commits June 15, 2022 10:55

added support for common node interfaces

124cc60

applied auto formatting

66b64cb

removed unused includes and comments

46ad9b7

added basic test for lifecycle node and fixed parameter declaration i…

c5cc8c8

…ssue

added more RDFLoader tests

bc1fd70

updated CMakeLists.txt

a4e5baf

cleaned up unused code

6f97a9d

code auto format

57f1445

applied clang-tidy Linting

9df1b60

ran pre-commit

baf29e3

ran pre-commit

24a3dfb

added NodeInterface wrapper struct in replace of provided node interf…

3d8ee5d

…aces

integrated interface into packages

4f86090

applied formatting

81b4ef5

remove white space

d7874f0

remove white space from moveit_core/CMakeLists.txt

b7e6fa6

fixed formatting

1042076

node interface fix ik test failure

1d947ba

fixed license and split node interface into src and header file

5e71085

added getter for lifecycle node

9399b03

pac48 added 10 commits June 15, 2022 10:56

fixed undefined symbols error

f3eb68f

chnaged shared pointer copy to std::move

4624838

changed additional shared pointer copy to std::move

b872237

cleaned up initializers and unused code

b0802a7

fixed return type from node interface

1f8a4ca

added moveit namespace to node interface + std::move change

545ad64

fixed clang format

a8dee26

addressed nits

a1e372e

ignore non-passing cmake tests

a1e75c9

removoe maintainer element

da89abb

pac48 force-pushed the pr-enable-moveit-kinimatic-within-ros2-control-loop branch from 97e5062 to da89abb Compare June 15, 2022 16:58

pac48 mentioned this pull request Jun 15, 2022

Chainable Admittance Controller #1371

Closed

pac48 and others added 3 commits June 15, 2022 16:46

merge upstream

188e8ab

re-added maintainer and changed order of package tags

6bf2ff8

Merge branch 'main' into pr-enable-moveit-kinimatic-within-ros2-contr…

4b78931

…ol-loop

henningkayser reviewed Jun 16, 2022

View reviewed changes

pac48 added 2 commits June 16, 2022 16:43

add NO_LINT to interface

8da869e

Merge branch 'pr-enable-moveit-kinimatic-within-ros2-control-loop' of…

77e8020

… github.com:pac48/moveit2 into pr-enable-moveit-kinimatic-within-ros2-control-loop

pac48 requested review from henningkayser and tylerjw June 21, 2022 02:49

henningkayser mentioned this pull request Sep 6, 2022

Adding support for lifecycle nodes #1538

Open

tylerjw closed this Nov 18, 2022

tylerjw mentioned this pull request Nov 18, 2022

Support both Node and LifecycleNode #1737

Open

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Enable moveit2 kinematics framework within ros2 control loop #1329

Enable moveit2 kinematics framework within ros2 control loop #1329

pac48 commented Jun 9, 2022 •

edited

Loading

codecov bot commented Jun 9, 2022 •

edited

Loading

henningkayser Jun 9, 2022

pac48 Jun 9, 2022

pac48 Jun 9, 2022

henningkayser Jun 9, 2022

pac48 Jun 9, 2022

henningkayser Jun 9, 2022

pac48 Jun 13, 2022

tylerjw left a comment

tylerjw Jun 12, 2022

pac48 Jun 12, 2022 •

edited

Loading

sjahr left a comment

sjahr Jun 13, 2022

pac48 Jun 13, 2022

henningkayser Jun 13, 2022

pac48 Jun 13, 2022 •

edited

Loading

ChrisThrasher Jun 13, 2022 •

edited

Loading

tylerjw Jun 13, 2022

henningkayser Jun 16, 2022

mergify bot commented Jun 14, 2022

mergify bot commented Jun 15, 2022

henningkayser Jun 16, 2022

pac48 Jun 16, 2022

mergify bot commented Jul 18, 2022

mergify bot commented Sep 27, 2022

tylerjw commented Nov 18, 2022

pac48 commented Nov 18, 2022

tylerjw commented Nov 18, 2022

		return initialize(node->get_rcl_node(), robot_model, group_name, base_frame, tip_frames,
		search_discretization);


		rclcpp::node_interfaces::NodeParametersInterface::SharedPtr get_node_parameters_interface() override;

		std::optional<std::shared_ptr<rclcpp::Node>> get_rcl_node() const override;

Enable moveit2 kinematics framework within ros2 control loop #1329

Enable moveit2 kinematics framework within ros2 control loop #1329

Conversation

pac48 commented Jun 9, 2022 • edited Loading

Description

Checklist

codecov bot commented Jun 9, 2022 • edited Loading

Codecov Report

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

tylerjw left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

pac48 Jun 12, 2022 • edited Loading

Choose a reason for hiding this comment

sjahr 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

pac48 Jun 13, 2022 • edited Loading

Choose a reason for hiding this comment

ChrisThrasher Jun 13, 2022 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

mergify bot commented Jun 14, 2022

mergify bot commented Jun 15, 2022

Choose a reason for hiding this comment

Choose a reason for hiding this comment

mergify bot commented Jul 18, 2022

mergify bot commented Sep 27, 2022

tylerjw commented Nov 18, 2022

pac48 commented Nov 18, 2022

tylerjw commented Nov 18, 2022

pac48 commented Jun 9, 2022 •

edited

Loading

codecov bot commented Jun 9, 2022 •

edited

Loading

pac48 Jun 12, 2022 •

edited

Loading

pac48 Jun 13, 2022 •

edited

Loading

ChrisThrasher Jun 13, 2022 •

edited

Loading