感谢@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,嘎嘎好用文章来源:https://www.toymoban.com/news/detail-514433.html
不要问我点云匹配相关的东西,我不懂emmm,但是能用
啧啧,今天真是高产,哈哈哈哈哈哈哈嗝文章来源地址https://www.toymoban.com/news/detail-514433.html
到了这里,关于ICP + Apriltag 实现机器人自动接驳/充电的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!