* Reverts #2985, Ports moveit #3388 #3470 #3539 (#3284)

* Revert "Fix RobotState::getRigidlyConnectedParentLinkModel() (#2985)"

This reverts commit 1f23344.

* Merge PR #3388: Generalize RobotState::setFromIK()

So far, setFromIK only accepted target (link) frames that were rigidly connected to a solver's tip frame.
This, for example, excluded the fingertip of an actuated gripper, because that would be separated by an
active joint from the arm's tooltip. However, as long as this joint is not part of the JMG,
the corresponding transform can be considered as fixed as well.

This PR generalizes the functions getRigidlyConnectedParentLinkModel() in
RobotState and RobotModel to receive an optional JMG pointer.
If present, only (active) joints from that group are considered non-fixed.
This PR also enables subframe support for setFromIK - simply by using
getRigidlyConnectedParentLinkModel(), which already supported that.

There is one drawback of this approach: A repeated application of setFromIK
with the same target frame and JMG (as in computeCartesianPath()), will
repeat the search for the common fixed parent link.
Additionally, the passed RobotState needs to be up-to-date.
We could mitigate this by pulling the corresponding code into a separate
function and calling it once in computeCartesianPath().

* Merge PR #3470: Avoid global transforms in getRigidlyConnectedParentLinkModel()

Fixes #3388

* Merge pull request #3539 from v4hn/find-links-with-slashes-again

find links with slashes again

* Ports to ROS2 and fixes problems introduced in merge conflicts.

* Fixes formatting.

* Makes robot_state_test.cpp include gmock.

* Updates trajectory_msgs::JointTrajectory to trajectory_msgs::msg::JointTrajectory.

* Adds braces to make clang-tidy happy.

* Removes test-only arguments; adds more comments.

* Fixes formatting.

* Fixes formatting.

* Adds missing class scope.

---------

Co-authored-by: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Co-authored-by: Robert Haschke <rhaschke@users.noreply.github.com>
Co-authored-by: Michael Görner <me@v4hn.de>
(cherry picked from commit 1794b8e)


* Resolves merge conflicts. (#3323)

---------

Co-authored-by: Mark Johnson <104826595+rr-mark@users.noreply.github.com>