Refactoring similar to lidarslam_ros2

graph_based_slam/include/graph_based_slam/graph_based_slam_component.h

@@ -142,6 +142,6 @@ namespace graphslam
     std::vector < LoopEdge > loop_edges_;
 
   };
-}
+} // namespace graphslam
 
 #endif  //GS_GBS_COMPONENT_H_INCLUDED

graph_based_slam/src/graph_based_slam_component.cpp

@@ -67,7 +67,7 @@ GraphBasedSlamComponent::GraphBasedSlamComponent(const rclcpp::NodeOptions & opt
     ndt->setNeighborhoodSearchMethod(pclomp::DIRECT7);
     if (ndt_num_threads > 0) {ndt->setNumThreads(ndt_num_threads);}
     registration_ = ndt;
-  } else {
+  } else if (registration_method == "GICP") {
     boost::shared_ptr<pclomp::GeneralizedIterativeClosestPoint<pcl::PointXYZI, pcl::PointXYZI>>
       gicp(new pclomp::GeneralizedIterativeClosestPoint<pcl::PointXYZI, pcl::PointXYZI>());
     gicp->setMaxCorrespondenceDistance(30);
@@ -77,6 +77,9 @@ GraphBasedSlamComponent::GraphBasedSlamComponent(const rclcpp::NodeOptions & opt
     gicp->setEuclideanFitnessEpsilon(1e-6);
     gicp->setRANSACIterations(0);
     registration_ = gicp;
+  } else {
+    RCLCPP_ERROR(get_logger(), "invalid registration method");
+    exit(1);
   }
 
   initializePubSub();
@@ -149,8 +152,7 @@ void GraphBasedSlamComponent::searchLoop()
   lidarslam_msgs::msg::MapArray map_array_msg = map_array_msg_;
   std::lock_guard<std::mutex> lock(mtx_);
   int num_submaps = map_array_msg.submaps.size();
-  std::cout << "----------------------------" << std::endl;
-  std::cout << "searching Loop, num_submaps:" << num_submaps << std::endl;
+  RCLCPP_INFO(get_logger(), "searching Loop, num_submaps:%d", num_submaps);
 
   double min_fitness_score = std::numeric_limits<double>::max();
   double distance_min_fitness_score = 0;
@@ -231,21 +233,16 @@ void GraphBasedSlamComponent::searchLoop()
         loop_edge.relative_pose = Eigen::Isometry3d(from.inverse() * to);
         loop_edges_.push_back(loop_edge);
 
-        std::cout << "---" << std::endl;
-        std::cout << "PoseAdjustment" << std::endl;
-        std::cout << "distance:" << min_submap.distance << ", score:" << fitness_score << std::endl;
-        std::cout << "id_loop_point 1:" << id_min << std::endl;
-        std::cout << "id_loop_point 2:" << num_submaps - 1 << std::endl;
+        std::cout << "PoseAdjustment" << " distance:" << min_submap.distance << ", score:" << fitness_score << std::endl;
+        std::cout << "id_loop_point 1:" << id_min << " id_loop_point 2:" << num_submaps - 1 << std::endl;
         std::cout << "final transformation:" << std::endl;
         std::cout << registration_->getFinalTransformation() << std::endl;
         doPoseAdjustment(map_array_msg, use_save_map_in_loop_);
 
         return;
       }
 
-      std::cout << "-" << std::endl;
-      std::cout << "min_submap_distance:" << min_submap.distance << std::endl;
-      std::cout << "min_fitness_score:" << fitness_score << std::endl;
+      std::cout << "min_submap_distance:" << min_submap.distance << " min_fitness_score:" << fitness_score << std::endl;
   }
 }
 
@@ -358,7 +355,7 @@ void GraphBasedSlamComponent::doPoseAdjustment(
 
 }
 
-}
+} // namespace graphslam
 
 #include <rclcpp_components/register_node_macro.hpp>
 RCLCPP_COMPONENTS_REGISTER_NODE(graphslam::GraphBasedSlamComponent)

scanmatcher/include/scanmatcher/scanmatcher_component.h

@@ -124,6 +124,7 @@ namespace graphslam
     rclcpp::Publisher < nav_msgs::msg::Odometry > ::SharedPtr odom_pub_;
 
     void initializePubSub();
+    void initializeMap(const pcl::PointCloud <pcl::PointXYZI>::Ptr & cloud_ptr, const std_msgs::msg::Header & header);
     void receiveCloud(
       const pcl::PointCloud < PointType > ::ConstPtr & input_cloud_ptr,
       const rclcpp::Time stamp);
@@ -135,7 +136,7 @@ namespace graphslam
       const rclcpp::Time stamp
     );
     Eigen::Matrix4f getTransformation(const geometry_msgs::msg::Pose pose);
-    void publishMap();
+    void publishMap(const lidarslam_msgs::msg::MapArray & map_array_msg , const std::string & map_frame_id);
     void updateMap(
       const pcl::PointCloud < PointType > ::ConstPtr cloud_ptr,
       const Eigen::Matrix4f final_transformation,

scanmatcher/param/lio_bigloop.yaml

@@ -2,9 +2,9 @@
     ros__parameters:
       # LiDAR setting
       pointCloudTopic: "points_raw"
-      sensor: velodyne
-      N_SCAN: 16
-      Horizon_SCAN: 1800
+      sensor: velodyne    # lidar sensor type, 'velodyne' or 'ouster' or 'livox'
+      N_SCAN: 16          # number of lidar channel (i.e., Velodyne/Ouster: 16, 32, 64, 128, Livox Horizon: 6)
+      Horizon_SCAN: 1800  # lidar horizontal resolution (Velodyne:1800, Ouster:512,1024,2048, Livox Horizon: 4000)
       # IMU setting
       imuTopic: "imu_correct"
       imuAccNoise: 3.9939570888238808e-03

scanmatcher/src/scanmatcher_component.cpp

@@ -107,13 +107,16 @@ ScanMatcherComponent::ScanMatcherComponent(const rclcpp::NodeOptions & options)
 
     registration_ = ndt;
 
-  } else {
+  } else if (registration_method == "GICP") {
     boost::shared_ptr<pclomp::GeneralizedIterativeClosestPoint<PointType, PointType>>
       gicp(new pclomp::GeneralizedIterativeClosestPoint<PointType, PointType>());
     gicp->setMaxCorrespondenceDistance(gicp_corr_dist_threshold);
     //gicp->setCorrespondenceRandomness(20);
     gicp->setTransformationEpsilon(1e-8);
     registration_ = gicp;
+  } else {
+    RCLCPP_ERROR(get_logger(), "invalid registration method");
+    exit(1);
   }
 
   map_array_msg_.header.frame_id = global_frame_id_;
@@ -169,73 +172,45 @@ void ScanMatcherComponent::initializePubSub()
   auto cloud_callback =
     [this](const typename sensor_msgs::msg::PointCloud2::SharedPtr msg) -> void
     {
-      if (initial_pose_received_) {
-        sensor_msgs::msg::PointCloud2 transformed_msg;
-        try {
-          tf2::TimePoint time_point = tf2::TimePoint(
-            std::chrono::seconds(msg->header.stamp.sec) +
-            std::chrono::nanoseconds(msg->header.stamp.nanosec));
-          const geometry_msgs::msg::TransformStamped transform = tfbuffer_.lookupTransform(
-            robot_frame_id_, msg->header.frame_id, time_point);
-          tf2::doTransform(*msg, transformed_msg, transform); // TODO:slow now(https://github.com/ros/geometry2/pull/432)
-        } catch (tf2::TransformException & e) {
-          RCLCPP_ERROR(this->get_logger(), "%s", e.what());
-          return;
-        }
-
-        pcl::PointCloud<PointType>::Ptr tmp_ptr(new pcl::PointCloud<PointType>());
-        pcl::fromROSMsg(transformed_msg, *tmp_ptr);
-
-        if (use_min_max_filter_) {
-          double r;
-          pcl::PointCloud<PointType> tmp;
-          for (const auto & p : tmp_ptr->points) {
-            r = sqrt(pow(p.x, 2.0) + pow(p.y, 2.0));
-            if (scan_min_range_ < r && r < scan_max_range_) {tmp.push_back(p);}
-          }
-        }
+      if (!initial_pose_received_)
+      {
+        RCLCPP_WARN(get_logger(), "initial_pose is not received");
+        return;
+      }
+      sensor_msgs::msg::PointCloud2 transformed_msg;
+      try {
+        tf2::TimePoint time_point = tf2::TimePoint(
+          std::chrono::seconds(msg->header.stamp.sec) +
+          std::chrono::nanoseconds(msg->header.stamp.nanosec));
+        const geometry_msgs::msg::TransformStamped transform = tfbuffer_.lookupTransform(
+          robot_frame_id_, msg->header.frame_id, time_point);
+        tf2::doTransform(*msg, transformed_msg, transform); // TODO:slow now(https://github.com/ros/geometry2/pull/432)
+      } catch (tf2::TransformException & e) {
+        RCLCPP_ERROR(this->get_logger(), "%s", e.what());
+        return;
+      }
 
-        if (!initial_cloud_received_) {
-          RCLCPP_INFO(get_logger(), "create a first map");
-          pcl::PointCloud<PointType>::Ptr cloud_ptr(new pcl::PointCloud<PointType>());
-          pcl::VoxelGrid<PointType> voxel_grid;
-          voxel_grid.setLeafSize(vg_size_for_map_, vg_size_for_map_, vg_size_for_map_);
-          voxel_grid.setInputCloud(tmp_ptr);
-          voxel_grid.filter(*cloud_ptr);
-
-          initial_cloud_received_ = true;
-
-          Eigen::Matrix4f sim_trans = getTransformation(corrent_pose_stamped_.pose);
-          pcl::PointCloud<PointType>::Ptr transformed_cloud_ptr(
-            new pcl::PointCloud<PointType>());
-          pcl::transformPointCloud(*cloud_ptr, *transformed_cloud_ptr, sim_trans);
-          registration_->setInputTarget(transformed_cloud_ptr);
-
-          // map
-          sensor_msgs::msg::PointCloud2::SharedPtr map_msg_ptr(new sensor_msgs::msg::PointCloud2);
-          pcl::toROSMsg(*transformed_cloud_ptr, *map_msg_ptr);
-
-          //map array
-          sensor_msgs::msg::PointCloud2::SharedPtr cloud_msg_ptr(
-            new sensor_msgs::msg::PointCloud2);
-          pcl::toROSMsg(*cloud_ptr, *cloud_msg_ptr);
-          lidarslam_msgs::msg::SubMap submap;
-          submap.header = msg->header;
-          submap.distance = 0;
-          submap.pose = corrent_pose_stamped_.pose;
-          submap.cloud = *cloud_msg_ptr;
-          map_array_msg_.header = msg->header;
-          map_array_msg_.submaps.push_back(submap);
-
-          map_pub_->publish(submap.cloud);
-
-          last_map_time_ = clock_.now();
+      pcl::PointCloud<PointType>::Ptr tmp_ptr(new pcl::PointCloud<PointType>());
+      pcl::fromROSMsg(transformed_msg, *tmp_ptr);
 
+      if (use_min_max_filter_) {
+        double r;
+        pcl::PointCloud<PointType> tmp;
+        for (const auto & p : tmp_ptr->points) {
+          r = sqrt(pow(p.x, 2.0) + pow(p.y, 2.0));
+          if (scan_min_range_ < r && r < scan_max_range_) {tmp.push_back(p);}
         }
+      }
 
-        if (initial_cloud_received_) {receiveCloud(tmp_ptr, msg->header.stamp);}
+      if (!initial_cloud_received_) {
+        RCLCPP_INFO(get_logger(), "initial_cloud is received");
+        initial_cloud_received_ = true;
+        initializeMap(tmp_ptr, msg->header);
+        last_map_time_ = clock_.now();
       }
 
+      if (initial_cloud_received_) {receiveCloud(tmp_ptr, msg->header.stamp);}
+
     };
 
 
@@ -278,6 +253,40 @@ void ScanMatcherComponent::initializePubSub()
         1)).reliable());
 }
 
+void ScanMatcherComponent::initializeMap(const pcl::PointCloud <pcl::PointXYZI>::Ptr & tmp_ptr, const std_msgs::msg::Header & header)
+{
+  RCLCPP_INFO(get_logger(), "create a first map");
+  pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_ptr(new pcl::PointCloud<pcl::PointXYZI>());
+  pcl::VoxelGrid<pcl::PointXYZI> voxel_grid;
+  voxel_grid.setLeafSize(vg_size_for_map_, vg_size_for_map_, vg_size_for_map_);
+  voxel_grid.setInputCloud(tmp_ptr);
+  voxel_grid.filter(*cloud_ptr);
+
+  Eigen::Matrix4f sim_trans = getTransformation(corrent_pose_stamped_.pose);
+  pcl::PointCloud<pcl::PointXYZI>::Ptr transformed_cloud_ptr(
+    new pcl::PointCloud<pcl::PointXYZI>());
+  pcl::transformPointCloud(*cloud_ptr, *transformed_cloud_ptr, sim_trans);
+  registration_->setInputTarget(transformed_cloud_ptr);
+
+  // map
+  sensor_msgs::msg::PointCloud2::SharedPtr map_msg_ptr(new sensor_msgs::msg::PointCloud2);
+  pcl::toROSMsg(*transformed_cloud_ptr, *map_msg_ptr);
+
+  // map array
+  sensor_msgs::msg::PointCloud2::SharedPtr cloud_msg_ptr(
+    new sensor_msgs::msg::PointCloud2);
+  pcl::toROSMsg(*cloud_ptr, *cloud_msg_ptr);
+  lidarslam_msgs::msg::SubMap submap;
+  submap.header = header;
+  submap.distance = 0;
+  submap.pose = corrent_pose_stamped_.pose;
+  submap.cloud = *cloud_msg_ptr;
+  map_array_msg_.header = header;
+  map_array_msg_.submaps.push_back(submap);
+
+  map_pub_->publish(submap.cloud);
+}
+
 void ScanMatcherComponent::receiveCloud(
   const pcl::PointCloud<PointType>::ConstPtr & cloud_ptr,
   const rclcpp::Time stamp)
@@ -476,7 +485,7 @@ void ScanMatcherComponent::updateMap(
   rclcpp::Time map_time = clock_.now();
   double dt = map_time.seconds() - last_map_time_.seconds();
   if (dt > map_publish_period_) {
-    publishMap();
+    publishMap(map_array_msg_, global_frame_id_);
     last_map_time_ = map_time;
   }
 }
@@ -500,32 +509,32 @@ void ScanMatcherComponent::receiveOdom(const nav_msgs::msg::Odometry odom_msg)
   }
 }
 
-void ScanMatcherComponent::publishMap()
+void ScanMatcherComponent::publishMap(const lidarslam_msgs::msg::MapArray & map_array_msg , const std::string & map_frame_id)
 {
-  RCLCPP_INFO(get_logger(), "publish a map");
-
   pcl::PointCloud<pcl::PointXYZI>::Ptr map_ptr(new pcl::PointCloud<pcl::PointXYZI>);
-  for (auto & submap : map_array_msg_.submaps) {
+  for (auto & submap : map_array_msg.submaps) {
     pcl::PointCloud<pcl::PointXYZI>::Ptr submap_cloud_ptr(new pcl::PointCloud<pcl::PointXYZI>);
     pcl::PointCloud<pcl::PointXYZI>::Ptr transformed_submap_cloud_ptr(
         new pcl::PointCloud<pcl::PointXYZI>);
     pcl::fromROSMsg(submap.cloud, *submap_cloud_ptr);
+
     Eigen::Affine3d affine;
     tf2::fromMsg(submap.pose, affine);
     pcl::transformPointCloud(
       *submap_cloud_ptr, *transformed_submap_cloud_ptr,
       affine.matrix().cast<float>());
+
     *map_ptr += *transformed_submap_cloud_ptr;
   }
-  std::cout << "number of map　points: " << map_ptr->size() << std::endl;
+  RCLCPP_INFO(get_logger(), "publish a map, number of points in the map : %ld", map_ptr->size());
 
   sensor_msgs::msg::PointCloud2::SharedPtr map_msg_ptr(new sensor_msgs::msg::PointCloud2);
   pcl::toROSMsg(*map_ptr, *map_msg_ptr);
-  map_msg_ptr->header.frame_id = global_frame_id_;
+  map_msg_ptr->header.frame_id = map_frame_id;
   map_pub_->publish(*map_msg_ptr);
 }
 
-}
+} // namespace graphslam
 
 #include <rclcpp_components/register_node_macro.hpp>
 RCLCPP_COMPONENTS_REGISTER_NODE(graphslam::ScanMatcherComponent)