IntelRealsenseZR300-TF坐标转换
接着上述的工作，在将近一个月的摸索实践中，终于完成了抓取的工作。其中包含tf坐标系的转换以及最后位置的调试工作。
这一篇先讲述如果进行根据camera_link以及base_link之间的转换关系完成一个点、一组点的进阶坐标转换。接着之前的手眼标定，连接起相机以及机械手臂的tf_tree.
#rosrun rqt_tf_tree rqt_tf_tree
这里要注意在camera_link中包含的子树camera_rgb_optical_frame以及camera_rgb_frame两个坐标系是不同的，从rviz里面来看其position大致重合，但是orentation是不一样的，而且相机检测到的可乐罐的位置坐标所属是在camera_rgb_optical_frame下的，所以在转换过程中（TF点的transform）一定要注意函数里面的参数是什么意思，坐标系代表的是哪一个坐标系，不然不清不楚在后面的位置标定过程中会出现很多错误。
开始tf转换过程的总结。首先当然是参考的我们官网教程，不可否认，虽然在他人的****博客可以学到或者拿到想看的内容，但是相比之下还是官网内容比较详细，如果某句程序、函数不懂可以很快的理解并修改。下面是核心的subscriber以及publisher（c++版本以及python版本）
http://wiki.ros.org/ROS/Tutorials/WritingPublisherSubscriber%28c%2B%2B%29
http://wiki.ros.org/ROS/Tutorials/WritingPublisherSubscriber%28python%29
以及依然可以替代参考的几篇****博客
https://blog.****.net/Will_Ye/article/details/79485964
https://blog.****.net/start_from_scratch/article/details/50762293/
https://blog.****.net/Will_Ye/article/details/79996311
由开发流程来看，首先要查看所要得到的物体位置信息（如何挖数据），接着要转换一个点从camera_link到base_link下，接着便是将物体的实时位置从camera_rgb_optical_frame到base_link坐标系下面。
这里插一个在pyhon里面查看所用到的函数：
ipython()
help(tf.TransformBroadcaster.sendTransform)
可以查看该函数的参数及相关信息。-----------------------------------------------------1---------------------------------------------------------
1.查看物体位置信息：
之前已经提到RecognizedObjectArray这个数组存放的是物体的位置信息，包括position orentation 以及header等等。这个数组是linemod算法在识别到物体位置以后将位置存储到这里（并且以话题的形式publish消息，所以我们能在rviz里面看到物体的标定位置）
下面是之前定义存放识别到物体信息存放的数组位置listener.py（写的一个简单的判断打印物体信息的位置）
#!/usr/bin/env python
#coding=utf-8
import rospy
# from std_msgs.msg import String
from object_recognition_msgs.msg import RecognizedObjectArray
def callback(data):
if len(data.objects)>0:
print "coke detected~~~~~~~~~~~~~~~~~~~~"
rospy.loginfo(rospy.get_caller_id() + 'heard %s', data.objects[0].pose.pose.pose)
# rospy.loginfo(rospy.get_caller_id() + 'heard %s', data.objects[0].pose)
else:
print "nothing detected................."
def listener():
# In ROS, nodes are uniquely named. If two nodes with the same
# name are launched, the previous one is kicked off. The
# anonymous=True flag means that rospy will choose a unique
# name for our 'listener' node so that multiple listeners can
# run simultaneously.
rospy.init_node('listener', anonymous=True)
rospy.Subscriber('recognized_object_array', RecognizedObjectArray, callback)
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
if __name__ == '__main__':
listener()这个程序只是打印物体的位置信息并且加了一个简单的判断，让我们来看为什么要这样写（怎么来挖数据）。挖数据核心主要是rospy.Subscriber('recognized_object_array', RecognizedObjectArray, callback)用来订阅RecognizedObjectArray话题并且一旦接收到消息就调用回调函数callback。而另一句打印 rospy.loginfo(rospy.get_caller_id() + 'heard %s', data.objects[0].pose.pose.pose) ，前面的id没什么用，后面的回调函数先判断是不是空的内容（是否识别到物体），data指代了RecognizedObjectArray的所有信息，也就是一个形参。用下面的命令来查看数据：
#rosmsg show RecognizedObjectArray用来查看发布的消息的类型以及包含的内容
这样来看data=RecognizedObjectArray,然后data.object[]=RecognizedObjectArray[],也就是指向消息的第二个变量object[]数组，如果用object[0]代表取当前检测到的第一个物体位置信息的数组信息。继续挖，data.object[]=RecognizedObjectArray[]包含了几个对象，比如header、type、confidence、pose(未截到图)等等以及他们对应的数据类型。运行listener来继续看
#rosrun ur10_rg2_moveit_config listener.py打印识别到的物体位置上面是只挖到data.object[].pose。这样来看可以看到，data.object[].pose.pose.pose即指到只有posetion和orientation数据处。这样我们就可以挖到指定的数据，并且对挖数据有一个直观的学习了解。
-----------------------------------------------------2---------------------------------------------------------
2.将一个点从camera_link到base_link:
首先说明之前标定两坐标系以后以及发布了一个关系：在创建的tf_broadcaster广播两坐标关系并且放置在tf::transform
函数中。
#rosrun robot_setup_tf tf_broadcaster#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
int main(int argc, char** argv){
ros::init(argc, argv, "robot_tf_publisher");
ros::NodeHandle n;
ros::Rate r(100);
tf::TransformBroadcaster broadcaster;
while(n.ok()){
broadcaster.sendTransform(
tf::StampedTransform(
tf::Transform(tf::Quaternion(0.0144077350003, -0.0295559697343, -0.0183787903043, 0.999290289101), tf::Vector3(0.244332058703, -0.0155924006203, -0.0434545849503)),
ros::Time::now(),"base_link", "camera_link"));
r.sleep();
}
}
编写了一个tf_listener_OnePosetransform.cpp 用于单独发布一个点，这个以后在位置标定也有作用：
#rosrun robot_setup_tf tf_listener_OnePosetransform#include <ros/ros.h>
// #include <geometry_msgs/PointStamped.h>
#include "geometry_msgs/PoseStamped.h"
#include "geometry_msgs/Pose.h"
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>
#include <visualization_msgs/MarkerArray.h>
geometry_msgs::PoseStamped real_pose;
geometry_msgs::PoseStamped transed_pose;
void transformPose(const tf::TransformListener& listener){
//we'll create a point in the base_laser frame that we'd like to transform to the base_link frame
// geometry_msgs::PointStamped laser_point;
// geometry_msgs::PoseStamped msg;
// laser_point.header.frame_id = "camera_link";
// real_pose.header.frame_id = "camera_link";
real_pose.header.frame_id = "camera_rgb_optical_frame";
// real_pose.header.frame_id = "camera_rgb_frame";
//we'll just use the most recent transform available for our simple example
// laser_point.header.stamp = ros::Time();
real_pose.header.stamp = ros::Time();
real_pose.pose.position.x = -0.0521919690073;
real_pose.pose.position.y = 0.0947469994426;
real_pose.pose.position.z = 0.928996682167;
real_pose.pose.orientation.w = 0.487851679325;
real_pose.pose.orientation.x = 0.518789410591;
real_pose.pose.orientation.y = -0.516209602356;
real_pose.pose.orientation.z = 0.47580036521;
//just an arbitrary point in space
try{
// geometry_msgs::PointStamped base_point;
// listener.transformPoint("base_link", laser_point, base_point);
listener.transformPose("base_link", real_pose, transed_pose);
ROS_INFO("camera_rgb_frame: (%.2f, %.2f. %.2f, %.2f, %.2f, %.2f, %.2f) -----> base_link: (%.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f) at time %.2f",
real_pose.pose.position.x, real_pose.pose.position.y, real_pose.pose.position.z, real_pose.pose.orientation.x, real_pose.pose.orientation.y, real_pose.pose.orientation.z, real_pose.pose.orientation.w,
transed_pose.pose.position.x, transed_pose.pose.position.y, transed_pose.pose.position.z, transed_pose.pose.orientation.x, transed_pose.pose.orientation.y, transed_pose.pose.orientation.z, transed_pose.pose.orientation.w, transed_pose.header.stamp.toSec());
}
catch(tf::TransformException& ex){
ROS_ERROR("Received an exception trying to transform a point from \"camera_rgb_optical_frame\" to \"base_link\": %s", ex.what());
}
}
int main(int argc, char** argv){
ros::init(argc, argv, "tf_listener_Posetransform");
ros::NodeHandle n;
tf::TransformListener listener(ros::Duration(10));
//we'll transform a point once every second
ros::Timer timer = n.createTimer(ros::Duration(1.0), boost::bind(&transformPose, boost::ref(listener)));
ros::Publisher my_publisher_realPose = n.advertise<geometry_msgs::PoseStamped>("real_pose", 1000);
ros::Publisher my_publisher_transedPose = n.advertise<geometry_msgs::PoseStamped>("transed_pose", 1000);
ros::Rate loop_rate(10);
while (ros::ok())
{
my_publisher_transedPose.publish(transed_pose);
my_publisher_realPose.publish(real_pose);
ros::spinOnce();
loop_rate.sleep();
}
ros::spin();
return 0;
}这里的点是之后自己检测到物体的点自己写进去的，实际中可以修改来帮助自己查看点的位置变化。这里的real_pose以及transed_pose都是geometry_msgs::PoseStamped消息类型的，以后查看转换信息也要在这里查看。tf_listener_OnePosetransform文件中只定义了一个节点，但是它同时完成了订阅（subscriber）以及发布（publish）的工作。listener.transformPose("base_link", real_pose, transed_pose);监听之前发布的两坐标系关系，利用之前的transformPose函数即可将camera_link下的一个坐标位置（自定义的real_pose坐标）转换到base_link下面。另外一定要注意坐标所属的坐标系real_pose.header.frame_id = "camera_rgb_optical_frame"。%.2f表示f为浮点型数据，2表示只展示到小数点后两位，所以可以改为4位。1000代表缓存消息的最大内存。
接着就是要将这两个点发布出去
ros::Publisher my_publisher_realPose = n.advertise<geometry_msgs::PoseStamped>("real_pose", 1000);my_publisher_realPose是自己命名的消息名，消息类型为geometry_msgs::PoseStamped，话题名为real_pose(这样就可以在rviz里面add poseStamped类型，选择相应话题即可在rviz里面显示观察。）。
上面那一行程序只是说要发布这个消息，但是具体的发布操作却是下一步：
my_publisher_realPose.publish(real_pose);这样我们就完成了一个自定义点的从camera_rgb_optical_frame到base_link的转换。
用rosmsg即可查看： 要挖的话也是一样的道理。
rosmsg show geometry_msgs/PoseStamped 打印poseStamped的消息-----------------------------------------------------3---------------------------------------------------------
3.将物体位置从camera_rgb_optical_frame到base_link转换
#rosrun robot_setup_tf tf_listener创建tf_listener.cpp（包括所需的依赖项 创建过程在官网的编写subscriber以及publisher都有提到，读者可以自行到文章最前面查看）
(原谅我 在tf这块参考代码比较多的是cpp文件，所以在修改完程序以后每次都得cmake重新编译，而且订阅和发布都是cpp来完成的，不过在最后的控制机械臂运行的python程序中又变成在python里面订阅这个消息，所以读者要注意博主在这块的限制（更熟悉cpp一点）)。
#include <ros/ros.h>
#include "geometry_msgs/PoseStamped.h"
#include "geometry_msgs/Pose.h"
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>
// #include "std_msgs/String.h"
#include <object_recognition_msgs/RecognizedObjectArray.h> //using ORK
#include <visualization_msgs/MarkerArray.h>
//实时转换采集到的可乐罐的位置点转换
std::string Object_id = "";
double Object_assurance = 0;
geometry_msgs::PoseStamped Object_pose;
geometry_msgs::PoseStamped transed_pose;
bool firstCB = false;
void myCallback(const object_recognition_msgs::RecognizedObjectArray objects_msg) //回调函数
{
double confident = 0;
int id = -1;
if (firstCB == false && (int)objects_msg.objects.size() == 1) {
Object_id.assign(objects_msg.objects[0].type.key.c_str());
firstCB = true;
}
for (int i = 0; i < objects_msg.objects.size(); ++i) {
if (Object_id.compare(objects_msg.objects[i].type.key.c_str()) == 0) {
if (objects_msg.objects[i].confidence > confident) {
confident = objects_msg.objects[i].confidence;
id = i;
}
}
}
if (id >= 0) {
Object_pose.pose = objects_msg.objects[id].pose.pose.pose;
Object_assurance = objects_msg.objects[id].confidence;
} else {
confident = 0;
}
}
void transformPose(const tf::TransformListener& listener){
//we'll create a point in the base_laser frame that we'd like to transform to the base_link frame
// geometry_msgs::PointStamped laser_point;
// geometry_msgs::PoseStamped transed_pose;
// laser_point.header.frame_id = "camera_link";
// msg.header.frame_id = "camera_link";
// Object_pose.header.frame_id = "camera_rgb_frame";
Object_pose.header.frame_id = "camera_rgb_optical_frame";
//we'll just use the most recent transform available for our simple example
// laser_point.header.stamp = ros::Time();
ROS_INFO("Waiting for the object");
if (Object_assurance > 0.8) {
ROS_INFO("Best Similarity = %f ", Object_assurance);
ROS_INFO("pose x is: %f", Object_pose.pose.position.x);
ROS_INFO("pose y is: %f", Object_pose.pose.position.y);
ROS_INFO("pose z is: %f", Object_pose.pose.position.z);
bool tferr = true;
while (tferr) {
tferr = false;
try{
listener.transformPose("base_link", Object_pose, transed_pose);
}
catch (tf::TransformException &exception) {
ROS_ERROR("Received an exception trying to transform a point from \"camera_rgb_frame\" to \"base_link\": %s", exception.what());
tferr = true;
ros::Duration(0.1).sleep();
ros::spinOnce();
}
} //while
ROS_INFO("camera_rgb_optical_frame: (%.2f, %.2f. %.2f, %.2f, %.2f, %.2f, %.2f) -----> base_link: (%.2f, %.2f, %.2f, %.2f, %.2f, %.2f, %.2f) at time %.2f",
Object_pose.pose.position.x, Object_pose.pose.position.y, Object_pose.pose.position.z, Object_pose.pose.orientation.x, Object_pose.pose.orientation.y, Object_pose.pose.orientation.z, Object_pose.pose.orientation.w,
transed_pose.pose.position.x, transed_pose.pose.position.y, transed_pose.pose.position.z, transed_pose.pose.orientation.x, transed_pose.pose.orientation.y, transed_pose.pose.orientation.z, transed_pose.pose.orientation.w, transed_pose.header.stamp.toSec());
} //if
} //void
int main(int argc, char** argv){
ros::init(argc, argv, "robot_tf_listener");
ros::NodeHandle n;
ros::Subscriber my_subscriber_object= n.subscribe("recognized_object_array",1000,myCallback);
tf::TransformListener listener(ros::Duration(10));
//we'll transform a point once every second
ros::Timer timer = n.createTimer(ros::Duration(1.0), boost::bind(&transformPose, boost::ref(listener)));
ros::Publisher my_publisher_transPose = n.advertise<geometry_msgs::PoseStamped>("transformPose", 1000);
ros::Publisher my_publisher_ObjectPose = n.advertise<geometry_msgs::PoseStamped>("ObjectPose", 1000);
ros::Rate loop_rate(10);
while (ros::ok())
{
// std_msgs::String msg;
// msg.data = ss.str();
// ROS_INFO("=====transformedPose======: (%.2f, %.2f, %.2f, %.2f, %.2f, %.2f, %.2f)", transed_pose.pose.position.x, transed_pose.pose.position.y, transed_pose.pose.position.z, transed_pose.pose.orientation.x, transed_pose.pose.orientation.y, transed_pose.pose.orientation.z, transed_pose.pose.orientation.w);
my_publisher_transPose.publish(transed_pose);
my_publisher_ObjectPose.publish(Object_pose);
ros::spinOnce();
loop_rate.sleep();
}
ros::spin();
return 0;
}这个就是最终的tf变换过程所需要的内容程序。这个程序中完成了三件事：
（1）物体信息的订阅、简单判断以及发布
（2）将物体位置从camera_rgb_optical_frame到base_link转换
(3) 将转换后的位置信息发布出去。
这段程序和之前一个点的转换过程类似所以不再赘述，值得注意的是：
geometry_msgs::PoseStamped Object_pose;
geometry_msgs::PoseStamped transed_pose;
物体的位置以及转换后的位置要声明为全局变量。
其他：
终于在十月份的尾巴完成了这个小项目的工作任务，当然今天只是对tf这一块儿做个简单总结，明天继续总结完剩下的调整标定工作，记录怎样分析错误并一步步解决最终使机械臂抓取到物体。像下面就是之前一直卡的一个情况，下一篇博客也是最为重要也是最完整的找自己出现的错误，修改程序并抓到物体，也可以发自己的测试视频。目前来看这个算法在抓取任务方面还是蛮靠谱的，虽然稳定性还是不是很好，尤其在边远的位置处检测效果不好（很容易卡主没有检测结果），当然只要有结果就能抓起来证明自己在其他的程序并没有问题，算法的问题也是今后论文创新的关键。
明天的最终效果图：
记录自己的一月，完成明天的最后内容的总结就可以继续下一步的工作了，继续努力，↖(^ω^)↗加油！