00001 #include "ros/ros.h" 00002 #include <ndt_map/ndt_conversions.h> 00003 #include <ndt_map/ndt_map.h> 00004 #include <ndt_map/ndt_cell.h> 00005 #include <ndt_map/lazy_grid.h> 00006 #include <ndt_map/pointcloud_utils.h> 00007 #include <nav_msgs/OccupancyGrid.h> 00008 #include <ndt_map/NDTMapMsg.h> 00009 00010 #include "pcl/point_cloud.h" 00011 #include "pcl/io/pcd_io.h" 00012 #include "pcl/features/feature.h" 00013 #include <cstdio> 00014 #include <cstring> 00015 #include <string> 00016 00017 int main(int argc, char** argv){ 00018 ros::init(argc,argv,"occ_map_topic"); 00019 ros::NodeHandle nh; 00020 ros::Publisher map_pub = nh.advertise<nav_msgs::OccupancyGrid>("dummy_occ_map_pub", 1000); 00021 ros::Rate loop_rate(1); 00022 nav_msgs::OccupancyGrid msg; 00023 00024 lslgeneric::NDTMap nd(new lslgeneric::LazyGrid(0.4)); 00025 ROS_INFO("loading from jff...\n"); 00026 if (nd.loadFromJFF("basement2d_map.jff") < 0) 00027 ROS_INFO("loading from jff failed\n"); 00028 00029 lslgeneric::toOccupancyGrid(&nd,msg,0.05,"/base"); 00030 while (ros::ok()){ 00031 map_pub.publish(msg); 00032 ros::spinOnce(); 00033 loop_rate.sleep(); 00034 } 00035 return 0; 00036 }