29 #include <pcl_conversions/pcl_conversions.h>
52 pcl::PointCloud<pcl::PointXYZ>::Ptr
pc_filter (
new pcl::PointCloud<pcl::PointXYZ>);
64 visualization_msgs::MarkerArray grid_markers_elavationmap, accessibility_markers;
65 visualization_msgs::Marker marker_normals;
66 visualization_msgs::Marker marker_polygon;
67 visualization_msgs::Marker marker_obstacle;
70 pcl::PointCloud<pcl::PointXYZ> pc_in;
71 pcl::fromROSMsg(pcmsg_in,pc_in);
72 pcl::PCLPointCloud2 pcl_pc;
73 pcl_conversions::toPCL(pcmsg_in, pcl_pc);
74 pcl::fromPCLPointCloud2(pcl_pc, pc_in);
77 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in (
new pcl::PointCloud<pcl::PointXYZ>);
84 std::cout<<
"Normal Estimation"<<std::endl;
144 int main(
int argc,
char **argv)
146 ros::init(argc, argv,
"navigability_map");
147 ros::NodeHandle n(
"~");
149 tf::TransformListener listener(n,ros::Duration(1000));
159 n.param(
"output_frequency", output_freq, 200.0);
160 n.param(
"PointCLoud_Xmin_filter",g_NavMap->
Xmin_filter,0.0);
161 n.param(
"PointCLoud_Xmax_filter",g_NavMap->
Xmax_filter,22.5);
162 n.param(
"PointCLoud_Ymax_filter",g_NavMap->
Ymax_filter,22.5);
163 n.param(
"PointCLoud_Ymin_filter",g_NavMap->
Ymin_filter,-22.5);
164 n.param(
"PointCLoud_Zmin_filter",g_NavMap->
Zmin_filter,-15.0);
165 n.param(
"PointCLoud_Zmax_filter",g_NavMap->
Zmax_filter,2.0);
166 n.param(
"Grid_Sx",g_NavMap->
Sx,0.2);
167 n.param(
"Grid_Sy",g_NavMap->
Sy,0.2);
184 ros::Subscriber sub_phiversion = n.subscribe (
"/pointcloud0", 1,
pointcloud_cb);
188 normals_pub = n.advertise<visualization_msgs::Marker>(
"/normals", 1);
190 ros::Publisher pub = n.advertise<sensor_msgs::PointCloud2>(
"/pc_filter_navmap", 1);
192 elevation_map_pub = n.advertise<visualization_msgs::MarkerArray>(
"/elevation_map", 1);
196 polygon_pub = n.advertise<visualization_msgs::Marker>(
"/polygon", 1);
197 obstacle_pub = n.advertise<visualization_msgs::Marker>(
"/obstacle", 1);
199 ros::Rate loop_rate(output_freq);
207 sensor_msgs::PointCloud2 pcmsg_out;
209 pcmsg_out.header.stamp = ros::Time::now();
211 pub.publish(pcmsg_out);
int debug_accessibility
Which accessibility map should be draw.
int Use_Radius_Search
1 - to use the radius search ; 0 - to use the number of neighbours search
visualization_msgs::Marker create_Markers_Normals(Navigability_Map &nav_map, std::string grid_frame)
Function to generate marker for the visualization of the Normals on the Class Navigability_Map.
double Ymin_filter
Min distance to be consider in the Y direction of the PointCloud.
double Sx
The X dimension of the cell.
Navigability_Map * g_NavMap
ros::Publisher normals_pub
pcl::PointCloud< pcl::PointXYZ >::Ptr pc_filter(new pcl::PointCloud< pcl::PointXYZ >)
double Standard_Deviation_anglez_max_confidence
Normalization constant for the Angle Z confidence value.
void inicialize_grid(void)
Function that inicialize the Grid with default data.
int main(int argc, char **argv)
double angleX_max_difference
Threshold for the Angle X difference between cells.
void Filter_PointCloud(pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud_in, pcl::PointCloud< pcl::PointXYZ >::Ptr &pc_filter)
Function that filters the PointCloud. Filters the point cloud receiveid, the filtered PointCloud is s...
double Xmax_filter
Max distance to be consider in the X direction of the PointCloud.
void calcGrid_data(void)
Function that calculates mean values and confidence of the x,y,z points and normals of the PointCloud...
ros::Publisher accessibility_map_pub
double angleZ_max_difference
Threshold for the Angle Z difference between cells.
tf::TransformListener * p_listener
double Standard_Deviation_max
Normalization constant for the Z confidence value.
double Zmax_heigh_difference
Threshold for the Z difference between cells.
double Sy
The Y dimension of the cell.
ros::Publisher polygon_pub
double Radius_neighbors
The radius of the neighbours search to estimate the normals in the PointCloud.
double Standard_Deviation_anglex_max_confidence
Normalization constant for the Angle X confidence value.
void fill_data_cells(void)
Function that interpolates data to empty cells.
double default_confidence
Default confidence for a cell with one point.
void pointcloud_cb(sensor_msgs::PointCloud2 pcmsg_in)
Callback from the Pointcloud subscribed topic This calback filter the pointcloud uses de Navigability...
vector< visualization_msgs::Marker > create_Markers_AccessibilityMap(Navigability_Map &nav_map, std::string grid_frame)
Function to generate marker for the visualization of the Accessibility Map on the Class Navigability_...
double Zmin_filter
Max distance to be consider in the Z direction of the PointCloud.
Header file with class declaration to creat a two-dimensional grid. The two-dimensional grid has cell...
double Standard_Deviation_angley_max_confidence
Normalization constant for the Angle Y confidence value.
void setGrid_data(pcl::PointCloud< pcl::PointXYZ > &cloud)
Function that sets the data to the grid based on the PointCloud information. Saves the x...
int K_neighbors
The number of neighbours search to estimate the normals in the PointCloud.
double Ymax_filter
Max distance to be consider in the Y direction of the PointCloud.
double Zmax_filter
Min distance to be consider in the Z direction of the PointCloud.
void set_Cells_accessibility(void)
Function that sets the accessibility of the cells.
ros::Publisher obstacle_pub
void setGrid_parameter(double max_Xval, double max_Yval)
Function that sets the parameters to generate the grid. Grid Parameter: Total of row, cols and the center col.
double angleY_max_difference
Threshold for the Angle Y difference between cells.
ros::Publisher elevation_map_pub
double Xmin_filter
Min distance to be consider in the X direction of the PointCloud.
void Normal_Estimation(pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud)
Computes the normal estimation for a PointCloud.
vector< visualization_msgs::Marker > create_Markers_ElavationMap(Navigability_Map &nav_map, std::string grid_frame)
Function to generate marker for the visualization of the Elevation Map on the Class Navigability_Map...
Declarion functions and class to handle the visualization markers of the Class Navigability_Map.
double fator_confidence_neighbour_limit
Limit of the max difference between a cell and a neighbour cell.