引子

        最近做實驗的時候需要用到夾持器在機械臂基座座標系下的位姿，於是我呼叫了moveit的move_group::getCurrentPose()來獲取末端夾持器位姿，可是很憂傷地發現，實時性達不到要求，下面對如何解決這一問題進行分析與思考。

move_group::getCurrentPose()

        首先來看一下如何使用move_group::getCurrentPose()獲取指定末端關節相對於指定參考系的位姿：

#include <ros/ros.h>
#include <moveit/move_group_interface/move_group.h>
 

#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit/planning_scene_monitor/planning_scene_monitor.h>

int main(int argc, char** argv)
{
    ros::init(argc, argv, "gripper_pose");
    ros::NodeHandle nh;

    moveit::planning_interface::MoveGroup ur5_group("ur5_arm"
 
);
    ur5_group.setPoseReferenceFrame("/base");
    ros::Rate rate(10);
    while(ros::ok())
    {
        gripper_pose = ur5_group.getCurrentPose();
        rate.sleep();
    }
    return 0;
}

        我測試了一下，呼叫getCurrentPose()大概需要1s，這是遠遠達不到我的實時性要求的。為什麼要這麼長時間呢？不就是幾個矩陣連乘麼？
        如果使用tf_echo檢視兩個link之間的相對關係：

rosrun tf tf_echo /base /grasp_link

        其中/base為source_frame，/grasp_link為target_frame。執行結果為：

        從圖中來看的話，我們這裡有多棵tf tree，並且/grasp_link與/base並不在同一棵tf tree中。這一點從/tf話題的訊息中也可以看出來：

rostopic echo /tf

        可以看出來其中有兩組不同的值，整理如下：

第一組：

第二組：

        然後我們使用view_frames指令來檢視整體的tf tree：

rosrun tf view_frames
evince frames.pdf

        因為整張圖比較大，所以我們只用區域性圖進行分析：

        這是整棵tf tree中的一部分，觀察整棵tf tree可以發現，上面兩組tf在這棵樹中是有著各自的規律的：第一組表示的是moveable joints，第二組表示的是static joints。並且，第一組中的Most recent trasform為0.018 sec old，第二組中則為-0.492 sec old。這樣看來，在robot_state_publisher中，moveable joints資訊處理得較頻繁，而static joints則相對較緩慢。不過值得注意的是，在/tf話題中，這兩組資訊的頻率都很高，在我這裡是50Hz。那為什麼結合這兩組transform資訊計算末端link的位姿時，速度卻慢到需要1s呢？我猜，因為robot_state_publisher的開發者對moveable joints與static joints進行了區分，得到了兩組/tf訊息，而moveit的開發者使用的時候基於某些我不知道的考慮，寫出了一個較慢的getCurrentPose()函式……這裡我們就不深究了，還是自己寫一個末端link計算函式吧。

自定義計算函式

        上面我們說到了，兩組/tf資訊的釋出頻率都比較高，達到了50Hz，但是我們並不需要通過message_filters來對兩組訊息進行同步，因為static joints對應的transform是不變的。因此，我們只需要在static joints第一次到來時將其記錄下來，之後就不用再去理會static joints對應的/tf訊息了。
        整體程式碼如下：

#include <ros/ros.h>
#include <std_msgs/Header.h>
#include <tf2_msgs/TFMessage.h>
#include <geometry_msgs/PoseStamped.h>
#include <tf/transform_listener.h>
#include <vector>
#include <time.h>

geometry_msgs::PoseStamped gripper_pose;
ros::Publisher* pubPtr;

bool first_flag = true;
const char* forearm_link_str = "forearm_link";
const char* base_link_str = "base";

// This should be tf::StampedTransform, not tf::Transform, cause lookupTransform func needs this type.
tf::StampedTransform baselink_to_base;
tf::StampedTransform grasplink_to_wrist3link;

// The final transform.
tf::Transform grasplink_to_base;

// Save the transforms of the UR5.
std::vector<tf::Transform> transforms;
int tf_index[6] = {8, 7, 0, 9, 10, 11};

void tfMessageReceived(const tf2_msgs::TFMessage& tf_msg)
{
    const char* source_link_str = tf_msg.transforms[0].child_frame_id.c_str();

    if(first_flag == true && !strcmp(source_link_str, base_link_str))
    {
        first_flag = false;
        tf::TransformListener listener;
        // {0} --> target link (basis), {1} --> source link
        listener.waitForTransform("/base", "/base_link", ros::Time(0), ros::Duration(1));
        listener.lookupTransform("/base", "base_link", ros::Time(0), baselink_to_base);
        listener.lookupTransform("/wrist_3_link", "/grasp_link", ros::Time(0), grasplink_to_wrist3link);
    }


    if(!strcmp(source_link_str, forearm_link_str))
    {
        for(int i = 0; i < 6; i++)
        {
            int temp_index = tf_index[i];
            geometry_msgs::Transform geo_trans = tf_msg.transforms[temp_index].transform;

            // Convert geometry_msgs::Transform to tf::Transform
            tf::Transform temp_transform;
            tf::transformMsgToTF(geo_trans, temp_transform);

            transforms.push_back(temp_transform);
        }

        // The final transform. tf::Transform type.
        grasplink_to_base = baselink_to_base * transforms[0] * transforms[1] * transforms[2] \
                        * transforms[3] * transforms[4] * transforms[5] *  grasplink_to_wrist3link;

        // Convert tf::Transform to geometry_msgs::Transform.
        geometry_msgs::Transform geo_trans_total;
        tf::transformTFToMsg(grasplink_to_base, geo_trans_total);

        // Construct the geometry_msgs::PoseStamped.
        gripper_pose.header.seq++;
        gripper_pose.header.stamp = ros::Time::now();
        gripper_pose.pose.position.x = geo_trans_total.translation.x;
        gripper_pose.pose.position.y = geo_trans_total.translation.y;
        gripper_pose.pose.position.z = geo_trans_total.translation.z;
        gripper_pose.pose.orientation.x = geo_trans_total.rotation.x;
        gripper_pose.pose.orientation.y = geo_trans_total.rotation.y;
        gripper_pose.pose.orientation.z = geo_trans_total.rotation.z;
        gripper_pose.pose.orientation.w = geo_trans_total.rotation.w;
        pubPtr->publish(gripper_pose);
        transforms.clear();
    }
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "gripper_pose");
    ros::NodeHandle nh;


    pubPtr = new ros::Publisher(nh.advertise<geometry_msgs::PoseStamped>("/ur_pkg_udef/gripper_pose", 1000));

    ros::spin();

    return 0;
}

        在上面的程式碼中，我們要計算的是grasp_link相對於base的位姿，於是我們先將grasp_link相對於wrist_3_link的transform以及base_link相對於base的transform記錄下來，然後再對/tf中的moveable joints訊息進行處理，利用tf::Transform型別的* operator進行計算，從而得到我們所需要的grasplink_to_base。值得注意的是，這只是一個示例，大家需要根據自己的實際情形來對程式碼進行改寫~

        上面這段程式碼可以達到與/tf中的moveable_joints一致的釋出頻率，也即差不多50Hz，這樣我們的目的就達成了~