Use emulated time in action-based controller

[galou]

Signed-off-by: Gaël Écorchard gael.ecorchard@cvut.cz

Description

future.wait_for() should not be used with a time obtained from node_->now() because of the mix between simulated and system time. This PR fix the issue that the action server would time out with a very slow simulated time.

Note that for technical reasons (cf. below) I cannot compile main but that same changes compile when based on foxy.

PS: can someone point me to the documentation to test a PR on the main branch? I use Foxy and I'm struggling with compiling the main branch. The documentation states that I could base my PR on foxy but my last PR was rejected because of this.

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

[vatanaksoytezer]

Hi @galou thanks a lot for the contributions!

PS: can someone point me to the documentation to test a PR on the main branch? I use Foxy and I'm struggling with compiling the main branch. The documentation states that I could base my PR on foxy but my last PR was rejected because of this.

We should update the documentation on this, it was a recent effort we have taken to merge everything to main and backport if there are no API / ABI breaks.

main branch would compile on both galactic and rolling. It should be sufficient to follow the source build instructions on the MoveIt website to build our main branch. Let me know if you have any troubles while doing so, and I'll be happy to help

[vatanaksoytezer]

Oh, also CI seems broken right now (not related to your PR), I'm working on fixing it.

[galou]

@vatanaksoytezer thanks for offering your help. I start the discussion here, please write me if it should continue elsewhere. The error I get with Rolling is:

--- stderr: moveit_hybrid_planning                                             
/usr/bin/ld: CMakeFiles/hybrid_planning_demo_node.dir/hybrid_planning_demo_node.cpp.o: in function `rclcpp_action::Client<moveit_msgs::action::HybridPlanner>::Client(std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface>, std::shared_ptr<rclcpp::node_interfaces::NodeGraphInterface>, std::shared_ptr<rclcpp::node_interfaces::NodeLoggingInterface>, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, rcl_action_client_options_t const&)':
/opt/ros/rolling/include/rclcpp_action/client.hpp:422: undefined reference to `rosidl_action_type_support_t const* rosidl_typesupport_cpp::get_action_type_support_handle<moveit_msgs::action::HybridPlanner>()'
collect2: error: ld returned 1 exit status
make[2]: *** [test/CMakeFiles/hybrid_planning_demo_node.dir/build.make:330: test/hybrid_planning_demo_node] Error 1
make[1]: *** [CMakeFiles/Makefile2:577: test/CMakeFiles/hybrid_planning_demo_node.dir/all] Error 2
make: *** [Makefile:141: all] Error 2
---
Failed   <<< moveit_hybrid_planning [13.8s, exited with code 2]

[tylerjw]

I bet that is because you need a newer version of moveit_msgs. Currently in ros2 if you have a package installed (into /opt) and have it in your workspace it doesn't correctly use the one in your workspace which can cause errors like this. Assuming you have moveit_msgs in your workspace, try uninstalling it with apt and re-building.

sudo apt remove ros-rolling-moveit-msgs

This is something that is on the Humble roadmap and should hopefully be fixed soon in colcon in a way that can work even if you can't upgrade to Humble right away.

[galou]

The issue with moveit_msgs is solved, I managed to compile on Rolling. Thanks!

[vatanaksoytezer]

@galou there seems a few legitimate build errors on this PR in CI, do you mind fixing them? I think this is a really nice PR that we would like to merge soon.

[codecov]

Codecov Report

Base: 50.95% // Head: 50.95% // No change to project coverage 👍

Coverage data is based on head (fda1223) compared to base (edc4fea).
Patch has no changes to coverable lines.

Additional details and impacted files

@@           Coverage Diff           @@
##             main     #899   +/-   ##
=======================================
  Coverage   50.95%   50.95%           
=======================================
  Files         378      378           
  Lines       31658    31658           
=======================================
  Hits        16129    16129           
  Misses      15529    15529           

Impacted Files	Coverage Δ
...dl_kinematics_plugin/src/kdl_kinematics_plugin.cpp	78.79% <0.00%> (-1.13%)	⬇️
moveit_core/robot_state/src/robot_state.cpp	47.83% <0.00%> (-0.07%)	⬇️
...nning_scene_monitor/src/planning_scene_monitor.cpp	45.65% <0.00%> (+0.44%)	⬆️

Help us with your feedback. Take ten seconds to tell us how you rate us. Have a feature suggestion? Share it here.

☔ View full report at Codecov.
📢 Do you have feedback about the report comment? Let us know in this issue.

[vatanaksoytezer]

@galou do you mind running pre-commit for this PR? I think it all looks good, we just need the correct formatting to merge.

[galou]

The pre-commit hook was not configured on my system, sorry.

[tylerjw]

I rebased these changes and tested them locally, and they seem to work. Approving and merging assuming CI passes.

[AndyZe]

@tylerjw let's not jump the gun. 2 reviews needed. Hopefully I'll approve soon.