-
Notifications
You must be signed in to change notification settings - Fork 523
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.
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
Enable moveit2 kinematics framework within ros2 control loop #1329
Enable moveit2 kinematics framework within ros2 control loop #1329
Conversation
Codecov Report
@@ 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
Continue to review full report at Codecov.
|
double /*search_discretization*/) | ||
{ | ||
RCLCPP_ERROR(LOGGER, | ||
"IK plugin for group '%s' relies on deprecated API. " |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
deprecated API should be annotated with [[deprecated("reason")]]. But this function is new. Could you clarify this?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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.
{ | ||
} | ||
|
||
KinematicsPluginLoader(const NodeInterfaceSharedPtr& node_interface, const TopicsInterfaceSharedPtr& topics_interface, |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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).
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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
Outdated
Show resolved
Hide resolved
moveit_ros/planning/rdf_loader/include/moveit/rdf_loader/rdf_loader.h
Outdated
Show resolved
Hide resolved
moveit_ros/planning/rdf_loader/include/moveit/rdf_loader/synchronized_string_parameter.h
Outdated
Show resolved
Hide resolved
moveit_ros/planning/rdf_loader/src/synchronized_string_parameter.cpp
Outdated
Show resolved
Hide resolved
moveit_setup_assistant/src/tools/compute_default_collisions.cpp
Outdated
Show resolved
Hide resolved
moveit_ros/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp
Outdated
Show resolved
Hide resolved
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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.
.../kinematics_plugin_loader/include/moveit/kinematics_plugin_loader/kinematics_plugin_loader.h
Outdated
Show resolved
Hide resolved
.../kinematics_plugin_loader/include/moveit/kinematics_plugin_loader/kinematics_plugin_loader.h
Outdated
Show resolved
Hide resolved
moveit_ros/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp
Outdated
Show resolved
Hide resolved
moveit_ros/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp
Outdated
Show resolved
Hide resolved
moveit_ros/moveit_node_interface/include/node_interface/node_interface.h
Outdated
Show resolved
Hide resolved
return initialize(node->get_rcl_node(), robot_model, group_name, base_frame, tip_frames, | ||
search_discretization); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I'm confused by this, if the node interface is the lifecycle one, it won't initalize?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I like this improvement of the API
moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h
Outdated
Show resolved
Hide resolved
moveit_ros/moveit_node_interface/include/node_interface/node_interface.h
Outdated
Show resolved
Hide resolved
|
||
#pragma once | ||
|
||
#include <rclcpp/rclcpp.hpp> |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Can we replace this with more specific headers? Apparently, including the whole rclcpp header increases the build time a lot --> #1333
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Replaced with #include <rclcpp/node.hpp>
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
How about forward-declaring the node types and only including the interface headers here? This should speed up build times even more.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Do you know if I can forward declare node like this:
class Node : public std::enable_shared_from_this<Node>
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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
Outdated
Show resolved
Hide resolved
moveit_ros/planning/rdf_loader/include/moveit/rdf_loader/rdf_loader.h
Outdated
Show resolved
Hide resolved
3258c97
to
c4c4bb8
Compare
This pull request is in conflict. Could you fix it @pac48? |
97e5062
to
da89abb
Compare
This pull request is in conflict. Could you fix it @pac48? |
|
||
rclcpp::node_interfaces::NodeParametersInterface::SharedPtr get_node_parameters_interface() override; | ||
|
||
std::optional<std::shared_ptr<rclcpp::Node>> get_rcl_node() const override; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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)
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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
This pull request is in conflict. Could you fix it @pac48? |
This pull request is in conflict. Could you fix it @pac48? |
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. |
@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. |
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