主要参考网址：https://blog.csdn.net/harrycomeon/article/details/90451692 https://industrial-training-master.readthedocs.io/en/kinetic/_source/setup/PC-Setup—ROS-Kinetic.html 之前已经实现了位姿的输出，下面编写订阅节点，并作为服务器服务端将发布出去 atnode_vision节点在apriltags2_node文件夹下， 1,首先编写订阅节点：见参考网址 2,然后将订阅到的主题作为服务器服务端发布出去 具体实现： 创建一个srv的文件夹，然后新建一个LocalizePart.srv的文件 #request string base_frame
#response geometry_msgs/Pose pose 可以看到服务器客户端请求的是坐标系的名称，服务器服务端回应的是此坐标系的位姿 下面是加入服务器服务端后的程序
<code class="has-numbering">class Localizer
{
public:
Localizer(ros::NodeHandle& nh)
{
ar_sub_ = nh.subscribe<apriltags2_ros::AprilTagDetectionArray>("tag_detections", 1,
&Localizer::number_callback, this);
server_ = nh.advertiseService("localize_part", &Localizer::localizePart, this); //将服务公布给ROS主服务器
}
bool localizePart(apriltags2_node::LocalizePart::Request& req,
apriltags2_node::LocalizePart::Response& res)
{
// Read last message
apriltags2_ros::AprilTagDetectionArrayConstPtr p = last_msg_;
//如果未订阅到主题，则不会运行服务器
if (!p)
return false;
res.pose = p->detections[0].pose.pose.pose;
return true;
}
void number_callback(const apriltags2_ros::AprilTagDetectionArray::ConstPtr& msg)
{
last_msg_ = msg;
ROS_INFO_STREAM(last_msg_->detections[0].pose.pose.pose);
//ROS_INFO("the number %d ",);
//ROS_INFO("%s\n", s.data.c_str());
}
ros::Subscriber ar_sub_;
apriltags2_ros::AprilTagDetectionArrayConstPtr last_msg_;
ros::ServiceServer server_;
};
</code>
server_ = nh.advertiseService(“localize_part”, &Localizer::localizePart, this); //将”localize_part”服务公布给ROS主服务器 res.pose = p->detections[0].pose.pose.pose;这一句是将订阅到的位姿赋给回应服务下的pose 3,然后编写服务器客户端apriltags2_client在apriltags2_node文件夹下，发出请求，请求”localize_part”的base_frame，然后atnode_vision回应相应的位姿 实现程序主要如下：
<code class="has-numbering">class ScanNPlan
{
public:
ScanNPlan(ros::NodeHandle& nh)
{
vision_client_ = nh.serviceClient<apriltags2_node::LocalizePart>("localize_part");
}
void start()
{
ROS_INFO("Attempting to localize part");
// Localize the part
apriltags2_node::LocalizePart srv;
if (!vision_client_.call(srv))
{
ROS_ERROR("Could not localize part");
return;
}
ROS_INFO_STREAM("part localized: " << srv.response);
return ;
}
private:
// Planning components
ros::ServiceClient vision_client_;
};
</code>
vision_client_ = nh.serviceClient<apriltags2_node::LocalizePart>(“localize_part”); 请求”localize_part”服务 if (!vision_client_.call(srv))判断是否有服务器服务，如果有执行 ROS_INFO_STREAM(“part localized: ” << srv.response);从而输出回应的位姿 以上程序是按照 Industrial Training的例程修改过来，大部分分析是个人观点，仅供参考，如有不对的地方，请多多指教。 最后我们将两个节点编写在一个launch文件下，只需要运行如下 roslaunch apriltags2_launch apriltags2_client.launch 加上之前的tag算法算出位姿部分，通过运行以下两个语句，便可以实现相应的功能。
<code class="has-numbering">roslaunch apriltags2_ros continuous_detection.launch roslaunch apriltags2_launch apriltags2_client.launch </code>
可以看出请求的基础坐标是“world”，这里实在launch文件下，声明
<code class="has-numbering"><param name="base_frame" value="world"/> </code>
更新————————————– 特别注意事项： 上面的介绍只是实现了初步的功能，但有个致命的错误，就是当tag被阻挡后会出现段错误，不过在github文件中已经进行了修改，下面是具体的原因分析和解决过程。 这里需要强调一下，因为之前有个话题可以订阅发布位姿的话题，即/tag_detections 和/tf，刚开始自己选择了/tag_detections 的话题，但是在摄像头采集不到相应的tag图像时，会导致后续last_msg_->detections[0]部分会出现空指针，从而导致段错误出现服务回调不成功，进而导致无法在检查不到tag后出现无法服务回调的情况。 刚开始是试图在atnode_vision节点下，试图改变节点订阅，从而当出现空指针时，阻止节点订阅，但发现话题发布的部分并没有bool型，从而难以采用常规的判断空指针的方法，即if（p!=NULL）;虽然在ros库中找到了可以终端服务回调的函数，但是只能订阅一次就结束，所以不符合我们的要求。 然后尝试改变ros位姿节点发布端的输出位姿，希望能够初始化位姿，从而不会出现空指针，但难以进行初始化，且同样面临没有bool型难以识别空指针的状况。 再然后尝试修改apriltags2_client客户端的服务回调判断部分，企图在判断出服务回调失败时，能够终止位姿空指针的输出，从而进行条件判断，但依然卡在了这里的空指针非常规类型的bool型，难以进行判断。 最后，想起来刚做出位姿接收时，就出现空指针时的段错误，未进行处理，当时想的就是返回来再处理，当时就没解决好。但也想到了刚开始apriltags位姿输出时，不单单一个主题发布了位姿（之前用的/tag_detections ,现在改用/tf ），而且还有另一个节点发布了位姿，而且这个节点当检测不到apriltags后会停在上一次发布的位姿上，这恰恰是自己想要的，果断修改了节点订阅端的主题，然后修改了一系列相关的类型声明，然后奇迹出现了，可以订阅到实时位姿，而且当apriltags被阻挡后可以避免段错误的同时，保证输出稳定在上一次位姿上，然后修改了服务器客户端，中间也遇到了话题不同后一系列的问题，但不得不感叹ros的强大，自己想要的函数下指定的形参类型恰恰都有函数编写好，所以只需要修改其中调用函数便可以实现客户端订阅。然后修改了workcell.xacro文件下摄像机不同的坐标名称，然后整个系统搭建完成，最后实现了实时的位姿订阅，也避免tag被阻挡后程序出现段错误的情况，并能够在一定运算时间后在rviz上进行机械手相应的位姿定位，从而实现了既定的功能。 说了这莫多，总结起来就是更换了之前参考网址上订阅的话题，修改成/tf话题，这个可以在github源码文件中看到。
