Fixes merge conflicts (humble backport 3077)

[TSNoble]

Description

Fixes merge conflicts in the backport PR

[mergify]

Please target the main branch for development, we will backport the changes to mergify/bp/humble/pr-3077 for you if approved and if they don't break API.

[rr-tom-noble]

I'm a little confused by the test failures:

    [unittest_trajectory_functions-1] The given transform is not an isometry! Its linear part involves non-unit scaling. The scaling matrix diagonal differs from [1 1 1] by [-1 -1 -1] but the required precision is 1e-12.
    [unittest_trajectory_functions-1] unittest_trajectory_functions: /home/runner/work/moveit2/moveit2/.work/target_ws/src/moveit2/moveit_core/robot_state/src/attached_body.cpp:67: moveit::core::AttachedBody::AttachedBody(const moveit::core::LinkModel*, const string&, const Isometry3d&, const std::vector<std::shared_ptr<const shapes::Shape> >&, const vector_Isometry3d&, const std::set<std::__cxx11::basic_string<char> >&, const JointTrajectory&, const FixedTransformsMap&): Assertion `!"Invalid isometry transform"' failed.

and

"/home/runner/work/moveit2/moveit2/.work/target_ws/src/moveit2/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_functions.cpp", line 230, in make_unique<moveit::core::AttachedBody, const moveit::core::LinkModel*&, const std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >&, const Eigen::Transform<double, 3, 1, 0>&, std::vector<std::shared_ptr<const shapes::Shape>, std::allocator<std::shared_ptr<const shapes::Shape> > >, std::vector<Eigen::Transform<double, 3, 1, 0>, Eigen::aligned_allocator<Eigen::Transform<double, 3, 1, 0> > >, std::set<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::less<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > >, trajectory_msgs::msg::JointTrajectory_<std::allocator<void> >, const std::map<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, Eigen::Transform<double, 3, 1, 0>, std::less<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >, Eigen::aligned_allocator<std::pair<const std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, Eigen::Transform<double, 3, 1, 0> > > >&>
    [unittest_trajectory_functions-1]     |   228:                                                const moveit::core::FixedTransformsMap& subframes)

seem to suggest it's an issue building collision objects... will investigate.

[rr-tom-noble]

The given transform is not an isometry!

This line in particular is a bit strange, as the only transforms we're passing to objects & subframes are identities, or isometries with an applied translation and rotation.

[rr-tom-noble]

  state.attachBody(std::make_unique<moveit::core::AttachedBody>(
      link, object_name, object_pose, std::vector<shapes::ShapeConstPtr>{}, EigenSTL::vector_Isometry3d{},
      std::set<std::string>{}, trajectory_msgs::msg::JointTrajectory{}, subframes));

maybe it's the empty shape isometry vector we're passing in?

[rr-tom-noble]

Weirdly the same test seems to be failing on the master branch.

    [unittest_trajectory_functions-1] [ RUN      ] TrajectoryFunctionsTestFlangeAndGripper.testIKRobotStateWithIdentityCollisionObject
    [unittest_trajectory_functions-1] [INFO] [1731505133.662894558] [unittest_trajectory_functions.moveit.ros.rdf_loader]: Loaded robot model in 0.00184161 seconds
    [unittest_trajectory_functions-1] [INFO] [1731505133.662935985] [unittest_trajectory_functions.moveit.core.robot_model]: Loading robot model 'prbt'...
    [unittest_trajectory_functions-1] [INFO] [1731505133.665731588] [ikfast]: Using link_prefix: 'prbt_'
    [unittest_trajectory_functions-1] The given transform is not an isometry! Its linear part involves non-unit scaling. The scaling matrix diagonal differs from [1 1 1] by [-1 -1 -1] but the required precision is 1e-12.
    [unittest_trajectory_functions-1] unittest_trajectory_functions: /home/runner/work/moveit2/moveit2/.work/target_ws/src/moveit2/moveit_core/robot_state/src/attached_body.cpp:67: moveit::core::AttachedBody::AttachedBody(const moveit::core::LinkModel*, const std::string&, const Eigen::Isometry3d&, const std::vector<std::shared_ptr<const shapes::Shape> >&, const EigenSTL::vector_Isometry3d&, const std::set<std::__cxx11::basic_string<char> >&, const trajectory_msgs::msg::JointTrajectory&, const moveit::core::FixedTransformsMap&): Assertion `!"Invalid isometry transform"' failed.
Error: ROR] [unittest_trajectory_functions-1]: process has died [pid 37461, exit code -6, cmd '/home/runner/work/moveit2/moveit2/.work/target_ws/install/pilz_industrial_motion_planner/lib/pilz_industrial_motion_planner/unittest_trajectory_functions --ros-args -r __node:=unittest_trajectory_functions --params-file /tmp/launch_params_5hcz6wul --params-file /tmp/launch_params_j8xlj9p_'].

I'll take a look into this tonight

[rr-tom-noble]

@sea-bass I tried reproducing the issues locally last night, but was unable to. A few observations:

• Master seems to be failing for the same reason, so it seems to be a general issue with Allow RobotState::setFromIK to work with subframes #3077 rather than just humble
• Only some pipelines seem to fail (these ones, master SonarScan) whereas some pass (Allow RobotState::setFromIK to work with subframes #3077's and the other master ones)
• The failure / success of a particular job seems to be non-sporadic. I haven't seen any failures pass (or vice versa)
• The scaling matrix diagonal differs from [1 1 1] by [-1 -1 -1] to me suggests default initialisation of an Isometry somewhere
• I'm unsure why the above would only happen on certain jobs, since the code is the same
• I'm also unsure why the tests on Allow RobotState::setFromIK to work with subframes #3077 passed for the same reason

If you get time, would you be able to run the pilz tests locally and see if you can reproduce the issue? No issues if not. Just want to check that my sanity is still intact 😛

[rr-tom-noble]

@sea-bass Tests should fixed now 🤞

Shoutout to @rr-mark for finding the issue in this line of code:

attachToLink(state, tip_link, "object", object_pose_in_tip, { {} });

Specifically, I was attempting to pass in an empty map of subframes, but was instead passing in a map containing one empty item. I've changed the non-subframe tests to pass in a subframe called "ignored" for parity with the other tests.

This will also need applying to rolling, so I'll raise a corresponding PR if these tests pass.

[rr-tom-noble]

@sjahr @sea-bass Success 🥳

Could I get a review when either of you are free?

[sjahr]

Puh, nice work and thanks for your perseverance