initial commit 2
This commit is contained in:
caroline
parent
23719e5a76
commit
5d509e4e85
@ -14,6 +14,14 @@ find_package(catkin REQUIRED COMPONENTS
|
|||||||
genmsg
|
genmsg
|
||||||
pcl_conversions
|
pcl_conversions
|
||||||
pcl_ros
|
pcl_ros
|
||||||
|
dynamic_reconfigure
|
||||||
|
)
|
||||||
|
|
||||||
|
#add dynamic reconfigure api
|
||||||
|
#find_package(catkin REQUIRED dynamic_reconfigure)
|
||||||
|
generate_dynamic_reconfigure_options(
|
||||||
|
cfg/config.cfg
|
||||||
|
#...
|
||||||
)
|
)
|
||||||
|
|
||||||
## System dependencies are found with CMake's conventions
|
## System dependencies are found with CMake's conventions
|
||||||
@ -208,5 +216,6 @@ add_executable(initial_processing src/initial_processing.cpp)
|
|||||||
target_link_libraries(initial_processing ${catkin_LIBRARIES})
|
target_link_libraries(initial_processing ${catkin_LIBRARIES})
|
||||||
add_dependencies(initial_processing pcl_tutorial_generate_messages_cpp)
|
add_dependencies(initial_processing pcl_tutorial_generate_messages_cpp)
|
||||||
|
|
||||||
|
# make sure configure headers are built before any node using them
|
||||||
|
add_dependencies(initial_processing ${PROJECT_NAME}_gencfg)
|
||||||
|
|
||||||
|
|||||||
19
cfg/config.cfg
Executable file
19
cfg/config.cfg
Executable file
@ -0,0 +1,19 @@
|
|||||||
|
#!/usr/bin/env python
|
||||||
|
PACKAGE = "pcl_tutorial"
|
||||||
|
|
||||||
|
from dynamic_reconfigure.parameter_generator_catkin import *
|
||||||
|
|
||||||
|
gen = ParameterGenerator()
|
||||||
|
|
||||||
|
gen.add("xbpos", double_t, 0, "Max bound of X - Axis cropping", 5, -5, 5)
|
||||||
|
gen.add("xbneg", double_t, 0, "Min bound of X - Axis cropping", 0, -5, 5)
|
||||||
|
|
||||||
|
gen.add("ybpos", double_t, 0, "Max bound of Y - Axis cropping", 5, -5, 5)
|
||||||
|
gen.add("ybneg", double_t, 0, "Min bound of Y - Axis cropping", -5, -5, 5)
|
||||||
|
|
||||||
|
gen.add("zbpos", double_t, 0, "Max bound of Z - Axis cropping", 5, -5, 5)
|
||||||
|
gen.add("zbneg", double_t, 0, "Min bound of Z - Axis cropping", 0, -5, 5)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
exit(gen.generate(PACKAGE, "pcl_tutorial", "config"))
|
||||||
@ -2,16 +2,22 @@
|
|||||||
#include "std_msgs/String.h"
|
#include "std_msgs/String.h"
|
||||||
#include "std_msgs/Bool.h"
|
#include "std_msgs/Bool.h"
|
||||||
#include "std_msgs/Float32.h"
|
#include "std_msgs/Float32.h"
|
||||||
|
#include <dynamic_reconfigure/server.h>
|
||||||
|
#include <pcl_tutorial/configConfig.h>
|
||||||
|
|
||||||
|
|
||||||
#include <sstream>
|
#include <sstream>
|
||||||
|
#include <iostream>
|
||||||
#include <math.h>
|
#include <math.h>
|
||||||
|
|
||||||
#include <pcl/filters/passthrough.h>
|
#include <pcl/filters/passthrough.h>
|
||||||
#include <pcl/conversions.h>
|
#include <pcl/conversions.h>
|
||||||
#include <pcl_conversions/pcl_conversions.h>
|
#include <pcl_conversions/pcl_conversions.h>
|
||||||
|
#include <pcl/filters/passthrough.h>
|
||||||
|
#include <pcl/filters/voxel_grid.h>
|
||||||
#include <sensor_msgs/PointCloud2.h>
|
#include <sensor_msgs/PointCloud2.h>
|
||||||
#include <visualization_msgs/Marker.h>
|
#include <visualization_msgs/Marker.h>
|
||||||
|
#include <visualization_msgs/MarkerArray.h>
|
||||||
|
|
||||||
/*
|
/*
|
||||||
TBD: get to know typedef such as stringstream
|
TBD: get to know typedef such as stringstream
|
||||||
@ -19,38 +25,90 @@ TBD: get to know typedef such as stringstream
|
|||||||
|
|
||||||
// define publisher
|
// define publisher
|
||||||
ros::Publisher pub;
|
ros::Publisher pub;
|
||||||
|
ros::Publisher pub_2;
|
||||||
ros::Publisher visual;
|
ros::Publisher visual;
|
||||||
|
|
||||||
|
float xbpos = 5.0;
|
||||||
|
float xbneg = 0.0;
|
||||||
|
float ybpos = 5.0;
|
||||||
|
float ybneg = -5.0;
|
||||||
|
float zbpos = 5.0;
|
||||||
|
float zbneg = 0.0;
|
||||||
|
|
||||||
typedef pcl::PointXYZRGB PointT;
|
typedef pcl::PointXYZRGB PointT;
|
||||||
|
|
||||||
|
void callback(pcl_tutorial::configConfig &config, uint32_t level)
|
||||||
|
{
|
||||||
|
xbpos = config.xbpos;
|
||||||
|
xbneg = config.xbneg;
|
||||||
|
ybpos = config.ybpos;
|
||||||
|
ybneg = config.ybneg;
|
||||||
|
zbpos = config.zbpos;
|
||||||
|
zbneg = config.zbneg;
|
||||||
|
}
|
||||||
|
|
||||||
void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_msg){
|
void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_msg){
|
||||||
|
|
||||||
|
|
||||||
// create pointer to Point Cloud in pcl
|
// create pointer to Point Cloud in pcl
|
||||||
pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
|
pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
|
||||||
|
pcl::PointCloud<PointT>::Ptr cloud_filtered (new pcl::PointCloud<PointT>);
|
||||||
|
|
||||||
// convert ROS message data to point cloud in pcl
|
// convert ROS message data to point cloud in pcl
|
||||||
pcl::fromROSMsg(*cloud_msg, *cloud);
|
pcl::fromROSMsg(*cloud_msg, *cloud);
|
||||||
|
|
||||||
|
|
||||||
|
// Create filtering object
|
||||||
|
pcl::PassThrough<PointT> passx;
|
||||||
|
passx.setInputCloud (cloud);
|
||||||
|
passx.setFilterFieldName ("x");
|
||||||
|
passx.setFilterLimits (0.0, 5.0);
|
||||||
|
passx.filter (*cloud);
|
||||||
|
|
||||||
|
pcl::PassThrough<PointT> passy;
|
||||||
|
passy.setInputCloud (cloud);
|
||||||
|
passy.setFilterFieldName ("y");
|
||||||
|
passy.setFilterLimits (-5.0, 5.0);
|
||||||
|
passy.filter (*cloud);
|
||||||
|
|
||||||
|
pcl::PassThrough<PointT> passz;
|
||||||
|
passz.setInputCloud (cloud);
|
||||||
|
passz.setFilterFieldName ("z");
|
||||||
|
passz.setFilterLimits (0.0, 5.0);
|
||||||
|
passz.filter (*cloud);
|
||||||
|
|
||||||
|
|
||||||
|
/* pcl::VoxelGrid<PointT> vox; // Create the object for performing voxelgrid filtering //Maybe this has to be changes to RGB as well
|
||||||
|
vox.setInputCloud (cloud); // Set input cloud
|
||||||
|
vox.setLeafSize (0.02, 0.02, 0.02); // Set the size of the voxel
|
||||||
|
vox.filter (*cloud); */
|
||||||
|
|
||||||
// convert to ROS data type
|
// convert to ROS data type
|
||||||
sensor_msgs::PointCloud2 output;
|
sensor_msgs::PointCloud2 output;
|
||||||
pcl::toROSMsg(*cloud, output);
|
pcl::toROSMsg(*cloud, output);
|
||||||
pub.publish(output);
|
pub.publish(output);
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
int main(int argc, char **argv){
|
int main(int argc, char **argv){
|
||||||
|
|
||||||
// specify node name and initialize ROS
|
// specify node name and initialize ROS
|
||||||
ros::init(argc, argv, "image_processing");
|
ros::init(argc, argv, "image_processing");
|
||||||
|
|
||||||
// first/last node handle does the initialization of node/cleanup of used resources by node
|
// first/last node handle does the initialization of node/cleanup of used resources by node
|
||||||
ros::NodeHandle n;
|
ros::NodeHandle n;
|
||||||
|
|
||||||
|
dynamic_reconfigure::Server<pcl_tutorial::configConfig> server;
|
||||||
|
dynamic_reconfigure::Server<pcl_tutorial::configConfig>::CallbackType f;
|
||||||
|
|
||||||
|
f = boost::bind(&callback, _1, _2);
|
||||||
|
server.setCallback(f);
|
||||||
|
|
||||||
// create subscriber to receive point cloud
|
// create subscriber to receive point cloud
|
||||||
ros::Subscriber sub = n.subscribe<sensor_msgs::PointCloud2>("/zed/zed_node/point_cloud/cloud_registered", 1, cloud_cb);
|
ros::Subscriber sub = n.subscribe<sensor_msgs::PointCloud2>("/zed/zed_node/point_cloud/cloud_registered", 1, cloud_cb);
|
||||||
|
|
||||||
// create publisher for output processed point cloud
|
// create publisher for output processed point cloud
|
||||||
pub = n.advertise<sensor_msgs::PointCloud2>("processed_cloud", 1);
|
pub = n.advertise<sensor_msgs::PointCloud2>("processed_cloud", 1);
|
||||||
|
pub_2 = n.advertise<sensor_msgs::PointCloud2>("unprocessed_cloud", 1);
|
||||||
|
|
||||||
// create publisher for visualising marker
|
// create publisher for visualising marker
|
||||||
visual = n.advertise<visualization_msgs::Marker> ("marker", 1);
|
visual = n.advertise<visualization_msgs::Marker> ("marker", 1);
|
||||||
|
|||||||
693
src/markose_preprocessing.cpp
Executable file
693
src/markose_preprocessing.cpp
Executable file
@ -0,0 +1,693 @@
|
|||||||
|
#include <ros/ros.h>
|
||||||
|
// PCL specific includes
|
||||||
|
#include "std_msgs/Bool.h"
|
||||||
|
#include "std_msgs/Float32.h"
|
||||||
|
#include <dynamic_reconfigure/server.h>
|
||||||
|
#include <iostream>
|
||||||
|
#include <math.h>
|
||||||
|
#include <pcl/ModelCoefficients.h>
|
||||||
|
#include <pcl/common/centroid.h>
|
||||||
|
#include <pcl/common/common.h>
|
||||||
|
#include <pcl/conversions.h>
|
||||||
|
#include <pcl/features/normal_3d.h>
|
||||||
|
#include <pcl/filters/conditional_removal.h>
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
#include <pcl/common/transforms.h>
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
#include <pcl/filters/extract_indices.h>
|
||||||
|
#include <pcl/filters/passthrough.h>
|
||||||
|
#include <pcl/filters/statistical_outlier_removal.h>
|
||||||
|
#include <pcl/filters/voxel_grid.h>
|
||||||
|
#include <pcl/io/pcd_io.h>
|
||||||
|
#include <pcl/kdtree/kdtree.h>
|
||||||
|
#include <pcl/point_cloud.h>
|
||||||
|
#include <pcl/point_types.h>
|
||||||
|
#include <pcl/point_cloud.h>
|
||||||
|
#include <pcl/point_types_conversion.h>
|
||||||
|
#include <pcl/sample_consensus/method_types.h>
|
||||||
|
#include <pcl/sample_consensus/sac_model.h>
|
||||||
|
#include <pcl/segmentation/extract_clusters.h>
|
||||||
|
#include <pcl/segmentation/sac_segmentation.h>
|
||||||
|
#include <pcl_conversions/pcl_conversions.h>
|
||||||
|
#include <pcl_proc/configConfig.h>
|
||||||
|
#include <sensor_msgs/PointCloud2.h>
|
||||||
|
#include <visualization_msgs/Marker.h>
|
||||||
|
#include <visualization_msgs/MarkerArray.h>
|
||||||
|
|
||||||
|
|
||||||
|
bool color = false;
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
ros::Publisher pub;
|
||||||
|
ros::Publisher pub2;
|
||||||
|
ros::Publisher object;
|
||||||
|
ros::Publisher mark_l;
|
||||||
|
ros::Publisher mark_r;
|
||||||
|
ros::Publisher steer;
|
||||||
|
ros::Publisher cont_val;
|
||||||
|
|
||||||
|
float prevcentroid = 0.0;
|
||||||
|
float prevweight = 0.8;
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
// Dynamic parameters
|
||||||
|
// Voxel grid params
|
||||||
|
bool voxel_enable = true;
|
||||||
|
float voxel_size = 0.02;
|
||||||
|
// X - Passthrough filtering params
|
||||||
|
bool xpass_enable = true;
|
||||||
|
float xbpos = 5.0;
|
||||||
|
float xbneg = 0.0;
|
||||||
|
// Y - Passthrough filtering params
|
||||||
|
bool ypass_enable = true;
|
||||||
|
float ybpos = 5.0;
|
||||||
|
float ybneg = -5.0;
|
||||||
|
// Z - Passthrough filtering params
|
||||||
|
bool zpass_enable = true;
|
||||||
|
float zbpos = 5.0;
|
||||||
|
float zbneg = 0.0;
|
||||||
|
// Planar Segmentation params
|
||||||
|
bool plane_segmentation_enable = true;
|
||||||
|
float segthresh = 0.025; // 0.01 - 0.1
|
||||||
|
|
||||||
|
// Outlier Removal params
|
||||||
|
bool outlier_filtering_enable = true;
|
||||||
|
int k_neighbors = 50; // 1 - 500
|
||||||
|
int stdthresh = 3; // 1 - 50
|
||||||
|
|
||||||
|
// Euclidean clustering params
|
||||||
|
float clusttol = 0.2; // 0 - 1
|
||||||
|
int minclust = 50;
|
||||||
|
int maxclust = 25000;
|
||||||
|
|
||||||
|
//Conditional color segmentation
|
||||||
|
bool color_filtering_enable = true;
|
||||||
|
int greenthresh = 150;
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
typedef pcl::PointXYZRGB PointT;
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
void callback(pcl_proc::configConfig &config, uint32_t level)
|
||||||
|
{
|
||||||
|
voxel_enable = config.voxel_enable;
|
||||||
|
voxel_size = config.voxel_size;
|
||||||
|
xpass_enable = config.xpass_enable;
|
||||||
|
xbpos = config.xbpos;
|
||||||
|
xbneg = config.xbneg;
|
||||||
|
ypass_enable = config.ypass_enable;
|
||||||
|
ybpos = config.ybpos;
|
||||||
|
ybneg = config.ybneg;
|
||||||
|
zpass_enable = config.zpass_enable;
|
||||||
|
zbpos = config.zbpos;
|
||||||
|
zbneg = config.zbneg;
|
||||||
|
plane_segmentation_enable = config.plane_segmentation_enable;
|
||||||
|
segthresh = config.segthresh;
|
||||||
|
outlier_filtering_enable = config.outlier_filtering_enable;
|
||||||
|
k_neighbors = config.k_neighbors;
|
||||||
|
stdthresh = config.stdthresh;
|
||||||
|
clusttol = config.clusttol;
|
||||||
|
minclust = config.minclust;
|
||||||
|
maxclust = config.maxclust;
|
||||||
|
color_filtering_enable = config.color_filtering_enable;
|
||||||
|
greenthresh = config.greenthresh;
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_msg)
|
||||||
|
{
|
||||||
|
// Container for original & filtered data //Tested : This works, Changing the type of pointer changes whether the cloud is processed in color
|
||||||
|
pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
|
||||||
|
pcl::PointCloud<PointT>::Ptr uncloud(new pcl::PointCloud<PointT>);
|
||||||
|
|
||||||
|
|
||||||
|
// Convert to PCL Data type irrespective of type
|
||||||
|
pcl::fromROSMsg(*cloud_msg, *cloud);
|
||||||
|
pcl::fromROSMsg(*cloud_msg, *uncloud);
|
||||||
|
pcl::PointCloud<pcl::PointXYZHSV>::Ptr hsv_cloud(new pcl::PointCloud<pcl::PointXYZHSV>);
|
||||||
|
|
||||||
|
// Voxelgrid filtering (reduces number of points)
|
||||||
|
// Voxelgrid filtering discretizes the entire point cloud into boxes named voxels and calculates the centroid of all the points ina voxel.
|
||||||
|
// These points are all then replaced by the centroid thereby downsampling the cloud effectively.
|
||||||
|
|
||||||
|
|
||||||
|
// //Perform a translation and rotation on the object cloud
|
||||||
|
// float rotx = 0;
|
||||||
|
// float roty = 45;
|
||||||
|
// float rotz = 0;
|
||||||
|
// Eigen::Affine3f transform = Eigen::Affine3f::Identity();
|
||||||
|
// transform.translation() << 0.0, 0.0, 0.8;
|
||||||
|
// transform.rotate(Eigen::AngleAxisf((rotx*M_PI) / 180, Eigen::Vector3f::UnitX()));
|
||||||
|
// transform.rotate(Eigen::AngleAxisf((roty*M_PI) / 180, Eigen::Vector3f::UnitY()));
|
||||||
|
// transform.rotate(Eigen::AngleAxisf((rotz*M_PI) / 180, Eigen::Vector3f::UnitZ()));
|
||||||
|
// pcl::transformPointCloud(*cloud, *cloud, transform);
|
||||||
|
|
||||||
|
|
||||||
|
if (xpass_enable)
|
||||||
|
{
|
||||||
|
pcl::PassThrough<PointT> passx;
|
||||||
|
passx.setInputCloud (cloud);
|
||||||
|
passx.setFilterFieldName ("x");
|
||||||
|
passx.setFilterLimits (xbneg, xbpos);
|
||||||
|
passx.filter (*cloud);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (ypass_enable)
|
||||||
|
{
|
||||||
|
pcl::PassThrough<PointT> passy;
|
||||||
|
passy.setInputCloud (cloud);
|
||||||
|
passy.setFilterFieldName ("y");
|
||||||
|
passy.setFilterLimits (ybneg, ybpos);
|
||||||
|
passy.filter (*cloud);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (ypass_enable)
|
||||||
|
{
|
||||||
|
pcl::PassThrough<PointT> passz;
|
||||||
|
passz.setInputCloud (cloud);
|
||||||
|
passz.setFilterFieldName ("z");
|
||||||
|
passz.setFilterLimits (zbneg, zbpos);
|
||||||
|
passz.filter (*cloud);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
if (voxel_enable)
|
||||||
|
{
|
||||||
|
pcl::VoxelGrid<PointT> vox; // Create the object for performing voxelgrid filtering //Maybe this has to be changes to RGB as well
|
||||||
|
vox.setInputCloud (cloud); // Set input cloud
|
||||||
|
vox.setLeafSize (voxel_size, voxel_size, voxel_size); // Set the size of the voxel
|
||||||
|
vox.filter (*cloud); // Perform the actual filtering
|
||||||
|
}
|
||||||
|
|
||||||
|
//Passthrough filtering (set maximum and minimum bounds)
|
||||||
|
|
||||||
|
|
||||||
|
// Segmentation step
|
||||||
|
|
||||||
|
if (plane_segmentation_enable)
|
||||||
|
{
|
||||||
|
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
|
||||||
|
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
|
||||||
|
// Create the segmentation object
|
||||||
|
pcl::SACSegmentation<PointT> seg;
|
||||||
|
// Optional
|
||||||
|
seg.setOptimizeCoefficients (true);
|
||||||
|
// Mandatory
|
||||||
|
seg.setModelType (pcl::SACMODEL_PLANE);
|
||||||
|
seg.setMethodType (pcl::SAC_RANSAC);
|
||||||
|
seg.setDistanceThreshold (segthresh);
|
||||||
|
seg.setInputCloud (cloud);
|
||||||
|
seg.segment (*inliers, *coefficients);
|
||||||
|
|
||||||
|
/*Calculation of unit vector for transformation of the point cloud in later steps*/
|
||||||
|
// std::cout<<inliers[1].x<<" X Point ";
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
//Extraction step
|
||||||
|
|
||||||
|
pcl::ExtractIndices<PointT> extract;
|
||||||
|
extract.setInputCloud (cloud);
|
||||||
|
extract.setIndices (inliers);
|
||||||
|
extract.setNegative (true);
|
||||||
|
extract.filter(*cloud);
|
||||||
|
}
|
||||||
|
|
||||||
|
// pcl::PointCloudXYZRGBtoXYZHSV(*cloud, *hsv_cloud);
|
||||||
|
// pcl::PointIndices::Ptr inliers2(new pcl::PointIndices);
|
||||||
|
// for (int i = 0; i < hsv_cloud->size(); i ++)
|
||||||
|
// {
|
||||||
|
// if (hsv_cloud->points[i].h > 36 && hsv_cloud->points[i].h < 76)
|
||||||
|
// {
|
||||||
|
// inliers2->indices.push_back(i);
|
||||||
|
|
||||||
|
// }
|
||||||
|
// }
|
||||||
|
// pcl::ExtractIndices<pcl::PointXYZHSV> filter(true);
|
||||||
|
|
||||||
|
// filter.setInputCloud(hsv_cloud);
|
||||||
|
// filter.setIndices(inliers2);
|
||||||
|
// filter.filter(*hsv_cloud);
|
||||||
|
// pcl::PointXYZHSVtoXYZRGB(*hsv_cloud, *cloud);
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
if (color_filtering_enable)
|
||||||
|
{
|
||||||
|
pcl::ConditionalRemoval<pcl::PointXYZRGB> color_filter;
|
||||||
|
pcl::PackedRGBComparison<pcl::PointXYZRGB>::Ptr
|
||||||
|
green_condition(new pcl::PackedRGBComparison<pcl::PointXYZRGB>("g", pcl::ComparisonOps::GT, greenthresh));
|
||||||
|
pcl::ConditionAnd<pcl::PointXYZRGB>::Ptr color_cond (new pcl::ConditionAnd<pcl::PointXYZRGB> ());
|
||||||
|
color_cond->addComparison (green_condition);
|
||||||
|
|
||||||
|
// Build the filter
|
||||||
|
color_filter.setInputCloud(cloud);
|
||||||
|
color_filter.setCondition (color_cond);
|
||||||
|
color_filter.filter(*cloud);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
// if (outlier_filtering_enable)
|
||||||
|
// {
|
||||||
|
// pcl::StatisticalOutlierRemoval<PointT> sori;
|
||||||
|
// sori.setInputCloud (cloud);
|
||||||
|
// sori.setMeanK (k_neighbors);
|
||||||
|
// sori.setStddevMulThresh (stdthresh);
|
||||||
|
// sori.filter (*cloud);
|
||||||
|
// }
|
||||||
|
|
||||||
|
// ////////////////////////////////////////////////////////////////////////////
|
||||||
|
// //Radiusoutlier removal can be added here
|
||||||
|
// ////////////////////////////////////////////////////////////////////////////
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
// // // //Boolean variable to signify if any obstacles were detected
|
||||||
|
// // // std_msgs::Bool objectDetected;
|
||||||
|
// // // objectDetected.data=false;
|
||||||
|
|
||||||
|
|
||||||
|
// Creating the KdTree object for the search method of the extraction
|
||||||
|
pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT>);
|
||||||
|
tree->setInputCloud (cloud);
|
||||||
|
|
||||||
|
std::vector<pcl::PointIndices> cluster_indices;
|
||||||
|
pcl::EuclideanClusterExtraction<PointT> ec;
|
||||||
|
ec.setClusterTolerance (clusttol); // 2cm
|
||||||
|
ec.setMinClusterSize (minclust);
|
||||||
|
ec.setMaxClusterSize (maxclust);
|
||||||
|
ec.setSearchMethod (tree);
|
||||||
|
ec.setInputCloud (cloud);
|
||||||
|
ec.extract (cluster_indices);
|
||||||
|
ROS_INFO_STREAM(cluster_indices.size()<<" clusters extracted ");
|
||||||
|
|
||||||
|
Eigen::Vector4f centroidL;
|
||||||
|
Eigen::Vector4f minL;
|
||||||
|
Eigen::Vector4f maxL;
|
||||||
|
Eigen::Vector4f centroidR;
|
||||||
|
Eigen::Vector4f minR;
|
||||||
|
Eigen::Vector4f maxR;
|
||||||
|
bool first_Lcentroid_detected = false;
|
||||||
|
bool first_Rcentroid_detected = false;
|
||||||
|
Eigen::Vector4f centroidCENT;
|
||||||
|
Eigen::Vector4f minCENT;
|
||||||
|
Eigen::Vector4f maxCENT;
|
||||||
|
bool first_centroid_detected = false;
|
||||||
|
|
||||||
|
|
||||||
|
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
|
||||||
|
{
|
||||||
|
pcl::PointCloud<PointT>::Ptr cloud_clusterL (new pcl::PointCloud<PointT>);
|
||||||
|
for (const auto& idx : it->indices)
|
||||||
|
cloud_clusterL->push_back ((*cloud)[idx]); //*
|
||||||
|
cloud_clusterL->width = cloud_clusterL->size ();
|
||||||
|
cloud_clusterL->height = 1;
|
||||||
|
cloud_clusterL->is_dense = true;
|
||||||
|
Eigen::Vector4f centroid;
|
||||||
|
Eigen::Vector4f min;
|
||||||
|
Eigen::Vector4f max;
|
||||||
|
pcl::compute3DCentroid (*cloud_clusterL, centroid);
|
||||||
|
pcl::getMinMax3D (*cloud_clusterL, min, max);
|
||||||
|
|
||||||
|
|
||||||
|
if (first_centroid_detected)
|
||||||
|
{
|
||||||
|
if (abs(centroid[1]) < abs(centroidCENT[1]))
|
||||||
|
{
|
||||||
|
centroidCENT = centroid;
|
||||||
|
minCENT = min;
|
||||||
|
maxCENT = max;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
centroidCENT = centroid;
|
||||||
|
minCENT = min;
|
||||||
|
maxCENT = max;
|
||||||
|
first_centroid_detected = true;
|
||||||
|
|
||||||
|
}
|
||||||
|
}
|
||||||
|
// if (centroid[1] >= 0)
|
||||||
|
// {
|
||||||
|
// if (first_Lcentroid_detected)
|
||||||
|
// {
|
||||||
|
// if (centroid[1] < centroidL[1])
|
||||||
|
// {
|
||||||
|
// centroidL = centroid;
|
||||||
|
// minL = min;
|
||||||
|
// maxL = max;
|
||||||
|
// }
|
||||||
|
// }
|
||||||
|
// else
|
||||||
|
// {
|
||||||
|
// centroidL = centroid;
|
||||||
|
// minL = min;
|
||||||
|
// maxL = max;
|
||||||
|
// first_Lcentroid_detected = true;
|
||||||
|
// }
|
||||||
|
|
||||||
|
// }
|
||||||
|
// else if (centroid[1] <= 0)
|
||||||
|
// {
|
||||||
|
// if (first_Rcentroid_detected)
|
||||||
|
// {
|
||||||
|
// if (centroid[1] > centroidR[1])
|
||||||
|
// {
|
||||||
|
// centroidR = centroid;
|
||||||
|
// minR = min;
|
||||||
|
// maxR = max;
|
||||||
|
// }
|
||||||
|
// }
|
||||||
|
// else
|
||||||
|
// {
|
||||||
|
// centroidR = centroid;
|
||||||
|
// minR = min;
|
||||||
|
// maxR = max;
|
||||||
|
// first_Rcentroid_detected = true;
|
||||||
|
// }
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
uint32_t shape = visualization_msgs::Marker::CUBE;
|
||||||
|
// visualization_msgs::Marker markerL;
|
||||||
|
// markerL.header.stamp = ros::Time::now();
|
||||||
|
// markerL.type = shape;
|
||||||
|
// markerL.action = visualization_msgs::Marker::ADD;
|
||||||
|
|
||||||
|
// markerL.pose.position.x = 6;
|
||||||
|
// markerL.pose.position.y = centroidL[1];
|
||||||
|
// markerL.pose.position.z = 2.5;
|
||||||
|
// markerL.pose.orientation.x = 0.0;
|
||||||
|
// markerL.pose.orientation.y = -1.78;
|
||||||
|
// markerL.pose.orientation.z = 0.0;
|
||||||
|
// markerL.pose.orientation.w = 1.0;
|
||||||
|
|
||||||
|
// markerL.scale.x = (1.0);
|
||||||
|
// markerL.scale.y = (maxL[1]-minL[1]);
|
||||||
|
// markerL.scale.z = (10.0);
|
||||||
|
|
||||||
|
// if (markerL.scale.x ==0)
|
||||||
|
// markerL.scale.x=0.1;
|
||||||
|
|
||||||
|
// if (markerL.scale.y ==0)
|
||||||
|
// markerL.scale.y=0.1;
|
||||||
|
|
||||||
|
// if (markerL.scale.z ==0)
|
||||||
|
// markerL.scale.z=0.1;
|
||||||
|
|
||||||
|
// markerL.color.r = 0.0;
|
||||||
|
// markerL.color.g = 0.0;
|
||||||
|
// markerL.color.b = 1.0;
|
||||||
|
// markerL.color.a = 0.5;
|
||||||
|
|
||||||
|
// markerL.lifetime = ros::Duration();
|
||||||
|
// markerL.header.frame_id = "zed_camera_center";
|
||||||
|
// mark_l.publish (markerL);
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
// visualization_msgs::Marker markerR;
|
||||||
|
// markerR.header.stamp = ros::Time::now();
|
||||||
|
// markerR.type = shape;
|
||||||
|
// markerR.action = visualization_msgs::Marker::ADD;
|
||||||
|
|
||||||
|
// markerR.pose.position.x = 6;
|
||||||
|
// markerR.pose.position.y = centroidR[1];
|
||||||
|
// markerR.pose.position.z = 2.5;
|
||||||
|
// markerR.pose.orientation.x = 0.0;
|
||||||
|
// markerR.pose.orientation.y = -1.78;
|
||||||
|
// markerR.pose.orientation.z = 0.0;
|
||||||
|
// markerR.pose.orientation.w = 1.0;
|
||||||
|
|
||||||
|
// markerR.scale.x = (1.0);
|
||||||
|
// markerR.scale.y = (maxR[1]-minR[1]);
|
||||||
|
// markerR.scale.z = (10.0);
|
||||||
|
|
||||||
|
// if (markerR.scale.x ==0)
|
||||||
|
// markerR.scale.x=0.1;
|
||||||
|
|
||||||
|
// if (markerR.scale.y ==0)
|
||||||
|
// markerR.scale.y=0.1;
|
||||||
|
|
||||||
|
// if (markerR.scale.z ==0)
|
||||||
|
// markerR.scale.z=0.1;
|
||||||
|
|
||||||
|
// markerR.color.r = 1.0;
|
||||||
|
// markerR.color.g = 0.0;
|
||||||
|
// markerR.color.b = 0.0;
|
||||||
|
// markerR.color.a = 0.5;
|
||||||
|
|
||||||
|
// markerR.lifetime = ros::Duration();
|
||||||
|
// markerR.header.frame_id = "zed_camera_center";
|
||||||
|
// mark_r.publish (markerR);
|
||||||
|
|
||||||
|
|
||||||
|
//This is for two row following
|
||||||
|
// centroidCENT[0] = (centroidL[0] + centroidR[0])/2;
|
||||||
|
// centroidCENT[1] = (centroidL[1] + centroidR[1])/2;
|
||||||
|
// centroidCENT[2] = (centroidL[2] + centroidR[2])/2;
|
||||||
|
// maxCENT[0] = (maxL[0] + maxR[0])/2;
|
||||||
|
// maxCENT[1] = (maxL[1] + maxR[1])/2;
|
||||||
|
// maxCENT[2] = (maxL[2] + maxR[2])/2;
|
||||||
|
// minCENT[0] = (minL[0] + minR[0])/2;
|
||||||
|
// minCENT[1] = (minL[1] + minR[1])/2;
|
||||||
|
// minCENT[2] = (minL[2] + minR[2])/2;
|
||||||
|
|
||||||
|
//Averaging step
|
||||||
|
|
||||||
|
// centroidCENT[1] = (prevweight * prevcentroid) + ((1 - prevweight) * centroidCENT[1]);
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
visualization_msgs::Marker markerCENT;
|
||||||
|
markerCENT.header.stamp = ros::Time::now();
|
||||||
|
markerCENT.type = shape;
|
||||||
|
markerCENT.action = visualization_msgs::Marker::ADD;
|
||||||
|
|
||||||
|
markerCENT.pose.position.x = centroidCENT[0];
|
||||||
|
markerCENT.pose.position.y = centroidCENT[1];
|
||||||
|
markerCENT.pose.position.z = centroidCENT[2];
|
||||||
|
markerCENT.pose.orientation.x = 0.0;
|
||||||
|
markerCENT.pose.orientation.y = 0.0;
|
||||||
|
markerCENT.pose.orientation.z = 0.0;
|
||||||
|
markerCENT.pose.orientation.w = 1.0;
|
||||||
|
|
||||||
|
markerCENT.scale.x = (maxCENT[0] - minCENT[0]);
|
||||||
|
markerCENT.scale.y = (maxCENT[1] - minCENT[1]);
|
||||||
|
markerCENT.scale.z = (maxCENT[2] - minCENT[2]);
|
||||||
|
|
||||||
|
if (markerCENT.scale.x ==0)
|
||||||
|
markerCENT.scale.x=0.1;
|
||||||
|
|
||||||
|
if (markerCENT.scale.y ==0)
|
||||||
|
markerCENT.scale.y=0.1;
|
||||||
|
|
||||||
|
if (markerCENT.scale.z ==0)
|
||||||
|
markerCENT.scale.z=0.1;
|
||||||
|
|
||||||
|
markerCENT.color.r = 0.0;
|
||||||
|
markerCENT.color.g = 0.0;
|
||||||
|
markerCENT.color.b = 1.0;
|
||||||
|
markerCENT.color.a = 0.5;
|
||||||
|
|
||||||
|
markerCENT.lifetime = ros::Duration();
|
||||||
|
markerCENT.header.frame_id = "zed_camera_center";
|
||||||
|
steer.publish (markerCENT);
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
// uint32_t shape = visualization_msgs::Marker::CUBE;
|
||||||
|
// visualization_msgs::Marker markerL;
|
||||||
|
// markerL.header.stamp = ros::Time::now();
|
||||||
|
// markerL.type = shape;
|
||||||
|
// markerL.action = visualization_msgs::Marker::ADD;
|
||||||
|
|
||||||
|
// markerL.pose.position.x = centroidL[0];
|
||||||
|
// markerL.pose.position.y = centroidL[1];
|
||||||
|
// markerL.pose.position.z = centroidL[2];
|
||||||
|
// markerL.pose.orientation.x = 0.0;
|
||||||
|
// markerL.pose.orientation.y = 0.0;
|
||||||
|
// markerL.pose.orientation.z = 0.0;
|
||||||
|
// markerL.pose.orientation.w = 1.0;
|
||||||
|
|
||||||
|
// markerL.scale.x = (maxL[0]-minL[0]);
|
||||||
|
// markerL.scale.y = (maxL[1]-minL[1]);
|
||||||
|
// markerL.scale.z = (maxL[2]-minL[2]);
|
||||||
|
|
||||||
|
// if (markerL.scale.x ==0)
|
||||||
|
// markerL.scale.x=0.1;
|
||||||
|
|
||||||
|
// if (markerL.scale.y ==0)
|
||||||
|
// markerL.scale.y=0.1;
|
||||||
|
|
||||||
|
// if (markerL.scale.z ==0)
|
||||||
|
// markerL.scale.z=0.1;
|
||||||
|
|
||||||
|
// markerL.color.r = 0.0;
|
||||||
|
// markerL.color.g = 0.0;
|
||||||
|
// markerL.color.b = 1.0;
|
||||||
|
// markerL.color.a = 0.5;
|
||||||
|
|
||||||
|
// markerL.lifetime = ros::Duration();
|
||||||
|
// markerL.header.frame_id = "zed_camera_center";
|
||||||
|
// mark_l.publish (markerL);
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
// visualization_msgs::Marker markerR;
|
||||||
|
// markerR.header.stamp = ros::Time::now();
|
||||||
|
// markerR.type = shape;
|
||||||
|
// markerR.action = visualization_msgs::Marker::ADD;
|
||||||
|
|
||||||
|
// markerR.pose.position.x = centroidR[0];
|
||||||
|
// markerR.pose.position.y = centroidR[1];
|
||||||
|
// markerR.pose.position.z = centroidR[2];
|
||||||
|
// markerR.pose.orientation.x = 0.0;
|
||||||
|
// markerR.pose.orientation.y = 0.0;
|
||||||
|
// markerR.pose.orientation.z = 0.0;
|
||||||
|
// markerR.pose.orientation.w = 1.0;
|
||||||
|
|
||||||
|
// markerR.scale.x = (maxR[0]-minR[0]);
|
||||||
|
// markerR.scale.y = (maxR[1]-minR[1]);
|
||||||
|
// markerR.scale.z = (maxR[2]-minR[2]);
|
||||||
|
|
||||||
|
// if (markerR.scale.x ==0)
|
||||||
|
// markerR.scale.x=0.1;
|
||||||
|
|
||||||
|
// if (markerR.scale.y ==0)
|
||||||
|
// markerR.scale.y=0.1;
|
||||||
|
|
||||||
|
// if (markerR.scale.z ==0)
|
||||||
|
// markerR.scale.z=0.1;
|
||||||
|
|
||||||
|
// markerR.color.r = 1.0;
|
||||||
|
// markerR.color.g = 0.0;
|
||||||
|
// markerR.color.b = 0.0;
|
||||||
|
// markerR.color.a = 0.5;
|
||||||
|
|
||||||
|
// markerR.lifetime = ros::Duration();
|
||||||
|
// markerR.header.frame_id = "zed_camera_center";
|
||||||
|
// mark_r.publish (markerR);
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
// float center = (centroidL[1] + centroidR[1])/2;
|
||||||
|
// visualization_msgs::Marker steercube;
|
||||||
|
// steercube.header.stamp = ros::Time::now();
|
||||||
|
// steercube.type = shape;
|
||||||
|
// steercube.action = visualization_msgs::Marker::ADD;
|
||||||
|
|
||||||
|
// steercube.pose.position.x = 1.0;
|
||||||
|
// steercube.pose.position.y = center / 2;
|
||||||
|
// steercube.pose.position.z = 1.0;
|
||||||
|
// steercube.pose.orientation.x = 0.0;
|
||||||
|
// steercube.pose.orientation.y = -0.78;
|
||||||
|
// steercube.pose.orientation.z = 0.0;
|
||||||
|
// steercube.pose.orientation.w = 1.0;
|
||||||
|
// float yvalue = (center > 0? center:center * -1);
|
||||||
|
// steercube.scale.x = (0.1);
|
||||||
|
// steercube.scale.y = yvalue;
|
||||||
|
// steercube.scale.z = (0.1);
|
||||||
|
|
||||||
|
// if (steercube.scale.x ==0)
|
||||||
|
// steercube.scale.x=0.1;
|
||||||
|
|
||||||
|
// if (steercube.scale.y ==0)
|
||||||
|
// steercube.scale.y=0.1;
|
||||||
|
|
||||||
|
// if (steercube.scale.z ==0)
|
||||||
|
// steercube.scale.z=0.1;
|
||||||
|
|
||||||
|
// steercube.color.r = 0.0;
|
||||||
|
// steercube.color.g = 1.0;
|
||||||
|
// steercube.color.b = 0.0;
|
||||||
|
// steercube.color.a = 0.5;
|
||||||
|
|
||||||
|
// steercube.lifetime = ros::Duration();
|
||||||
|
// steercube.header.frame_id = "zed_camera_center";
|
||||||
|
// steer.publish (steercube);
|
||||||
|
std_msgs::Float32 steeringval;
|
||||||
|
steeringval.data = centroidCENT[1];
|
||||||
|
cont_val.publish(steeringval);
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
// Convert to ROS data type
|
||||||
|
sensor_msgs::PointCloud2 output;
|
||||||
|
sensor_msgs::PointCloud2 otheroutput;
|
||||||
|
pcl::toROSMsg(*cloud, output);
|
||||||
|
pcl::toROSMsg(*uncloud, otheroutput);
|
||||||
|
output.header.frame_id = "zed_camera_center";
|
||||||
|
otheroutput.header.frame_id = "zed_camera_center";
|
||||||
|
|
||||||
|
// Publish the data
|
||||||
|
pub.publish (output);
|
||||||
|
pub2.publish (otheroutput);
|
||||||
|
|
||||||
|
}
|
||||||
|
int
|
||||||
|
main (int argc, char** argv)
|
||||||
|
{
|
||||||
|
// Initialize ROS
|
||||||
|
ros::init (argc, argv, "preprocessing");
|
||||||
|
ros::NodeHandle nh;
|
||||||
|
dynamic_reconfigure::Server<pcl_proc::configConfig> server;
|
||||||
|
dynamic_reconfigure::Server<pcl_proc::configConfig>::CallbackType f;
|
||||||
|
|
||||||
|
f = boost::bind(&callback, _1, _2);
|
||||||
|
server.setCallback(f);
|
||||||
|
|
||||||
|
// Create a ROS subscriber for the input point cloud
|
||||||
|
ros::Subscriber sub = nh.subscribe<sensor_msgs::PointCloud2> ("/zed/zed_node/point_cloud/cloud_registered", 1, cloud_cb);
|
||||||
|
|
||||||
|
// Create a ROS publisher for the output point cloud
|
||||||
|
pub = nh.advertise<sensor_msgs::PointCloud2> ("processed_cloud", 1);
|
||||||
|
pub2 = nh.advertise<sensor_msgs::PointCloud2> ("unprocessed_cloud", 1);
|
||||||
|
|
||||||
|
// //ROS Publisher tht publishes TRUE if obstacle detected, else FALSE
|
||||||
|
// object= nh.advertise<std_msgs::Bool> ("object", 1);//To be changed
|
||||||
|
|
||||||
|
//ROS Publisher tht publishes the visualization box thingy
|
||||||
|
mark_l= nh.advertise<visualization_msgs::Marker> ("marker_L", 1);//To be changed
|
||||||
|
//ROS Publisher tht publishes the visualization box thingy
|
||||||
|
mark_r= nh.advertise<visualization_msgs::Marker> ("marker_R", 1);//To be changed
|
||||||
|
|
||||||
|
steer= nh.advertise<visualization_msgs::Marker> ("marker_CENT", 1);//To be changed
|
||||||
|
|
||||||
|
cont_val = nh.advertise<std_msgs::Float32> ("steering_value", 1);
|
||||||
|
|
||||||
|
// Spin
|
||||||
|
ros::spin ();
|
||||||
|
}
|
||||||
Loading…
x
Reference in New Issue
Block a user