From 5d509e4e855c382d5c2f323bf8c93a528fb3743b Mon Sep 17 00:00:00 2001 From: caroline Date: Fri, 22 Apr 2022 15:51:53 +0200 Subject: [PATCH] initial commit 2 --- CMakeLists.txt | 11 +- cfg/config.cfg | 19 + src/initial_processing.cpp | 62 ++- src/markose_preprocessing.cpp | 693 ++++++++++++++++++++++++++++++++++ 4 files changed, 782 insertions(+), 3 deletions(-) create mode 100755 cfg/config.cfg create mode 100755 src/markose_preprocessing.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index bdaf818..1574ff8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -14,6 +14,14 @@ find_package(catkin REQUIRED COMPONENTS genmsg pcl_conversions 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 @@ -208,5 +216,6 @@ add_executable(initial_processing src/initial_processing.cpp) target_link_libraries(initial_processing ${catkin_LIBRARIES}) 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) diff --git a/cfg/config.cfg b/cfg/config.cfg new file mode 100755 index 0000000..39c120c --- /dev/null +++ b/cfg/config.cfg @@ -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")) diff --git a/src/initial_processing.cpp b/src/initial_processing.cpp index 1430044..9dec29e 100755 --- a/src/initial_processing.cpp +++ b/src/initial_processing.cpp @@ -2,16 +2,22 @@ #include "std_msgs/String.h" #include "std_msgs/Bool.h" #include "std_msgs/Float32.h" +#include +#include #include +#include #include #include #include #include +#include +#include #include #include +#include /* TBD: get to know typedef such as stringstream @@ -19,38 +25,90 @@ TBD: get to know typedef such as stringstream // define publisher ros::Publisher pub; +ros::Publisher pub_2; 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; +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){ - + // create pointer to Point Cloud in pcl pcl::PointCloud::Ptr cloud(new pcl::PointCloud); + pcl::PointCloud::Ptr cloud_filtered (new pcl::PointCloud); // convert ROS message data to point cloud in pcl pcl::fromROSMsg(*cloud_msg, *cloud); + + + // Create filtering object + pcl::PassThrough passx; + passx.setInputCloud (cloud); + passx.setFilterFieldName ("x"); + passx.setFilterLimits (0.0, 5.0); + passx.filter (*cloud); + + pcl::PassThrough passy; + passy.setInputCloud (cloud); + passy.setFilterFieldName ("y"); + passy.setFilterLimits (-5.0, 5.0); + passy.filter (*cloud); + + pcl::PassThrough passz; + passz.setInputCloud (cloud); + passz.setFilterFieldName ("z"); + passz.setFilterLimits (0.0, 5.0); + passz.filter (*cloud); + + +/* pcl::VoxelGrid 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 sensor_msgs::PointCloud2 output; pcl::toROSMsg(*cloud, output); pub.publish(output); + } int main(int argc, char **argv){ - // specify node name and initialize ROS ros::init(argc, argv, "image_processing"); // first/last node handle does the initialization of node/cleanup of used resources by node ros::NodeHandle n; + dynamic_reconfigure::Server server; + dynamic_reconfigure::Server::CallbackType f; + + f = boost::bind(&callback, _1, _2); + server.setCallback(f); + // create subscriber to receive point cloud ros::Subscriber sub = n.subscribe("/zed/zed_node/point_cloud/cloud_registered", 1, cloud_cb); // create publisher for output processed point cloud pub = n.advertise("processed_cloud", 1); + pub_2 = n.advertise("unprocessed_cloud", 1); // create publisher for visualising marker visual = n.advertise ("marker", 1); diff --git a/src/markose_preprocessing.cpp b/src/markose_preprocessing.cpp new file mode 100755 index 0000000..e91df7d --- /dev/null +++ b/src/markose_preprocessing.cpp @@ -0,0 +1,693 @@ +#include +// PCL specific includes +#include "std_msgs/Bool.h" +#include "std_msgs/Float32.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include + + + + #include + + + + + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +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::Ptr cloud(new pcl::PointCloud); + pcl::PointCloud::Ptr uncloud(new pcl::PointCloud); + + + // Convert to PCL Data type irrespective of type + pcl::fromROSMsg(*cloud_msg, *cloud); + pcl::fromROSMsg(*cloud_msg, *uncloud); + pcl::PointCloud::Ptr hsv_cloud(new pcl::PointCloud); + + // 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 passx; + passx.setInputCloud (cloud); + passx.setFilterFieldName ("x"); + passx.setFilterLimits (xbneg, xbpos); + passx.filter (*cloud); + } + + if (ypass_enable) + { + pcl::PassThrough passy; + passy.setInputCloud (cloud); + passy.setFilterFieldName ("y"); + passy.setFilterLimits (ybneg, ybpos); + passy.filter (*cloud); + } + + if (ypass_enable) + { + pcl::PassThrough passz; + passz.setInputCloud (cloud); + passz.setFilterFieldName ("z"); + passz.setFilterLimits (zbneg, zbpos); + passz.filter (*cloud); + } + + + if (voxel_enable) + { + pcl::VoxelGrid 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 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< 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 filter(true); + + // filter.setInputCloud(hsv_cloud); + // filter.setIndices(inliers2); + // filter.filter(*hsv_cloud); + // pcl::PointXYZHSVtoXYZRGB(*hsv_cloud, *cloud); + + + + if (color_filtering_enable) + { + pcl::ConditionalRemoval color_filter; + pcl::PackedRGBComparison::Ptr + green_condition(new pcl::PackedRGBComparison("g", pcl::ComparisonOps::GT, greenthresh)); + pcl::ConditionAnd::Ptr color_cond (new pcl::ConditionAnd ()); + 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 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::Ptr tree (new pcl::search::KdTree); + tree->setInputCloud (cloud); + + std::vector cluster_indices; + pcl::EuclideanClusterExtraction 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::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it) + { + pcl::PointCloud::Ptr cloud_clusterL (new pcl::PointCloud); + 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 server; + dynamic_reconfigure::Server::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 ("/zed/zed_node/point_cloud/cloud_registered", 1, cloud_cb); + + // Create a ROS publisher for the output point cloud + pub = nh.advertise ("processed_cloud", 1); + pub2 = nh.advertise ("unprocessed_cloud", 1); + + // //ROS Publisher tht publishes TRUE if obstacle detected, else FALSE + // object= nh.advertise ("object", 1);//To be changed + + //ROS Publisher tht publishes the visualization box thingy + mark_l= nh.advertise ("marker_L", 1);//To be changed + //ROS Publisher tht publishes the visualization box thingy + mark_r= nh.advertise ("marker_R", 1);//To be changed + + steer= nh.advertise ("marker_CENT", 1);//To be changed + + cont_val = nh.advertise ("steering_value", 1); + + // Spin + ros::spin (); +}