Fix log output in graph slam and add develop branch CI

rsasaki0109 · Nov 26, 2023 · bb31294 · bb31294
1 parent d2281b0
commit bb31294
Show file tree

Hide file tree

Showing 3 changed files with 13 additions and 18 deletions.
diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml
@@ -4,16 +4,16 @@ on:
   push:
     branches:
       - humble
+      - develop
   pull_request:
     branches:
       - humble
+      - develop
 
 jobs:
     job1:
         name: Build
         runs-on: ubuntu-22.04
-        # container:
-        #   image: ubuntu:jammy 
         steps:
             - name: ROS2 Install
               run: |
@@ -40,7 +40,7 @@ jobs:
                   sudo apt install -y python3-rosdep2
                   rosdep update
                   cd ~/ros2_ws/src
-                  rosdep install -r -y --from-paths . --ignore-src       
+                  rosdep install -r -y --from-paths . --ignore-src
             - name: Build packages
               run: |
                   source /opt/ros/humble/setup.bash

diff --git a/graph_based_slam/src/graph_based_slam_component.cpp b/graph_based_slam/src/graph_based_slam_component.cpp
@@ -70,7 +70,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);
@@ -80,6 +80,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();
@@ -155,7 +158,7 @@ void GraphBasedSlamComponent::searchLoop()
 
   if(debug_flag_)
   {
-    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();
@@ -244,20 +247,15 @@ void GraphBasedSlamComponent::searchLoop()
       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;
   }
 }
 

diff --git a/scanmatcher/src/scanmatcher_component.cpp b/scanmatcher/src/scanmatcher_component.cpp
@@ -221,6 +221,7 @@ void ScanMatcherComponent::initializePubSub()
         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);}
@@ -290,8 +291,6 @@ void ScanMatcherComponent::initializeMap(const pcl::PointCloud <pcl::PointXYZI>:
   map_array_msg_.submaps.push_back(submap);
 
   map_pub_->publish(submap.cloud);
-
-  last_map_time_ = clock_.now();
 }
 
 void ScanMatcherComponent::receiveCloud(
@@ -529,8 +528,6 @@ void ScanMatcherComponent::receiveImu(const sensor_msgs::msg::Imu msg)
 
 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) {
     pcl::PointCloud<pcl::PointXYZI>::Ptr submap_cloud_ptr(new pcl::PointCloud<pcl::PointXYZI>);
@@ -546,7 +543,7 @@ void ScanMatcherComponent::publishMap(const lidarslam_msgs::msg::MapArray & map_
 
     *map_ptr += *transformed_submap_cloud_ptr;
   }
-  RCLCPP_INFO(get_logger(), "number of map points: %ld", map_ptr->size());
+  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);