Apriltags+Ros+Ur5项目–位姿订阅及其服务器请求和回应

235
2
2021年1月19日 09时08分

主要参考网址: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源码文件中看到。

 

发表评论

后才能评论

评论列表(2条)

  • ka95v_9486 2021年2月23日 上午1:41

    感谢分享!我同样也遇到了段错误的问题,我的解决方法是在获取detections[0].pose.pose.pose前加入一个if(msg->detections.size( )>0)的判断,这样就不会报错了。