[Servo] Make conversion operations into free functions

[ibrahiminfinite]

Description

Should be merged after #2146

This PR makes the following conversions into free functions and moves them into utilities.

1. Converting from command frame to MoveGroup frame
2. Converting change in cartesian position (delta_x) to a Pose

These changes help in keeping the code more readable and will make writing tests easier.

Checklist

• Required by CI: Code is auto formatted using clang-format

[codecov]

Codecov Report

Patch coverage: 100.00% and project coverage change: +0.04 🎉

Comparison is base (d2d27a7) 50.47% compared to head (b48ae7b) 50.51%.

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #2149      +/-   ##
==========================================
+ Coverage   50.47%   50.51%   +0.04%     
==========================================
  Files         387      387              
  Lines       31815    31816       +1     
==========================================
+ Hits        16056    16068      +12     
+ Misses      15759    15748      -11     

Impacted Files	Coverage Δ
moveit_ros/moveit_servo/src/servo_calcs.cpp	64.67% <100.00%> (-0.89%)	⬇️
moveit_ros/moveit_servo/src/utilities.cpp	84.27% <100.00%> (+11.73%)	⬆️

... and 2 files with indirect coverage changes

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

[sjahr]

Nice refactoring. In a follow-up PR we could think about adding unit tests here. Would you mind opening an issue for that?

[ibrahiminfinite]

If the cost of computing the transformation is not significant , we can compress the toPlanningFrame function to a more cleaner version.
What do you think @AndyZe , @sjahr

void toPlanningFrame(geometry_msgs::msg::TwistStamped& cmd, const std::string& planning_frame, const moveit::core::RobotStatePtr& current_state)
{
    if (cmd.header.frame_id.empty())
    {
        RCLCPP_WARN(LOGGER, "No frame specified for command , will use planning_frame");
        cmd.header.frame_id = planning_frame;
    }
    Eigen::Vector3d translation_vector(cmd.twist.linear.x, cmd.twist.linear.y, cmd.twist.linear.z);
    Eigen::Vector3d angular_vector(cmd.twist.angular.x, cmd.twist.angular.y, cmd.twist.angular.z);
    
    const auto tf_moveit_to_incoming_cmd_frame =
        current_state->getGlobalLinkTransform(planning_frame).inverse() *
        current_state->getGlobalLinkTransform(cmd.header.frame_id);
    translation_vector = tf_moveit_to_incoming_cmd_frame.linear() * translation_vector;
    angular_vector = tf_moveit_to_incoming_cmd_frame.linear() * angular_vector;

    // Put these components back into a TwistStamped
    cmd.header.frame_id = planning_frame;
    cmd.twist.linear.x = translation_vector(0);
    cmd.twist.linear.y = translation_vector(1);
    cmd.twist.linear.z = translation_vector(2);
    cmd.twist.angular.x = angular_vector(0);
    cmd.twist.angular.y = angular_vector(1);
    cmd.twist.angular.z = angular_vector(2);
}

[AndyZe]

Tested the functionality and it seems good 👍

[ibrahiminfinite]

If the cost of computing the transformation is not significant , we can compress the toPlanningFrame function to a more cleaner version. What do you think @AndyZe , @sjahr

void toPlanningFrame(geometry_msgs::msg::TwistStamped& cmd, const std::string& planning_frame, const moveit::core::RobotStatePtr& current_state)
{
    if (cmd.header.frame_id.empty())
    {
        RCLCPP_WARN(LOGGER, "No frame specified for command , will use planning_frame");
        cmd.header.frame_id = planning_frame;
    }
    Eigen::Vector3d translation_vector(cmd.twist.linear.x, cmd.twist.linear.y, cmd.twist.linear.z);
    Eigen::Vector3d angular_vector(cmd.twist.angular.x, cmd.twist.angular.y, cmd.twist.angular.z);
    
    const auto tf_moveit_to_incoming_cmd_frame =
        current_state->getGlobalLinkTransform(planning_frame).inverse() *
        current_state->getGlobalLinkTransform(cmd.header.frame_id);
    translation_vector = tf_moveit_to_incoming_cmd_frame.linear() * translation_vector;
    angular_vector = tf_moveit_to_incoming_cmd_frame.linear() * angular_vector;

    // Put these components back into a TwistStamped
    cmd.header.frame_id = planning_frame;
    cmd.twist.linear.x = translation_vector(0);
    cmd.twist.linear.y = translation_vector(1);
    cmd.twist.linear.z = translation_vector(2);
    cmd.twist.angular.x = angular_vector(0);
    cmd.twist.angular.y = angular_vector(1);
    cmd.twist.angular.z = angular_vector(2);
}

@AndyZe should we include this compressed version or go with the original ?

[AndyZe]

Prob want to throttle the warning:

RCLCPP_WARN_THROTTLE(LOGGER, ..., "No frame specified for command , will use planning_frame");

Keep this comment too:

    // We solve (planning_frame -> base -> cmd.header.frame_id)
    // by computing (base->planning_frame)^-1 * (base->cmd.header.frame_id)

Seems good to me besides that