ICP + Apriltag 实现机器人自动接驳/充电

感谢@Cuma Özavcı 提供的ros可用的开源ICP算法

如果感兴趣，可以去Pattern Matching on 2D Lidar Data看一下作者本人的描述

先描述一下大概的使用场景，我做这个是因为公司实际要用到这个模块，所以在主管的指导下看了这个包，然后改吧改吧，跟现有的一些东西做了结合，最终完成了这个过程。

大概过程就是，cartographer将机器人导航到接驳点，然后开始对接流程，包括寻找apriltg、获取底盘到apriltag的位姿变换，给出一个pattern_matcher的先验位姿，通过pattern_matcher找到最终机器人停泊的点。大概是这么个流程。

pattern_macher的使用

首先要有ros环境，这个应该没什么说的。

在编译完以后，直接把机器人放到最终需要到达的位置，或者说需要记录点云的位置，

启动recorder.launch

注：如果是多个激光雷达，需要先把雷达数据合成为一个，可以考虑使用 ros官方的 ira_laser_tools工具进行合并

在点云文件保存好以后，可以通过cloudcompare进行点云的选取，去掉一些没什么意义的杂点，噪点

只保留需要的关键点就可以了

启动matcher.launch

直接启动matcher.launch，在未改动的情况下，他会去找图形的质心，作为初始位姿，此时大概率时不准的，

除非你就把机器人放在你需要的位置，那么就需要一个额外的节点来提供这个初始位姿，

在我们公司的实践上，用的是apriltag，至于为啥不直接用apriltag一步到位。。。apriltag太漂啦！在一米左右根本无法得到一个稳定的坐标。也可能是因为，算法比较笨吧，要先把机器人调整到一个相对比较好的位姿，然后再倒车入库。

#include "ros/ros.h"
#include "ros/package.h"

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/search/kdtree.h>
#include <pcl/filters/filter.h>
#include <pcl/registration/icp.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/search/impl/search.hpp>
#include <apriltag_ros/AprilTagDetectionArray.h>
#include <pattern_matcher/pattern_matcher.h>
#include "sensor_msgs/LaserScan.h"
#include "sensor_msgs/PointCloud2.h"

#include "tf/transform_listener.h"
#include "tf/transform_broadcaster.h"
#include "laser_geometry/laser_geometry.h"

bool scan_msg_came;
sensor_msgs::LaserScan scan_msg;
void scan_cb(const sensor_msgs::LaserScan::ConstPtr &msg)
{
    scan_msg = *msg;
    scan_msg_came = true;
}

// handle apriltag detection
bool tag_detection;
apriltag_ros::AprilTagDetectionArray detection_msg;
int32_t tag_id;
double tag_size;

void detection_cb(const apriltag_ros::AprilTagDetectionArray::ConstPtr& msg)
{
    ROS_INFO("detection_cb recalled!");
    if(msg->detections.empty())
    {
        ROS_INFO("waiting for detection msgs!");

    }
    else
    {
        if (tag_id == msg->detections[0].id[0] && tag_size == msg->detections[0].size[0])
        {
            detection_msg = *msg;
            tag_detection = true;
        }
        else
        {
            ROS_WARN("tag_id: %d and tag_size %f mismatched. Expected: tag_id: %d tag_size: %f", 
                    msg->detections[0].id[0], msg->detections[0].size[0], tag_id, tag_size);
            tag_detection = false;
        }
    }
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "pattern_matcher");
    ros::NodeHandle n;

    // Parameters
    std::string scan_topic;
    std::string pattern_topic;
    std::string clustered_points_topic;
    std::string pattern_filepath;


    // Get Parameters from ROS Parameter Server
    ros::NodeHandle n_private("~");
    if(!n_private.getParam("scan_topic", scan_topic)) scan_topic = "scan";
    if(!n_private.getParam("pattern_topic", pattern_topic)) pattern_topic = "pattern";
    if(!n_private.getParam("clustered_points_topic", clustered_points_topic)) clustered_points_topic = "clustered_points";
    if(!n_private.getParam("pattern_filepath", pattern_filepath)) pattern_filepath = ros::package::getPath("pattern_matcher") + "/pcd/pattern.pcd";;

    // get apriltag related params
    if(!n.getParam("/amr_carry/tag_id", tag_id)) tag_id = 2;
    if(!n.getParam("/amr_carry/tag_size", tag_size)) tag_size = 0.08;
    // ROS Subscribers and Publishers
    ros::Subscriber apriltag_sub = n.subscribe("/tag_detections",1,detection_cb); 
    ros::Subscriber scan_sub = n.subscribe(scan_topic, 1, scan_cb);
    ros::Publisher pattern_pub = n.advertise<sensor_msgs::PointCloud2>(pattern_topic, 100);
    ros::Publisher clustered_pub = n.advertise<sensor_msgs::PointCloud2>(clustered_points_topic, 100);
    ros::Publisher icp_pub = n.advertise<pattern_matcher::pattern_matcher>("/icp_detection",1);
    // declare icp_msg
    pattern_matcher::pattern_matcher icp_msg;
    icp_msg.icp_detected = false;
    scan_msg_came = false;
    tag_detection = false;
    laser_geometry::LaserProjection projector;

    // TF Listener and Broadcaster
    tf::TransformListener tfListener;
    static tf::TransformBroadcaster tfBroadcaster;
    // TF Listener for initial pose
    tf2_ros::Buffer tfBuffer_initial;
    tf2_ros::TransformListener tfListener_initial(tfBuffer_initial);

    // Read pattern cloud from file
    pcl::PointCloud<pcl::PointXYZ>::Ptr pattern(new pcl::PointCloud<pcl::PointXYZ>);
    if (pcl::io::loadPCDFile<pcl::PointXYZ> (pattern_filepath, *pattern) == -1)
    {
        ROS_ERROR ("Could not read pattern file");
        return 0;
    }

    // PCL Spesific
    pcl::console::setVerbosityLevel(pcl::console::L_ALWAYS);

    pcl::VoxelGrid<pcl::PointXYZ> vg;
    vg.setLeafSize (0.01f, 0.01f, 0.01f);

    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);

    pcl::PointCloud<pcl::PointXYZ> cloud;
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr icp_result (new pcl::PointCloud<pcl::PointXYZ>);

    pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
    ec.setClusterTolerance(0.2);
    ec.setMinClusterSize(50);
    ec.setMaxClusterSize(500);

    pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
    icp.setInputSource(pattern);
    icp.setMaxCorrespondenceDistance(0.04);

    sensor_msgs::PointCloud2 pointCloud;
    sensor_msgs::PointCloud2 cloud_reconstructed_msg;

    // Convert pattern cloud from PCL Cloud to ROS PointCloud2
    sensor_msgs::PointCloud2 pattern_msg;
    pcl::toROSMsg(*pattern, pattern_msg);
    pattern_msg.header.frame_id = "result";

    // Colors
    int color_index = 0;
    int colors[] = {255, 0, 0,          // RED
                    0, 255, 0,          // GREEN
                    0, 0, 255,          // BLUE
                    255, 255, 0,        // ???
                    0, 255, 255,        // ???
                    255, 255, 255       // WHITE
                    };
    int color_count = sizeof(colors) / sizeof(int) / 3;

    // For transforming between PCL and TF
    Eigen::Affine3f affine_transform;
    Eigen::Matrix4f initial_aligment;
    float tx, ty, tz, roll, pitch, yaw;

    // Best ICP fitness score
    double best_fitness;
    ros::Rate rate(6);
    while(ros::ok())
    {
        ros::spinOnce();
        rate.sleep();
        // Check if new scan msg arrived
        if(!scan_msg_came) continue;
        if(!tag_detection) continue;
        scan_msg_came = false;
        // Convert sensor_msgs::LaserScan to PCL Cloud
        projector.transformLaserScanToPointCloud(scan_msg.header.frame_id, scan_msg, pointCloud, tfListener);
        pcl::fromROSMsg (pointCloud, cloud);

        // Apply voxel filter
        vg.setInputCloud (cloud.makeShared());
        vg.filter (*cloud_filtered);

        // Set filtered cloud as input for KD Tree
        tree->setInputCloud (cloud_filtered);

        // Extract clusters
        std::vector<pcl::PointIndices> cluster_indices;
        ec.setSearchMethod (tree);
        ec.setInputCloud (cloud_filtered);
        ec.extract (cluster_indices);
        
        pcl::PointCloud<pcl::PointXYZRGB> cloud_reconstructed;
        std::vector<std::pair<pcl::PointCloud<pcl::PointXYZ>::Ptr, pcl::PointXYZ>> clusters;

        best_fitness = 10;

        /**
         * For Every Cluster:
         * 1. Give different color
         * 2. Run ICP on it
         * 3. Check if ICP result is the best
         */
        if(tag_detection == true)
        {
            for(auto cluster_indice : cluster_indices)
            {
                pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
                pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_cluster_rgb (new pcl::PointCloud<pcl::PointXYZRGB>);

                // Create two PCL clouds from cluster_indice
                // Give different color for every cluster
                for (const auto& indice : cluster_indice.indices)
                {
                    pcl::PointXYZ point;
                    pcl::PointXYZRGB point_rgb;

                    point.x = point_rgb.x = (*cloud_filtered)[indice].x;
                    point.y = point_rgb.y = (*cloud_filtered)[indice].y;
                    point.z = point_rgb.z = (*cloud_filtered)[indice].z;

                    point_rgb.r = *(colors + (color_index*3));
                    point_rgb.g = *(colors + (color_index*3) + 1);
                    point_rgb.b = *(colors + (color_index*3) + 2);

                    cloud_cluster->push_back(point);
                    cloud_cluster_rgb->push_back(point_rgb);
                }

                color_index += 1;
                if(color_index >= color_count) color_index = 0;

                cloud_cluster_rgb->width = cloud_cluster_rgb->size();
                cloud_cluster_rgb->height = 1;
                cloud_cluster_rgb->is_dense = true;
                
                cloud_reconstructed += *cloud_cluster_rgb;

                pcl::toROSMsg(cloud_reconstructed, cloud_reconstructed_msg);
                cloud_reconstructed_msg.header.stamp = ros::Time::now();
                cloud_reconstructed_msg.header.frame_id = scan_msg.header.frame_id;
                clustered_pub.publish(cloud_reconstructed_msg);
                
                // calculate tf between base_link to AprilTag_Rotated to provide a initial pose
                double initial_x = 0;
                double initial_y = 0;
                double initial_z = 0;
                try
                {
                    // 获取base_link到AprilTag_Rotated的坐标变换
                    geometry_msgs::TransformStamped transformStamped;
                    // 这里更好的办法是直接读取/tag_detections中的pose，可以避免在tf树没建立好时的一些报错
                    transformStamped = tfBuffer_initial.lookupTransform("base_link", "AprilTag_Rotated", ros::Time(0));

                    // 提取x、y、z的值
                    // 这里的0.455是一个固定误差，写成参数传进来应该更好，但是我懒。
                    // 这个0.455不是必须的，只是我们这个场景位置是读到的距离加上0.455而已
                    initial_x = transformStamped.transform.translation.x + 0.455; 
                    // initial_x = transformStamped.transform.translation.x;
                    initial_y = transformStamped.transform.translation.y;
                    initial_z = transformStamped.transform.translation.z;

                    // 输出坐标值
                    ROS_INFO("base_link to AprilTag_Rotated: x = %f, y = %f, z = %f", initial_x, initial_y, initial_z);
                    icp_msg.icp_detected = true;
                }
                catch (tf2::TransformException& ex)
                {
                    ROS_WARN("Failed to lookup transform: %s", ex.what());
                    continue;
                }

                // Calculate initial alignment 在这里添加初始位姿，x,y,z,r,p,y
                pcl::getTransformation(initial_x, initial_y, initial_z, 0, 0, 0, affine_transform);
                initial_aligment = affine_transform.matrix();

                // Run ICP
                icp.setInputTarget(cloud_cluster);
                icp.align (*icp_result, initial_aligment);

                // Check if ICP converged
                if(!icp.hasConverged()) continue;            

                // Check if this convergence is better
                if(best_fitness < icp.getFitnessScore()) continue;

                // Store best fitness and transformation values
                best_fitness = icp.getFitnessScore();
                pcl::getTranslationAndEulerAngles(Eigen::Affine3f(icp.getFinalTransformation()), tx, ty, tz, roll, pitch, yaw);
                tag_detection = false;
            }
            // Check if a good match were found
            if(best_fitness >= 1.0) {continue;
            ROS_INFO("best_finess=%f",best_fitness);}
            // Pattern Match Transformation
            tf::Transform transformation;
            ROS_INFO("geting info transform!");
            transformation.setOrigin(tf::Vector3(tx, ty, 0.0));
            transformation.setRotation(tf::createQuaternionFromYaw(yaw));

            // Publish TF and Pattern PointCloud2
            tfBroadcaster.sendTransform(tf::StampedTransform(transformation, ros::Time::now(), scan_msg.header.frame_id, "result"));
            pattern_msg.header.stamp = ros::Time::now();
            pattern_pub.publish(pattern_msg);

            // Publish TF to /icp_detection topic
            if(icp_msg.icp_detected == true)
            {
                geometry_msgs::TransformStamped transform_msg;
                tf::transformTFToMsg(transformation, transform_msg.transform);
                transform_msg.header.stamp = ros::Time::now();
                transform_msg.header.frame_id = scan_msg.header.frame_id;
                transform_msg.child_frame_id = "result";
                icp_msg.transform = transform_msg;
                icp_pub.publish(icp_msg);
            }
            else
            {
                ROS_INFO("waiting for detection....");
            }
        }
        

    }
}

大概就这样，如果有其他的方法也可以，比如easy_aruco或者landmark等等，都可以提供一个先验位姿，塞进去就能用，搭配上前面说过的move_basic，嘎嘎好用

不要问我点云匹配相关的东西，我不懂emmm，但是能用
啧啧，今天真是高产，哈哈哈哈哈哈哈嗝