参考网址：https://industrial-training-master.readthedocs.io/en/kinetic/_source/session3/Intro-to-URDF.html 根据上述网址：我们修改了部分变量，建成workcell.xacro文件如下 可以看到这里是在ur5.urdf.xacro的基础上，添加了相机和桌子 根据参考网址：https://industrial-training-master.readthedocs.io/en/kinetic/_source/session3/Workcell-XACRO.html 创建了 XACRO文件 文件下载处：https://github.com/harrycomeon/apriltags2_ros-and-ur5/tree/master/apriltags2_launch/urdf 并创建了urdf.launch文件在rviz上加载模型，如下：
<launch>
<arg name="gui" default="true"/>
<param name="robot_description" command="$(find xacro)/xacro --inorder '$(find myworkcell_support)/urdf/workcell.xacro'" />
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
<param name="use_gui" value="$(arg gui)"/>
</node>
<node name="rviz" pkg="rviz" type="rviz" if="$(arg gui)"/>
</launch> 运行roslaunch apriltags2_launch urdf.launch 在rviz下进行如下设定 设置‘Fixed Frame’ 为 ‘world’ 添加 RobotModel and TF displays to the tree view on the left, to show the robot and some transforms. 尝试移动ur5机械臂发布的关节滑块看UR5的移动 效果图如下：
现在已经有了模型，包括之前我们已经能够得到了tag相对于相机的位姿，那麽怎么怎么将这个坐标和机械手坐标联系起来呢？看到模型中的tf发布的世界坐标系，其实它就可以看成坐标变换的一个桥梁。 再说明下，这里的相机是固定的，类似于眼在手外的情况，这里我们在模型中把相机坐标固定了，可以查看urdf文件，那麽实际操作时，怎么布置相机在空间中的这个坐标是个难点，感觉这一块在实际操作时需要进行手眼标定，这里暂时存在疑问。 既然我们直到相机相对于世界坐标系的位姿，而且知道机械手末端相对于世界坐标系的位姿，最后通过算法得到了tag在相机坐标系下的位姿，那麽我们是不是就可以通过坐标变换来得到tag相对于世界坐标系的位姿，也可以直到机械手末端到tag的相对位姿，这样我们不就可以利用moveit进行机械手的运动规划，从而让机械手末端到达tag的空间所在位置。 那麽我们需要怎么编写这之间的变换矩阵呢，是不是很头疼，很幸运，也感叹ros工具的强大，它完全可以帮我们实现我们想要的。 下面ros industry对TF的一段描述： 很难想象一个有用的，物理的“机器人”不会移动自己或看别的东西移动。 ROS中的一个有用的应用程序将不可避免地有一些组件需要监视零件，机器人链接或工具的位置。在ROS中，促进这一点的“生态系统”和库被称为TF。 TF是一个基本工具，允许查找任何连接帧之间的转换，甚至可以追溯到时间。它允许你提出类似的问题：“10秒前A和B之间的转换是什么。”这是有用的东西。 这里我们用tf通过几个已知的相对位姿得出tag相对于世界坐标系的坐标 具体实现代码在atnode_vision节点下，实例程序如下：
bool localizePart(apriltags2_node::LocalizePart::Request& req,
apriltags2_node::LocalizePart::Response& res)
{
// Read last message
tf2_msgs::TFMessageConstPtr p = last_msg_;
if (!p)
return false;
//将线上格式geometry_msgs::Pose转换为：
tf::Transform object
tf::Transform cam_to_target;
tf::transformMsgToTF(p->transforms[0].transform, cam_to_target);
//使用侦听器对象request.base_frame从ARMarker消息（应该是“camera_frame”）查找和参考帧之间的最新转换：lookupTransform（）可以获得两个坐标系的转换-旋转的平移
tf::StampedTransform req_to_cam; //定义存放坐标变换的变量
try{
ros::Time now = ros::Time::now();
listener_.waitForTransform(req.base_frame, p->transforms[0].header.frame_id,
now, ros::Duration(1.0));
listener_.lookupTransform(req.base_frame, p->transforms[0].header.frame_id,
now, req_to_cam);
}
catch (tf::TransformException &ex)
{
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
//continue;
}
//对象姿势转换为目标帧,定义存放转换信息（平移，转动）的变量
tf::Transform req_to_target;
req_to_target = req_to_cam * cam_to_target;
//在服务响应中返回转换后的姿势
tf::poseTFToMsg(req_to_target, res.pose);
return true;
}
这里说一下： 通过节点算法得出的位姿是tag在相机坐标系下的位姿，是不是换个说法就是相机到对象的坐标变换，即tf坐标系下cam_to_target; 所以我们这里写成 tf::Transform cam_to_target; tf::transformMsgToTF(p->transforms[0].transform, cam_to_target); 即将得出的位姿转换到tf坐标系下。 req_to_target = req_to_cam * cam_to_target; 通过这一语句得出tag相对于req（也即世界坐标系）位姿。
try{ listener.lookupTransform("/turtle2", "/turtle1",ros::Time(0), transform); }
Here, the real work is done, we query the listener for a specific transformation. Let’s take a look at the four arguments: We want the transform from frame /turtle1 to frame /turtle2. The time at which we want to transform. Providing ros::Time(0) will just get us the latest available transform. The object in which we store the resulting transform. If you get an error “Lookup would require extrapolation into the past” while running, you can try this alternative code to call the listener:这里自己也得到这种错误，所以采用这种形式。 try catch这块语句讲解可以参照官网例程：http://wiki.ros.org/tf/Tutorials/Writing a tf listener (C%2B%2B)
try {
listener.waitForTransform(destination_frame, original_frame, ros::Time(0), ros::Duration(10.0) );
listener.lookupTransform(destination_frame, original_frame, ros::Time(0), transform);
}
catch (tf::TransformException ex) {
ROS_ERROR("%s",ex.what());
}结合到我们的语句： req.base_frame为world坐标系 p->transforms[0].header.frame_id为camera坐标系 通过lookupTransform（）可以获得两个坐标系的转换-旋转的平移 最后运行下面语句：
roslaunch apriltags2_ros continuous_detection.launch
roslaunch apriltags2_launch urdf.launch
roslaunch apriltags2_launch apriltags2_client.launch 得到转换前后的位姿对比图：
转换到world下位姿图 从下面图示，可以看出目标位置相对于world坐标系的xyz是正确的相对位置
下面对上述位姿变换进行了理论和实际对比：
以上纯属个人观点，如有错误，请批评指教！
