-
Notifications
You must be signed in to change notification settings - Fork 1
/
main.cpp
124 lines (90 loc) · 2.81 KB
/
main.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
#include <iostream>
#include <string.h>
#include <fstream>
#include <algorithm>
#include <iterator>
#include "kf_tracker/featureDetection.h"
#include "kf_tracker/CKalmanFilter.h"
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/video/video.hpp>
#include "opencv2/video/tracking.hpp"
#include <ros/ros.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include "pcl_ros/point_cloud.h"
#include <geometry_msgs/Point.h>
#include <std_msgs/Float32MultiArray.h>
#include <std_msgs/Int32MultiArray.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/common/geometry.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/common/centroid.h>
#include <visualization_msgs/MarkerArray.h>
#include <visualization_msgs/Marker.h>
#include <limits>
#include <utility>
#include <pcl/registration/correspondence_estimation.h>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
#include <nav_msgs/OccupancyGrid.h>
#include <nav_msgs/GridCells.h>
#include <cv_bridge/cv_bridge.h>
#include <grid_map_ros/GridMapRosConverter.hpp>
#include <pcl/features/boundary.h>
#include <pcl/features/normal_3d.h>
#include <grid_map_ros/grid_map_ros.hpp>
#include <grid_map_msgs/GridMap.h>
#include <pcl/PCLPointCloud2.h>
#include <pcl/io/pcd_io.h>
#include <pcl/features/boundary.h>
#include <pcl/console/print.h>
#include <pcl/console/parse.h>
#include <pcl/console/time.h>
using namespace pcl;
using namespace pcl::io;
using namespace pcl::console;
using namespace std;
using namespace cv;
using namespace grid_map;
//ros::Publisher pub;
ros::Publisher pub;
void occupancyGridToGridMap(const nav_msgs::OccupancyGrid& input)
{
sensor_msgs::PointCloud2 pcloud ;
grid_map::GridMap gridMap;
std::string layer = "1";
std::vector<std::string> layer1;
layer1.push_back("1");
if( grid_map::GridMapRosConverter::fromOccupancyGrid(input ,layer , gridMap ))
{
};
sensor_msgs::PointCloud2 cloud_filtered;
grid_map::GridMapRosConverter::toPointCloud( gridMap, layer1, layer, pcloud);
while ( 1)
{
pub.publish(pcloud);
};
}
int main(int argc, char** argv)
{
// ROS init
ros::init (argc,argv,"PCLOUTPUT");
ros::NodeHandle nh;
ros::Subscriber sub1 = nh.subscribe ("map",1,occupancyGridToGridMap);
pub = nh.advertise<sensor_msgs::PointCloud2> ("pcloud", 1);
ros::spin();
}