@@ -278,9 +278,10 @@ void ScanMatcherComponent::receiveCloud(
278278{
279279 if (mapping_flag_ && mapping_future_.valid ()) {
280280 auto status = mapping_future_.wait_for (0s);
281- if (status == std::future_status::ready) {
281+ if (status == std::future_status::ready) {
282282 if (is_map_updated_ == true ) {
283- pcl::PointCloud<pcl::PointXYZI>::Ptr
targeted_cloud_ptr (
new pcl::PointCloud<pcl::PointXYZI>(targeted_cloud_));
283+ pcl::PointCloud<pcl::PointXYZI>::Ptr
targeted_cloud_ptr (
new pcl::PointCloud<pcl::PointXYZI>(
284+ targeted_cloud_));
284285 registration_->setInputTarget (targeted_cloud_ptr);
285286 is_map_updated_ = false ;
286287 }
@@ -402,7 +403,9 @@ void ScanMatcherComponent::publishMapAndPose(
402403 geometry_msgs::msg::PoseStamped corrent_pose_stamped;
403404 corrent_pose_stamped = corrent_pose_stamped_;
404405 previous_position_ = position;
405- mapping_task_ = std::packaged_task<void ()>(std::bind (&ScanMatcherComponent::updateMap, this , cloud_ptr, final_transformation, corrent_pose_stamped));
406+ mapping_task_ =
407+ std::packaged_task<void ()>(std::bind (&ScanMatcherComponent::updateMap, this , cloud_ptr,
408+ final_transformation, corrent_pose_stamped));
406409 mapping_future_ = mapping_task_.get_future ();
407410 mapping_thread_ = std::thread (std::move (std::ref (mapping_task_)));
408411 mapping_flag_ = true ;
@@ -413,49 +416,51 @@ void ScanMatcherComponent::updateMap(
413416 const pcl::PointCloud<pcl::PointXYZI>::ConstPtr cloud_ptr,
414417 Eigen::Matrix4f final_transformation, geometry_msgs::msg::PoseStamped corrent_pose_stamped)
415418{
419+ pcl::PointCloud<pcl::PointXYZI>::Ptr
tmp_ptr (
new pcl::PointCloud<pcl::PointXYZI>());
420+ pcl::VoxelGrid<pcl::PointXYZI> voxel_grid;
421+ voxel_grid.setLeafSize (vg_size_for_map_, vg_size_for_map_, vg_size_for_map_);
422+ voxel_grid.setInputCloud (cloud_ptr);
423+ voxel_grid.filter (*tmp_ptr);
424+
425+ pcl::PointCloud<pcl::PointXYZI>::Ptr
transformed_cloud_ptr (
new pcl::PointCloud<pcl::PointXYZI>());
426+ pcl::transformPointCloud (*tmp_ptr, *transformed_cloud_ptr, final_transformation);
427+
428+ map_ += *transformed_cloud_ptr;
429+
430+ targeted_cloud_.clear ();
431+ targeted_cloud_ += *transformed_cloud_ptr;
432+ int num_submaps = map_array_msg_.submaps .size ();
433+ for (int i = 0 ; i < num_targeted_cloud_ - 1 ; i++) {
434+ if (num_submaps - 1 - i < 0 ) {continue ;}
416435 pcl::PointCloud<pcl::PointXYZI>::Ptr
tmp_ptr (
new pcl::PointCloud<pcl::PointXYZI>());
417- pcl::VoxelGrid<pcl::PointXYZI> voxel_grid;
418- voxel_grid.setLeafSize (vg_size_for_map_, vg_size_for_map_, vg_size_for_map_);
419- voxel_grid.setInputCloud (cloud_ptr);
420- voxel_grid.filter (*tmp_ptr);
421-
422- pcl::PointCloud<pcl::PointXYZI>::Ptr
transformed_cloud_ptr (
new pcl::PointCloud<pcl::PointXYZI>());
423- pcl::transformPointCloud (*tmp_ptr, *transformed_cloud_ptr, final_transformation);
424-
425- map_ += *transformed_cloud_ptr;
426-
427- targeted_cloud_.clear ();
428- targeted_cloud_ += *transformed_cloud_ptr;
429- int num_submaps = map_array_msg_.submaps .size ();
430- for (int i = 0 ; i < num_targeted_cloud_ - 1 ; i++) {
431- if (num_submaps - 1 - i < 0 ) {continue ;}
432- pcl::PointCloud<pcl::PointXYZI>::Ptr
tmp_ptr (
new pcl::PointCloud<pcl::PointXYZI>());
433- pcl::fromROSMsg (map_array_msg_.submaps [num_submaps - 1 - i].cloud , *tmp_ptr);
434- targeted_cloud_ += *tmp_ptr;
435- }
436+ pcl::fromROSMsg (map_array_msg_.submaps [num_submaps - 1 - i].cloud , *tmp_ptr);
437+ targeted_cloud_ += *tmp_ptr;
438+ }
436439
437- /* map array */
438- sensor_msgs::msg::PointCloud2::Ptr transformed_cloud_msg_ptr (new sensor_msgs::msg::PointCloud2);
439- pcl::toROSMsg (*transformed_cloud_ptr, *transformed_cloud_msg_ptr);
440-
441- lidarslam_msgs::msg::SubMap submap;
442- submap.header .frame_id = global_frame_id_;
443- submap.header .stamp = corrent_pose_stamped.header .stamp ;
444- latest_distance_ += trans_;
445- submap.distance = latest_distance_;
446- submap.pose = corrent_pose_stamped.pose ;
447- submap.cloud = *transformed_cloud_msg_ptr;
448- submap.cloud .header .frame_id = global_frame_id_;
449- map_array_msg_.header .stamp = corrent_pose_stamped.header .stamp ;
450- map_array_msg_.submaps .push_back (submap);
451- map_array_pub_->publish (map_array_msg_);
452-
453- is_map_updated_ = true ;
454-
455- rclcpp::Time map_time = clock_.now ();
456- double dt = map_time.seconds () - last_map_time_.seconds ();
457- if (dt > map_publish_period_) publishMap ();
440+ /* map array */
441+ sensor_msgs::msg::PointCloud2::Ptr transformed_cloud_msg_ptr (new sensor_msgs::msg::PointCloud2);
442+ pcl::toROSMsg (*transformed_cloud_ptr, *transformed_cloud_msg_ptr);
443+
444+ lidarslam_msgs::msg::SubMap submap;
445+ submap.header .frame_id = global_frame_id_;
446+ submap.header .stamp = corrent_pose_stamped.header .stamp ;
447+ latest_distance_ += trans_;
448+ submap.distance = latest_distance_;
449+ submap.pose = corrent_pose_stamped.pose ;
450+ submap.cloud = *transformed_cloud_msg_ptr;
451+ submap.cloud .header .frame_id = global_frame_id_;
452+ map_array_msg_.header .stamp = corrent_pose_stamped.header .stamp ;
453+ map_array_msg_.submaps .push_back (submap);
454+ map_array_pub_->publish (map_array_msg_);
455+
456+ is_map_updated_ = true ;
457+
458+ rclcpp::Time map_time = clock_.now ();
459+ double dt = map_time.seconds () - last_map_time_.seconds ();
460+ if (dt > map_publish_period_) {
461+ publishMap ();
458462 last_map_time_ = map_time;
463+ }
459464}
460465
461466Eigen::Matrix4f ScanMatcherComponent::getTransformation (geometry_msgs::msg::Pose pose)
@@ -501,6 +506,7 @@ void ScanMatcherComponent::receiveOdom(const nav_msgs::msg::Odometry odom_msg)
501506
502507void ScanMatcherComponent::publishMap ()
503508{
509+ RCLCPP_INFO (get_logger (), " publish a map"
504510 pcl::PointCloud<pcl::PointXYZI>::Ptr
map_ptr (
new pcl::PointCloud<pcl::PointXYZI>(map_));
505511 sensor_msgs::msg::PointCloud2::Ptr map_msg_ptr (new sensor_msgs::msg::PointCloud2);
506512 pcl::toROSMsg (*map_ptr, *map_msg_ptr);
0 commit comments