目标:让小车用激光雷达和深度相机完成边建图边导航的任务,用以代替tare运行时依赖的environment_simulator仿真环境
一.小车自身的配置和功能
小车能够边建图边导航(rp激光雷达),另外配有深度相机(这里建图还未使用到,不过后面融合会用到)
二.使用方法和融合代码以及实际效果
变数1.可能要注意一下膨胀后进行一个z轴上的滤波,这个可能需要按照实际的情况调节一下滤波范围(保留)
2.点云的预先旋转平移处理需要按照不同规格的相机作出变换,对着话题/astra_downsample_points查看是否在"map"坐标系下正常显示
ros节点
1.roslaunch turn_on_wheeltec_robot my_mapping.launch navigation:=true 打开建图和导航(并且订阅的雷达消息是laser_scan_pcl_fusion)
2.roslaunch astra_camera my_astra.launch
3.rosrun turn_on_wheeltec_robot astra_points_ds_2scan
在上位机
1.rosrun tare_planner wheeltec_2_tare
2.roslaunch tare_planner explore_garage.launch
pcl和laser融合代码
astra_points_ds_2scan.cpp
代码如下
#include <ros/ros.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/passthrough.h>
#include <tf/transform_listener.h>
#include <pcl/common/transforms.h>
#include <sensor_msgs/LaserScan.h>
#include <math.h>
#define PI (3.141592653589f)
///!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!也许后面还可以搞个点云积累效果?积累成一个图? 在rviz上感觉点云数据挺滞后的,这个不知道有没有影响
//本节点用于融合scan和点云信息=>最终输出laserscan ,用于2D栅格建图和tare导航
class cloudHandler
{
public:
pcl::VoxelGrid<pcl::PointXYZ> voxelSampler; //下采样专用的类
pcl::PassThrough<pcl::PointXYZ> pass;// 创建滤波器对象
tf::TransformListener listener;
tf::StampedTransform transform;
bool pcl_initial=false; //用于判断是否点云数据已经出现
double roll, pitch, yaw;
pcl::PointCloud<pcl::PointXYZ> cloud;
pcl::PointCloud<pcl::PointXYZ> cloud_downsampled;
pcl::PointCloud<pcl::PointXYZ> cloud_for_fusion;
sensor_msgs::LaserScan laserscan_output;
pcl::PointCloud<pcl::PointXYZ> copy_cloud;
pcl::PointCloud<pcl::PointXYZ> copy_cloud_1;
pcl::PointCloud<pcl::PointXYZ> copy_cloud_2;
pcl::PointCloud<pcl::PointXYZ> copy_cloud_3;
cloudHandler()
{
pcl_sub = nh.subscribe("/camera/depth/points", 1, &cloudHandler::cloudCB, this);
pcl_pub = nh.advertise<sensor_msgs::PointCloud2>("/astra_downsample_points", 1);
pcl_test_pub = nh.advertise<sensor_msgs::PointCloud2>("/astra_downsample_points_laser", 1);
scan_sub = nh.subscribe("/scan", 2, &cloudHandler::PCL_SCAN_fusion, this);
scan_pub = nh.advertise<sensor_msgs::LaserScan>("/laser_scan_pcl_fusion", 1);
}
void cloudCB(const sensor_msgs::PointCloud2 &input)
//@@4点云数据每次回调函数会更新到map,直到laser的回调函数里才被使用在生成新的laser上
{
pcl_initial=true;
sensor_msgs::PointCloud2 output;
pcl::fromROSMsg(input, cloud);
// std::cout<<cloud.points.size()<<" ";
//@@1留下一个正方体内的点云,其余多余数据滤除,并进行体素降采样
pass.setInputCloud (cloud.makeShared());//设置输入点云
pass.setFilterFieldName ("x");//滤波字段名被设置为Z轴方向
pass.setFilterLimits (-2, 2);//可接受的范围为(0.0,1.0)
pass.setFilterLimitsNegative (false);//设置保留范围内 还是 过滤掉范围内
pass.filter (cloud_downsampled); //执行滤波,保存过滤结果在cloud_filtered
pass.setInputCloud (cloud_downsampled.makeShared());//设置输入点云
pass.setFilterFieldName ("y");//滤波字段名被设置为Z轴方向
pass.setFilterLimits (-2, 2);//可接受的范围为(0.0,1.0)
pass.setFilterLimitsNegative (false);//设置保留范围内 还是 过滤掉范围内
pass.filter (cloud_downsampled); //执行滤波,保存过滤结果在cloud_filtered
pass.setInputCloud (cloud_downsampled.makeShared());//设置输入点云
pass.setFilterFieldName ("z");//滤波字段名被设置为Z轴方向
pass.setFilterLimits (-2, 2);//可接受的范围为(0.0,1.0)
pass.setFilterLimitsNegative (false);//设置保留范围内 还是 过滤掉范围内
pass.filter (cloud_downsampled); //执行滤波,保存过滤结果在cloud_filtered
voxelSampler.setInputCloud(cloud_downsampled.makeShared());
voxelSampler.setLeafSize(0.1f, 0.1f, 0.1f); //
voxelSampler.filter(cloud_downsampled);
// /*下采样*/
try//这边是给
{
/*旋转平移*/ //将直通滤波后的点云旋转平移到map坐标系下
ros::Time now = input.header.stamp;
listener.waitForTransform("/map", "/camera_depth_frame", now, ros::Duration(3.0));
listener.lookupTransform("/map", "/camera_depth_frame", now, transform); //
tf::Quaternion q(transform.getRotation().getX(),transform.getRotation().getY(),transform.getRotation().getZ(),transform.getRotation().getW());
tf::Matrix3x3(q).getRPY(roll, pitch, yaw);
/*旋转平移*/
/*@@2PREturn*///一个提前的补偿(旋转),让点云正常分布
Eigen::Affine3f transform_me = Eigen::Affine3f::Identity();
transform_me.rotate (Eigen::AngleAxisf (PI/2, Eigen::Vector3f::UnitY()));
transform_me = Eigen::Affine3f::Identity();
transform_me.rotate (Eigen::AngleAxisf (-PI/2, Eigen::Vector3f::UnitX()));
pcl::transformPointCloud (cloud_downsampled, cloud_downsampled, transform_me);
transform_me = Eigen::Affine3f::Identity();
transform_me.rotate (Eigen::AngleAxisf (-PI/2, Eigen::Vector3f::UnitZ()));
pcl::transformPointCloud (cloud_downsampled, cloud_downsampled, transform_me);
/*@@3PREturn*///坐标系转换(真正的旋转平移)
transform_me = Eigen::Affine3f::Identity();
transform_me.translation() << transform.getOrigin().x(), transform.getOrigin().y(), transform.getOrigin().z();
transform_me.rotate (Eigen::AngleAxisf (yaw, Eigen::Vector3f::UnitZ()));
pcl::transformPointCloud (cloud_downsampled, cloud_downsampled, transform_me);
// std::cout<<transform.getOrigin().x()<<" "<<transform.getOrigin().y()<<" "<< transform.getOrigin().z()<<" "<<yaw<<" "<<roll<<" "<<pitch;
// std::cout<<" xuanzhunan--- ";
//发送
// std::cout<<cloud_downsampled.points.size()<<std::endl;
pcl::toROSMsg(cloud_downsampled, output);
output.header.frame_id="map";
output.header.stamp=input.header.stamp;
pcl_pub.publish(output);
}
catch (tf::TransformException &ex)
{
std::cout<<"pcl旋转map没有找到该时刻点坐标系"<<std::endl;
}
}
void PCL_SCAN_fusion(const sensor_msgs::LaserScan &input) //替换laser的rang的数据
{
if(!pcl_initial)
{
std::cout<<"pcl没有接准备好数据,quit"<<std::endl;
return;
}
// std::vector<float> *ranges = &input.ranges;
/*pcl数据旋转平移到laser坐标系下*/
try
{
//@@5把上面旋转平移到map的pcl数据旋转平移到雷达坐标系下
ros::Time now = input.header.stamp;
listener.waitForTransform("/laser", "/map", now, ros::Duration(3.0));
listener.lookupTransform("/laser", "/map", now, transform);
tf::Quaternion q(transform.getRotation().getX(),transform.getRotation().getY(),transform.getRotation().getZ(),transform.getRotation().getW());
tf::Matrix3x3(q).getRPY(roll, pitch, yaw);
Eigen::Affine3f transform_me = Eigen::Affine3f::Identity();
transform_me = Eigen::Affine3f::Identity();
transform_me.translation() << transform.getOrigin().x(), transform.getOrigin().y(), transform.getOrigin().z();
transform_me.rotate (Eigen::AngleAxisf (yaw, Eigen::Vector3f::UnitZ()));
pcl::transformPointCloud (cloud_downsampled, cloud_for_fusion, transform_me);
/*@@6膨胀*///让点云看起来密集一点,这样才有助于栅格地图建图
transform_me = Eigen::Affine3f::Identity();
transform_me.translation() << -0.02, 0, 0;
pcl::transformPointCloud (cloud_for_fusion, copy_cloud, transform_me);
transform_me.translation() << 0, -0.02, 0;
pcl::transformPointCloud (cloud_for_fusion, copy_cloud_1, transform_me);
transform_me.translation() << 0.02, 0, 0;
pcl::transformPointCloud (cloud_for_fusion, copy_cloud_2, transform_me);
transform_me.translation() << 0, 0.02, 0;
pcl::transformPointCloud (cloud_for_fusion, copy_cloud_3, transform_me);
copy_cloud =copy_cloud+copy_cloud_1;
copy_cloud_2=copy_cloud_2+copy_cloud_3;
copy_cloud =copy_cloud+copy_cloud_2;
cloud_for_fusion=cloud_for_fusion+ copy_cloud;
//@@7用于覆盖laserscan原数据的点云过多,计算延迟过大,减少数据量
pass.setInputCloud (cloud_for_fusion.makeShared());//设置输入点云
pass.setFilterFieldName ("z");//滤波字段名被设置为Z轴方向
pass.setFilterLimits (0, 1);//可接受的范围为(0.0,1.0) //膨胀后点云过多,去除一点
pass.setFilterLimitsNegative (false);//设置保留范围内 还是 过滤掉范围内
pass.filter (cloud_for_fusion); //执行滤波,保存过滤结果在cloud_filtered
/*测试点云旋转到laser效果*/
sensor_msgs::PointCloud2 output;
pcl::toROSMsg(cloud_for_fusion, output);
output.header.frame_id="laser";
output.header.stamp=input.header.stamp;
pcl_test_pub.publish(output);
/*遍历所有点云,将其替代range*/
//@@8计算laser坐标系下点云xy坐标的角度,找到laser里相应的ranges序号,并判断它到原点的距离,如果距离比原数据小则替代原
laserscan_output=input;
for (int i=0;i<cloud_for_fusion.points.size();i++)
{
//求角度
float x= cloud_for_fusion.points[i].x;
float y= cloud_for_fusion.points[i].y;
if(x==0)
{
continue;
}
float angel=atan2(y,x);
int seq_range = (angel-input.angle_min)/input.angle_increment;
std::cout<<" x:"<<x<<" y:"<<y<<" angel:"<<angel<<std::endl;
if(laserscan_output.ranges[seq_range]>sqrt(x*x+y*y))
{
laserscan_output.ranges[seq_range]=sqrt(x*x+y*y);
}
}
scan_pub.publish(laserscan_output);
/*遍历所有点云,将其替代range*/
}
catch(tf::TransformException &ex)
{
std::cout<<"pcl旋转到laser没有找到该时刻点坐标系"<<std::endl;
}
}
protected:
ros::NodeHandle nh;
ros::Subscriber pcl_sub;
ros::Publisher pcl_pub;
ros::Subscriber scan_sub;
ros::Publisher scan_pub;
ros::Publisher pcl_test_pub;
};
main(int argc, char **argv)
{
ros::init(argc, argv, "Astra_pcl_downsampling");
cloudHandler handler;
ros::spin();
return 0;
}
实现效果
图1
图2
图3
图1和图2为rviz中不同角度的截图,图3为小车在实际环境中的处境
合并工作的效果