| 20 | } |
| 21 | |
| 22 | void MapROS::init() { |
| 23 | node_.param("map_ros/fx", fx_, -1.0); |
| 24 | node_.param("map_ros/fy", fy_, -1.0); |
| 25 | node_.param("map_ros/cx", cx_, -1.0); |
| 26 | node_.param("map_ros/cy", cy_, -1.0); |
| 27 | node_.param("map_ros/depth_filter_maxdist", depth_filter_maxdist_, -1.0); |
| 28 | node_.param("map_ros/depth_filter_mindist", depth_filter_mindist_, -1.0); |
| 29 | node_.param("map_ros/depth_filter_margin", depth_filter_margin_, -1); |
| 30 | node_.param("map_ros/k_depth_scaling_factor", k_depth_scaling_factor_, -1.0); |
| 31 | node_.param("map_ros/skip_pixel", skip_pixel_, -1); |
| 32 | |
| 33 | node_.param("map_ros/esdf_slice_height", esdf_slice_height_, -0.1); |
| 34 | node_.param("map_ros/visualization_truncate_height", visualization_truncate_height_, -0.1); |
| 35 | node_.param("map_ros/visualization_truncate_low", visualization_truncate_low_, -0.1); |
| 36 | node_.param("map_ros/show_occ_time", show_occ_time_, false); |
| 37 | node_.param("map_ros/show_esdf_time", show_esdf_time_, false); |
| 38 | node_.param("map_ros/show_all_map", show_all_map_, false); |
| 39 | node_.param("map_ros/frame_id", frame_id_, string("world")); |
| 40 | |
| 41 | proj_points_.resize(640 * 480 / (skip_pixel_ * skip_pixel_)); |
| 42 | point_cloud_.points.resize(640 * 480 / (skip_pixel_ * skip_pixel_)); |
| 43 | // proj_points_.reserve(640 * 480 / map_->mp_->skip_pixel_ / map_->mp_->skip_pixel_); |
| 44 | proj_points_cnt = 0; |
| 45 | |
| 46 | local_updated_ = false; |
| 47 | esdf_need_update_ = false; |
| 48 | fuse_time_ = 0.0; |
| 49 | esdf_time_ = 0.0; |
| 50 | max_fuse_time_ = 0.0; |
| 51 | max_esdf_time_ = 0.0; |
| 52 | fuse_num_ = 0; |
| 53 | esdf_num_ = 0; |
| 54 | depth_image_.reset(new cv::Mat); |
| 55 | |
| 56 | rand_noise_ = normal_distribution<double>(0, 0.1); |
| 57 | random_device rd; |
| 58 | eng_ = default_random_engine(rd()); |
| 59 | |
| 60 | esdf_timer_ = node_.createTimer(ros::Duration(0.05), &MapROS::updateESDFCallback, this); |
| 61 | vis_timer_ = node_.createTimer(ros::Duration(0.2), &MapROS::visCallback, this); |
| 62 | |
| 63 | map_all_pub_ = node_.advertise<sensor_msgs::PointCloud2>("/sdf_map/occupancy_all", 10); |
| 64 | map_local_pub_ = node_.advertise<sensor_msgs::PointCloud2>("/sdf_map/occupancy_local", 10); |
| 65 | map_local_inflate_pub_ = |
| 66 | node_.advertise<sensor_msgs::PointCloud2>("/sdf_map/occupancy_local_inflate", 10); |
| 67 | unknown_pub_ = node_.advertise<sensor_msgs::PointCloud2>("/sdf_map/unknown", 10); |
| 68 | esdf_pub_ = node_.advertise<sensor_msgs::PointCloud2>("/sdf_map/esdf", 10); |
| 69 | update_range_pub_ = node_.advertise<visualization_msgs::Marker>("/sdf_map/update_range", 10); |
| 70 | depth_pub_ = node_.advertise<sensor_msgs::PointCloud2>("/sdf_map/depth_cloud", 10); |
| 71 | |
| 72 | // basecoor_sub_ = node_.subscribe("/sdf_map/basecoor", 10, &MapROS::basecoorCallback, this); |
| 73 | |
| 74 | depth_sub_.reset( |
| 75 | new message_filters::Subscriber<sensor_msgs::Image>(node_, "/map_ros/depth", 50)); |
| 76 | cloud_sub_.reset( |
| 77 | new message_filters::Subscriber<sensor_msgs::PointCloud2>(node_, "/map_ros/cloud", 50)); |
| 78 | pose_sub_.reset( |
| 79 | new message_filters::Subscriber<geometry_msgs::PoseStamped>(node_, "/map_ros/pose", 25)); |