主要参考网址：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   可以看到服务器客户端请求的是坐标系的名称，服务器服务端回应的是此坐标系的位姿 下面是加入服务器服务端后的程序  

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_;
};

  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回应相应的位姿   实现程序主要如下：  

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_;
};

  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算法算出位姿部分，通过运行以下两个语句，便可以实现相应的功能。  

roslaunch apriltags2_ros continuous_detection.launch 
roslaunch apriltags2_launch apriltags2_client.launch

  可以看出请求的基础坐标是“world”，这里实在launch文件下，声明  

<param name="base_frame" value="world"/>

  更新--------------------------------------   特别注意事项：   上面的介绍只是实现了初步的功能，但有个致命的错误，就是当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源码文件中看到。