Ports moveit/moveit/pull/3519 to ros2

[rr-tom-noble]

Description

Ports moveit/moveit#3519 to ROS2. This may partially fix #2857 for the LIN planner

Checklist

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

[rr-tom-noble]

I'm a little confused about the errors I'm seeing in the pipeline:

  --- stderr: pilz_industrial_motion_planner
  In file included from /opt/ros/rolling/src/gtest_vendor/include/gtest/gtest-printers.h:122,
                   from /opt/ros/rolling/src/gtest_vendor/include/gtest/gtest-matchers.h:49,
                   from /opt/ros/rolling/src/gtest_vendor/include/gtest/internal/gtest-death-test-internal.h:47,
                   from /opt/ros/rolling/src/gtest_vendor/include/gtest/gtest-death-test.h:43,
                   from /opt/ros/rolling/src/gtest_vendor/include/gtest/gtest.h:65,
                   from /home/runner/work/moveit2/moveit2/.work/target_ws/src/moveit2/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_functions.cpp:35:
  /opt/ros/rolling/src/gtest_vendor/include/gtest/internal/gtest-internal.h: In instantiation of ‘testing::Test* testing::internal::TestFactoryImpl<TestClass>::CreateTest() [with TestClass = TrajectoryFunctionsTestFlangeAndGripper_testIsRobotStateStationaryAccelerationUnequal_Test]’:
  /opt/ros/rolling/src/gtest_vendor/include/gtest/internal/gtest-internal.h:457:9:   required from here
  /opt/ros/rolling/src/gtest_vendor/include/gtest/internal/gtest-internal.h:457:40: error: use of deleted function ‘TrajectoryFunctionsTestFlangeAndGripper_testIsRobotStateStationaryAccelerationUnequal_Test::TrajectoryFunctionsTestFlangeAndGripper_testIsRobotStateStationaryAccelerationUnequal_Test()’
    457 |   Test* CreateTest() override { return new TestClass; }
        |                                        ^~~~~~~~~~~~~
  /home/runner/work/moveit2/moveit2/.work/target_ws/src/moveit2/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_functions.cpp:969:8: note: ‘TrajectoryFunctionsTestFlangeAndGripper_testIsRobotStateStationaryAccelerationUnequal_Test::TrajectoryFunctionsTestFlangeAndGripper_testIsRobotStateStationaryAccelerationUnequal_Test()’ is implicitly deleted because the default definition would be ill-formed:
    969 | TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testIsRobotStateStationaryAccelerationUnequal)
        |        ^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  /home/runner/work/moveit2/moveit2/.work/target_ws/src/moveit2/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_functions.cpp:969:8: error: use of deleted function ‘TrajectoryFunctionsTestFlangeAndGripper::TrajectoryFunctionsTestFlangeAndGripper()’
...

I don't believe I've changed the fixtures for those tests, and the only changes I've made are adding a field to a fixture for other tests, populating that field in the test setup, and deleting some tests which are no longer relevant

[rr-tom-noble]

Ah, I think the issue is that the fixture I modified is the base fixture for all tests in that file. I'm instantiating a robot state using moveit::core::RobotState(robot_model_) which I think may not be a thing in ROS?

[rr-tom-noble]

Oh although it does look like such a constructor exists:

  /** \brief A state can be constructed from a specified robot model. No values are initialized.
      Call setToDefaultValues() if a state needs to provide valid information. */
  RobotState(const RobotModelConstPtr& robot_model);

@sjahr Would you be able to advise?

[rr-tom-noble]

Snippets from TrajectoryFunctionTestBase::SetUp():

    // load robot model
    rm_loader_ = std::make_unique<robot_model_loader::RobotModelLoader>(node_);
    robot_model_ = rm_loader_->getModel();
    ASSERT_TRUE(bool(robot_model_)) << "Failed to load robot model";
    robot_state_ = moveit::core::RobotState(robot_model_);
    planning_scene_ = std::make_shared<planning_scene::PlanningScene>(robot_model_);

and from TrajectoryFunctionsTestBase protected members:

  // ros stuff
  rclcpp::Node::SharedPtr node_;
  moveit::core::RobotModelConstPtr robot_model_;
  moveit::core::RobotState robot_state_;
  std::unique_ptr<robot_model_loader::RobotModelLoader> rm_loader_;
  planning_scene::PlanningSceneConstPtr planning_scene_;

[rr-tom-noble]

I wonder if the issue is that it's trying to default construct a RobotState before Setup()? Should I replace the value type with a pointer type?

[rr-tom-noble]

Looks like that did the trick

[rr-tom-noble]

@sjahr Would you be free to take a look at this when free? I think the remaining failures are sporadic and / or unrelated as they are in different packages (gz_ros2_control and robotiq_driver respectively). Also, would it be possible to backport the fix to humble once merged (and could you explain what the process for doing so is?)

Thanks!

[sjahr]

Thanks for porting this over! Just some minor comments that don't block the merge. Just notify me if you're addressing them or we should just go ahead. Regarding the backport:
Since this doesn't break the API, I think we can easily do it. There's a github bot we're using for that and I will be active once this PR is merged. Tyler wrote a good summary of our current branch policies here: https://moveit.ai/moveit%202/ros/2023/05/31/balancing-stability-and-development.html
If you're interested

[rr-tom-noble]

Thanks for porting this over! Just some minor comments that don't block the merge. Just notify me if you're addressing them or we should just go ahead. Regarding the backport: Since this doesn't break the API, I think we can easily do it. There's a github bot we're using for that and I will be active once this PR is merged. Tyler wrote a good summary of our current branch policies here: https://moveit.ai/moveit%202/ros/2023/05/31/balancing-stability-and-development.html If you're interested

@sjahr Thanks for the quick response! I think I'd like to get this merged ASAP if that's alright as we're currently blocked on failing LIN plans. If you could backport to humble too that'd be much appreciated. Just taking a look over that doc now. That moveit2_packages repository sounds awesome. I was imagining having to fork & build-from-source to test the fix in our stack 😀