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

这篇具有很好参考价值的文章主要介绍了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,但是能用
啧啧,今天真是高产,哈哈哈哈哈哈哈嗝文章来源地址https://www.toymoban.com/news/detail-514433.html

到了这里,关于ICP + Apriltag 实现机器人自动接驳/充电的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处: 如若内容造成侵权/违法违规/事实不符,请点击违法举报进行投诉反馈,一经查实,立即删除!

领支付宝红包 赞助服务器费用

相关文章

  • 深度揭秘:返利机器人的自动赚佣金原理与实现方法

    大家好,我是免费搭建查券返利机器人赚佣金就用微赚淘客系统3.0的小编,也是冬天不穿秋裤,天冷也要风度的程序猿!今天,我将为大家揭示返利机器人的核心秘密——自动赚佣金的原理及实现方法。 在电商时代,返利机器人已经成为许多消费者追求实惠的得力助手。它们

    2024年02月02日
    浏览(42)
  • 如何使用机器人和物联网实现仓库自动化

    自动化和机器人化不再仅限于制造过程; 现代仓库也正在采用大量新技术。 物联网 (IoT) 的出现使得极其精确的室内地理定位成为可能; 仓库工作人员正在使用智能仓储车; 先进的传感器确保存储货物的质量。 了解仓库自动化以及它如何保护并加快您的物流流程。 什么是物联网

    2024年02月20日
    浏览(47)
  • 软件机器人助力基层网点实现存款数据自动化处理

    银行基层网点需要及时了解存款变动情况,以便能够做出相应的安排和决策。过去,各级机构经办人员需要多次登录员工渠道系统,在不同的时间点查询并下载本级及下属机构的实时科目余额表,然后通过人工加工,才能得到存款新增数据。 应用博为小帮软件机器人,可以将

    2024年02月12日
    浏览(44)
  • BOSS直聘自动投简历聊天机器人的实现过程

            这两年疫情,公司业务越来越差,必须得准备后路了,每天睡前都会在直聘上打一遍招呼,一直到打哈欠有睡意为止...,这样持续了一周,发现很难坚持,身为一名资深蜘蛛侠,怎么能这样下去呢?于是便萌生了对BOSS下手的念头。         boss的web端功能已经挺完

    2023年04月22日
    浏览(31)
  • FANUC机器人实现本地自动运行的相关配置和参数设置

    具体步骤可参考如下内容 : 如下图所示,按下MENU键,选择下一页, 如下图所示,找到系统—配置,enter回车进入, 如下图所示,在系统—配置中,按下item键,输入43,找到第43项, 如下图所示,找到第43项:远程-本地设置后,按下F4选择,选择2本地,enter回车确认, 如下图

    2024年02月16日
    浏览(53)
  • Python实现企业微信群机器人自动化推送

    人工智能(Artificial Intelligence),英文缩写为AI。它是研究、开发用于模拟、延伸和扩展人的智能的理论、方法、技术及应用系统的一门新的技术科学。 ——《百度百科》 实际工作中,有类似这样的场景, 需要将某些通知信息定期发送到企业微信群,需要将公司某些指标的异

    2024年02月09日
    浏览(60)
  • 基于人工势场算法的机器人自动避障及matlab实现

    基于人工势场算法的机器人自动避障及matlab实现 人工势场算法是一种用于机器人自主导航的常见方法。该算法利用势场来模拟机器人周围环境中的阻碍物和目标,并通过计算其施加在机器人上的力来导航机器人。本文将介绍如何使用人工势场算法实现机器人自动避障,并给出

    2024年02月06日
    浏览(51)
  • 【ChatGLM vs ChatGPT】怎样实现机器人自动写代码?不少于3000字。

       图:a robot is writing code, by Stable Diffusion 禅与计算机程序设计艺术: 总体来看,ChatGLM(6B)和 ChatGPT(175B)在技术领域的问答情况表现都很出色,考虑到模型参数和成本,整体看在这方面的表现  ChatGLM 优于 ChatGPT 。 目录 怎样实现机器人自动写代码?不少于3000字。

    2024年02月01日
    浏览(36)
  • 深度解读:微信自动查券返利机器人的工作原理与实现思路

    大家好,我是免费搭建查券返利机器人赚佣金就用微赚淘客系统3.0的小编,也是冬天不穿秋裤,天冷也要风度的程序猿!在当今的数字化时代,网络购物已经成为人们日常生活的一部分。然而,面对众多的购物平台和复杂的优惠券、返利政策,消费者往往感到无从下手。为了

    2024年02月03日
    浏览(54)
  • 提高错误日志处理效率!使用Python和钉钉机器人实现自动告警聚合

    日志是非常重要的信息资源。它们记录了应用程序的运行状态、错误和异常情况,帮助我们了解系统的健康状况以及发现潜在的问题。为了高效地管理和分析日志数据,许多组织采用了Elasticsearch、Logstash和Kibana(ELK)堆栈作为日志收集和分析的解决方案。 开发一个实时监控和

    2024年02月11日
    浏览(43)

觉得文章有用就打赏一下文章作者

支付宝扫一扫打赏

博客赞助

微信扫一扫打赏

请作者喝杯咖啡吧~博客赞助

支付宝扫一扫领取红包,优惠每天领

二维码1

领取红包

二维码2

领红包