引子
最近做实验的时候需要用到夹持器在机械臂基座坐标系下的位姿，于是我调用了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，这样我们的目的就达成了~