package文件结构

程序结构：

main.cpp（自己的聚类程序）
CMakeLists.txt（创建package时自动生成的，需要改造内容）
package.xml（创建package时自动生成的，需要改造内容）
include 文件夹（创建package时自动生成的，空文件夹）
src 文件夹（创建package时自动生成的，空文件夹）

实现一个初步的聚类package只要改动3个文件，分别是main.cpp, CMakeLists.txt, package.xml, 内容如下：

main.cpp

//参考：https://blog.csdn.net/weixin_42503785/article/details/110362740
#include <ros/ros.h>
#include <cv_bridge/cv_bridge.h>using namespace std;#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_cloud.h>#include <pcl/ModelCoefficients.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/filters/passthrough.h>#include "std_msgs/Int8.h"
#include "std_msgs/String.h"ros::Publisher pub;void pclCloudCallback(const sensor_msgs::PointCloud2ConstPtr& input)
{//修改相机参数的方法。// //备注：如果提示 No module named 'rospkg'，请退出conda环境再执行launch启动。// system("rosrun dynamic_reconfigure dynparam set /camera/rgb_camera/ enable_auto_exposure 0");// 创建一个输出的数据格式sensor_msgs::PointCloud2 output;  //ROS中点云的数据格式//对数据进行处理pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);output = *input;pcl::fromROSMsg(output,*cloud);std::cout<<"direct_trans: "<<std::endl;//*********************** 1 点云稀疏化 ***********************//// 创建过滤对象：使用1cm（0.01）的叶大小对数据集进行下采样pcl::VoxelGrid<pcl::PointXYZ> vg;pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);vg.setInputCloud(cloud);//vg.setLeafSize(0.01f, 0.01f, 0.01f);vg.setLeafSize(0.1f, 0.1f, 0.1f);//单位：米vg.filter(*cloud_filtered);std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size() << " data points." << std::endl; //*//*********************** 2 筛除平面点云 ***********************//pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_f(new pcl::PointCloud<pcl::PointXYZ>);pcl::SACSegmentation<pcl::PointXYZ> seg;    //实例化一个分割对象pcl::PointIndices::Ptr inliers(new pcl::PointIndices);//实例化一个索引pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);//实例化模型参数pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane(new pcl::PointCloud<pcl::PointXYZ>());//提取到的平面保存至cloud_planepcl::PCDWriter writer;seg.setOptimizeCoefficients(true);//可选设置,设置模型系数需要优化seg.setModelType(pcl::SACMODEL_PLANE);//设置分割的模型类型seg.setMethodType(pcl::SAC_RANSAC);//设置分割的参数估计方法seg.setMaxIterations(100);//最大迭代次数seg.setDistanceThreshold(0.02);//设置内点到模型的距离允许最大值int i = 0, nr_points = (int)cloud_filtered->points.size();//计数变量i，记下提取的平面的个数while (cloud_filtered->points.size() > 0.3 * nr_points){//从剩余的云中分割最大的平面组件seg.setInputCloud(cloud_filtered);//设置要分割的点云seg.segment(*inliers, *coefficients);   //分割，存储分割结果到点集合inliers及存储平面模型的系数coefficientsif (inliers->indices.size() == 0)//如果平面点索引的数量为0{//std::cout << "Could not estimate a planar model for the given dataset." << std::endl;break;}pcl::ExtractIndices<pcl::PointXYZ> extract;//实例化一个提取对象extract.setInputCloud(cloud_filtered);//设置要提取的点云extract.setIndices(inliers);//根据分割得到的平面的索引提取平面extract.setNegative(false);//false：提取内点// Write the planar inliers to diskextract.filter(*cloud_plane);//保存提取到的平面//std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size() << " data points." << std::endl;// Remove the planar inliers, extract the restextract.setNegative(true);//提取外点（除第一个平面之外的点）extract.filter(*cloud_f);//保存除平面之外的剩余点cloud_filtered = cloud_f;//将剩余点作为下一次分割、提取的平面的输入点云}//*********************** 3 点云欧式聚类 ***********************//// 为提取的搜索方法创建KdTree对象pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);tree->setInputCloud(cloud_filtered); //将无法提取平面的点云作为 cloud_filteredstd::vector<pcl::PointIndices> cluster_indices; //保存每一种聚类，每一种聚类下还有具体的聚类的点pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;//实例化一个欧式聚类提取对象//realsense点云XYZ值单位是：米m，Kinect点云XYZ值单位是：毫米mm// ec.setClusterTolerance(0.02);     //近邻搜索的搜索半径，重要参数, 单位：米 // ec.setMinClusterSize(50);         //设置一个聚类需要的最少点数目为100ec.setClusterTolerance(0.2);     //近邻搜索的搜索半径，重要参数, 单位：米 ec.setMinClusterSize(5);         //设置一个聚类需要的最少点数目为100ec.setMaxClusterSize(25000);      //设置一个聚类最大点数目为25000ec.setSearchMethod(tree);         //设置点云的搜索机制ec.setInputCloud(cloud_filtered); //设置输入点云ec.extract(cluster_indices);      //将聚类结果保存至 cluster_indices 中//*********************** 4 计算每一个聚类的bbox的中心点XYZ，及其bbox的length，width，height ***********************//// 改造自：https://blog.csdn.net/AdamShan/article/details/83015570double output_width;for (size_t i = 0; i < cluster_indices.size(); i++){//质心的坐标pcl::PointXYZ centroid_;pcl::PointXYZ min_point_;pcl::PointXYZ max_point_;float min_x = std::numeric_limits<float>::max();float max_x = -std::numeric_limits<float>::max();float min_y = std::numeric_limits<float>::max();float max_y = -std::numeric_limits<float>::max();float min_z = std::numeric_limits<float>::max();float max_z = -std::numeric_limits<float>::max();for (auto pit = cluster_indices[i].indices.begin(); pit != cluster_indices[i].indices.end(); ++pit){//fill new colored cluster point by pointpcl::PointXYZ p;p.x = cloud_filtered->points[*pit].x;p.y = cloud_filtered->points[*pit].y;p.z = cloud_filtered->points[*pit].z;centroid_.x += p.x;centroid_.y += p.y;centroid_.z += p.z;if (p.x < min_x)min_x = p.x;if (p.y < min_y)min_y = p.y;if (p.z < min_z)min_z = p.z;if (p.x > max_x)max_x = p.x;if (p.y > max_y)max_y = p.y;if (p.z > max_z)max_z = p.z;}//min, max pointsmin_point_.x = min_x;min_point_.y = min_y;min_point_.z = min_z;max_point_.x = max_x;max_point_.y = max_y;max_point_.z = max_z;//calculate centroid 计算质心if (cluster_indices[i].indices.size() > 0){centroid_.x /= cluster_indices[i].indices.size();centroid_.y /= cluster_indices[i].indices.size();centroid_.z /= cluster_indices[i].indices.size();}//calculate bounding boxdouble length_ = max_point_.x - min_point_.x;double width_ = max_point_.y - min_point_.y;double height_ = max_point_.z - min_point_.z;// std::cout << "聚类序号:" << i << std::endl;// std::cout << "center_x:" << min_point_.x + length_ / 2 << std::endl;// std::cout << "center_y:" << min_point_.y + width_ / 2 << std::endl;// std::cout << "center_z:" << min_point_.z + height_ / 2 << std::endl;// std::cout << "length:" << ((length_ < 0) ? -1 * length_ : length_) << std::endl;// std::cout << "width:" << ((width_ < 0) ? -1 * width_ : width_) << std::endl;// std::cout << "cheight:" << ((height_ < 0) ? -1 * height_ : height_) << std::endl;output_width = ((width_ < 0) ? -1 * width_ : width_); }// 将某个结果信息实时发布出去std_msgs::String msg;std::stringstream ss;ss << std::to_string(output_width);msg.data = ss.str();pub.publish(msg);
}int main(int argc, char **argv)
{//创建node第一步需要用到的函数ros::init(argc, argv, "jisuan_julei"); //第3个参数为本节点名，//ros::NodeHandle : 和topic、service、param等交互的公共接口//创建ros::NodeHandle对象，也就是节点的句柄，它可以用来创建Publisher、Subscriber以及做其他事情。//句柄(Handle)这个概念可以理解为一个“把手”，你握住了门把手，就可以很容易把整扇门拉开，而不必关心门是//什么样子。NodeHandle就是对节点资源的描述，有了它你就可以操作这个节点了，比如为程序提供服务、监听某个topic上的消息、访问和修改param等等。ros::NodeHandle nd; //实例化句柄，初始化node// Create a ROS subscriber for the input point cloudros::Subscriber sub = nd.subscribe("/camera1/depth/color/points", 1, pclCloudCallback);std::cout << "sub:" << sub << std::endl;// Create a ROS publisher for the output point cloudpub = nd.advertise<std_msgs::String>("julei_out", 1);ros::spin();return 0;
}

CMakeLists.txt

cmake_minimum_required(VERSION 3.0.2)
project(julei_pkg)## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTSroscpprospystd_msgscv_bridge
)###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if your package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES julei_pkg
#  CATKIN_DEPENDS roscpp rospy std_msgs
#  DEPENDS system_lib
)###########
## Build ##
############# Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
# include${catkin_INCLUDE_DIRS}
)#此句要在下面target_link_libraries语句之前
add_executable(julei julei.cpp)#PCL PCL PCL PCL PCL PCL PCL PCL PCL PCL
#如果你想把 PCL 里所有的模块都找到，那就这样写
find_package(PCL REQUIRED)
#为了让 CMake 寻找到 PCL 的头文件，需要以下三句
include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})
#把我们生成的可执行二进制文件链接到 PCL 的库
target_link_libraries(julei ${PCL_LIBRARIES})target_link_libraries(julei ${catkin_LIBRARIES})

package.xml

<?xml version="1.0"?>
<package format="2"><name>julei_pkg</name><version>0.0.0</version><description>The julei_pkg package</description><!-- One maintainer tag required, multiple allowed, one person per tag --><!-- Example:  --><!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> --><maintainer email="ym@todo.todo">ym</maintainer><!-- One license tag required, multiple allowed, one license per tag --><!-- Commonly used license strings: --><!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 --><license>TODO</license><!-- Url tags are optional, but multiple are allowed, one per tag --><!-- Optional attribute type can be: website, bugtracker, or repository --><!-- Example: --><!-- <url type="website">http://wiki.ros.org/julei_pkg</url> --><!-- Author tags are optional, multiple are allowed, one per tag --><!-- Authors do not have to be maintainers, but could be --><!-- Example: --><!-- <author email="jane.doe@example.com">Jane Doe</author> --><!-- The *depend tags are used to specify dependencies --><!-- Dependencies can be catkin packages or system dependencies --><!-- Examples: --><!-- Use depend as a shortcut for packages that are both build and exec dependencies --><!--   <depend>roscpp</depend> --><!--   Note that this is equivalent to the following: --><!--   <build_depend>roscpp</build_depend> --><!--   <exec_depend>roscpp</exec_depend> --><!-- Use build_depend for packages you need at compile time: --><!--   <build_depend>message_generation</build_depend> --><!-- Use build_export_depend for packages you need in order to build against this package: --><!--   <build_export_depend>message_generation</build_export_depend> --><!-- Use buildtool_depend for build tool packages: --><!--   <buildtool_depend>catkin</buildtool_depend> --><!-- Use exec_depend for packages you need at runtime: --><!--   <exec_depend>message_runtime</exec_depend> --><!-- Use test_depend for packages you need only for testing: --><!--   <test_depend>gtest</test_depend> --><!-- Use doc_depend for packages you need only for building documentation: --><!--   <doc_depend>doxygen</doc_depend> --><buildtool_depend>catkin</buildtool_depend><build_depend>roscpp</build_depend><build_depend>rospy</build_depend><build_depend>std_msgs</build_depend><build_depend>cv_bridge</build_depend><build_export_depend>roscpp</build_export_depend><build_export_depend>rospy</build_export_depend><build_export_depend>std_msgs</build_export_depend><build_export_depend>cv_bridge</build_export_depend><exec_depend>roscpp</exec_depend><exec_depend>rospy</exec_depend><exec_depend>std_msgs</exec_depend><exec_depend>cv_bridge</exec_depend><!-- The export tag contains other, unspecified, tags --><export><!-- Other tools can request additional information be placed here --></export>
</package>

ROS系统中实现点云聚类（realsense数据源）相关推荐

疑难杂症篇(十八)--ROS系统中使用SLAM算法建图时出现地图漂移的几种原因
本篇主要介绍在ROS系统中使用SLAM算法建地图时出现地图定位漂移的几种原因及可以采取的措施. 1.SLAM建图时出现的定位漂移现象 2.原因分析里程计数据发生异常: 计算机的配置不高,计算机建图过 ...
思岚A1激光雷达windows系统与ROS系统中的使用
前言: 忙了一天,遇到了无数个问题解不开,结果竟然是因为安卓的USB连接线有问题,博友们,如果你们遇到如下问题: 思岚A1激光雷达windows上位机的使用:下载了串口的驱动(CP210x VCP W ...
ROS系统中编写多个C++文件时，主文件调用其它文件函数或类时出现：对“xxxxxx“未定义的引用问题记录
问题描述主文件对其它文件内容调用时,明明已经正确引用相关头文件,并确保类和函数已被定义,仍然出现下面问题. CMakeFiles/robot_pose_ukf_node.dir/src/main.c ...
星网宇达-组合导航在ros系统中的使用方法
1.安装差分天线(两个).组合导航主机.DTU模块.不使用基站GPS的定位精度是米级的,园区里面自己搭建基站用DTU进行数据传输精度可达到厘米级.也可以利用千寻基站进行定位,效果不清楚. 2.根据&q ...
0基础在ROS系统中实现RRT算法（一）URDF集成rviz并建立机器人模型
小白一枚,毕设突发奇想加入了ROS的内容,不知道自己还能不能毕业.以下均为通过看视频,翻博客等整理而成的笔记,并非我的原创.可能会出现一些报错的修改或者简单的代码是我自己做的.哈哈. 一些小白知识: ...
ROS系统中的多个版本Boost问题
1.删除多余的boost版本,只需要删除该版本的libboost*库以及Boost的头文件,不需要使用sudo apt-get rm --pugre libboost-dev sudo apt-get ...
ROS系统中launch文件无法启动
方法一:LAUNCH文件无法启动,权限问题,开权限. 方法二: source devel/setup.bash
ROS系统——部署OpenVINO版Nanodet超轻量目标检测器
目录 0 背景本人的实测效果: 1 环境搭建 2 先熟悉OpenVINO版nanodet的流程 3 在ROS里部署openvino版nanodet的流程 4 源码 4.1 main.cpp内容 4 ...
SLAM+语音机器人DIY系列：（二）ROS入门——2.ROS系统整体架构
摘要 ROS机器人操作系统在机器人应用领域很流行,依托代码开源和模块间协作等特性,给机器人开发者带来了很大的方便.我们的机器人"miiboo"中的大部分程序也采用ROS进行开发,所 ...

ROS系统中实现点云聚类（realsense数据源）

package文件结构

main.cpp

CMakeLists.txt

package.xml

ROS系统中实现点云聚类（realsense数据源）相关推荐

最新文章

热门文章