Feature/refactoring (#80)

* Refactor for parameter support and better logging. * Add initializeMap function * Fix log output in graph slam and add develop branch CI * Use ros official docker in CI * Install git in CI * Fix CI
rsasaki0109 · Nov 26, 2023 · f4a47e9 · f4a47e9
1 parent f75a186
commit f4a47e9
Show file tree

Hide file tree

Showing 5 changed files with 135 additions and 132 deletions.
diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml
@@ -4,49 +4,44 @@ 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: |
-                  sudo apt update && sudo apt install locales
-                  sudo locale-gen en_US en_US.UTF-8
-                  sudo update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8
-                  export LANG=en_US.UTF-8
-                  sudo apt update && sudo apt install curl gnupg2 lsb-release
-                  curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | sudo apt-key add -
-                  sudo sh -c 'echo "deb [arch=amd64,arm64] http://packages.ros.org/ros2/ubuntu `lsb_release -cs` main" > /etc/apt/sources.list.d/ros2-latest.list'
-                  sudo apt update
-                  sudo apt install ros-humble-desktop
-                  source /opt/ros/humble/setup.bash
-            - uses: actions/checkout@v1
-              with:
-                submodules: true
-            - name: Copy repository
-              run: |
-                  mkdir -p ~/ros2_ws/src/lidarslam_ros2
-                  cp -rf . ~/ros2_ws/src/lidarslam_ros2
-            - name: Install dependencies
-              run: |
-                  source /opt/ros/humble/setup.bash
-                  sudo apt install -y python3-rosdep2
-                  rosdep update
-                  cd ~/ros2_ws/src
-                  rosdep install -r -y --from-paths . --ignore-src       
-            - name: Build packages
-              run: |
-                  source /opt/ros/humble/setup.bash
-                  # Install colcon
-                  # Ref: https://index.ros.org/doc/ros2/Tutorials/Colcon-Tutorial/
-                  sudo apt install python3-colcon-common-extensions
-                  cd ~/ros2_ws
-                  colcon build
-                  source ~/ros2_ws/install/setup.bash
+  job1:
+    name: Build
+    runs-on: ubuntu-22.04
+    container: ros:humble-ros-core
+    steps:
+      - name: Install Git
+        run: |
+            apt-get update
+            apt-get install -y git
+        shell: bash
+      - uses: actions/checkout@v2
+        with:
+          submodules: true
+      - name: Copy repository
+        run: |
+            mkdir -p ~/ros2_ws/src/lidarslam_ros2
+            cp -rf . ~/ros2_ws/src/lidarslam_ros2
+        shell: bash
+      - name: Install dependencies
+        run: |
+            apt-get install -y python3-rosdep
+            rosdep init
+            rosdep update
+            cd ~/ros2_ws/src
+            rosdep install -r -y --from-paths . --ignore-src
+        shell: bash
+      - name: Build packages
+        run: |
+            source /opt/ros/humble/setup.bash
+            apt-get install -y python3-colcon-common-extensions
+            cd ~/ros2_ws
+            colcon build
+            source ~/ros2_ws/install/setup.bash
+        shell: 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/lidarslam/CMakeLists.txt b/lidarslam/CMakeLists.txt
@@ -92,7 +92,7 @@ install(
   DESTINATION share/${PROJECT_NAME}
 )
 
-install(TARGETS 
+install(TARGETS
   lidarslam
   DESTINATION lib/${PROJECT_NAME}
 )

diff --git a/scanmatcher/include/scanmatcher/scanmatcher_component.h b/scanmatcher/include/scanmatcher/scanmatcher_component.h
@@ -114,8 +114,9 @@ namespace graphslam
     rclcpp::Publisher < nav_msgs::msg::Path > ::SharedPtr path_pub_;
 
     void initializePubSub();
+    void initializeMap(const pcl::PointCloud <pcl::PointXYZI>::Ptr & cloud_ptr, const std_msgs::msg::Header & header);
     void receiveCloud(
-      const pcl::PointCloud < pcl::PointXYZI > ::ConstPtr & input_cloud_ptr,
+      const pcl::PointCloud < pcl::PointXYZI> ::ConstPtr & input_cloud_ptr,
       const rclcpp::Time stamp);
     void receiveImu(const sensor_msgs::msg::Imu imu_msg);
     void publishMapAndPose(
@@ -124,7 +125,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 < pcl::PointXYZI > ::ConstPtr cloud_ptr,
       const Eigen::Matrix4f final_transformation,
@@ -173,6 +174,6 @@ namespace graphslam
     LidarUndistortion lidar_undistortion_;
 
   };
-}
+} // namespace graphslam
 
 #endif  //GS_SM_COMPONENT_H_INCLUDED