From f4a47e92b6cd41d09dff309d1303eb730c3b362c Mon Sep 17 00:00:00 2001 From: ryohei sasaki Date: Sun, 26 Nov 2023 17:38:55 +0900 Subject: [PATCH] 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 --- .github/workflows/main.yml | 77 ++++----- .../src/graph_based_slam_component.cpp | 18 +- lidarslam/CMakeLists.txt | 2 +- .../scanmatcher/scanmatcher_component.h | 7 +- scanmatcher/src/scanmatcher_component.cpp | 163 +++++++++--------- 5 files changed, 135 insertions(+), 132 deletions(-) diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index 6e59b13..940c405 100644 --- 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 \ No newline at end of file + 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 index 7727cec..557eb22 100644 --- 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> gicp(new pclomp::GeneralizedIterativeClosestPoint()); 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::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 index 50ba49d..919ccc8 100644 --- 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 index d804d7b..ac6200e 100644 --- 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 ::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 diff --git a/scanmatcher/src/scanmatcher_component.cpp b/scanmatcher/src/scanmatcher_component.cpp index d3419de..bbf9e73 100644 --- a/scanmatcher/src/scanmatcher_component.cpp +++ b/scanmatcher/src/scanmatcher_component.cpp @@ -12,6 +12,7 @@ ScanMatcherComponent::ScanMatcherComponent(const rclcpp::NodeOptions & options) listener_(tfbuffer_), broadcaster_(this) { + RCLCPP_INFO(get_logger(), "initialization start"); double ndt_resolution; int ndt_num_threads; double gicp_corr_dist_threshold; @@ -111,12 +112,15 @@ ScanMatcherComponent::ScanMatcherComponent(const rclcpp::NodeOptions & options) registration_ = ndt; - } else { + } else if (registration_method_ == "GICP") { boost::shared_ptr> gicp(new pclomp::GeneralizedIterativeClosestPoint()); gicp->setMaxCorrespondenceDistance(gicp_corr_dist_threshold); 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_; @@ -129,6 +133,7 @@ ScanMatcherComponent::ScanMatcherComponent(const rclcpp::NodeOptions & options) initializePubSub(); if (set_initial_pose_) { + RCLCPP_INFO(get_logger(), "set initial pose"); auto msg = std::make_shared(); msg->header.stamp = now(); msg->header.frame_id = global_frame_id_; @@ -174,81 +179,53 @@ 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::Ptr tmp_ptr(new pcl::PointCloud()); - pcl::fromROSMsg(transformed_msg, *tmp_ptr); - - if (use_imu_) { - double scan_time = msg->header.stamp.sec + - msg->header.stamp.nanosec * 1e-9; - lidar_undistortion_.adjustDistortion(tmp_ptr, scan_time); - } - - if (use_min_max_filter_) { - double r; - pcl::PointCloud::Ptr tmp_ptr2(new pcl::PointCloud()); - 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_ptr2->points.push_back(p);} - } - tmp_ptr = tmp_ptr2; - } - - - if (!initial_cloud_received_) { - RCLCPP_INFO(get_logger(), "create a first map"); - pcl::PointCloud::Ptr cloud_ptr(new pcl::PointCloud()); - pcl::VoxelGrid 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::Ptr transformed_cloud_ptr( - new pcl::PointCloud()); - 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); + if (!initial_pose_received_) + { + RCLCPP_WARN(get_logger(), "initial_pose is not received"); + return; + } - // 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); + 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; + } - map_pub_->publish(submap.cloud); + pcl::PointCloud::Ptr tmp_ptr(new pcl::PointCloud()); + pcl::fromROSMsg(transformed_msg, *tmp_ptr); - last_map_time_ = clock_.now(); + if (use_imu_) { + double scan_time = msg->header.stamp.sec + + msg->header.stamp.nanosec * 1e-9; + lidar_undistortion_.adjustDistortion(tmp_ptr, scan_time); + } + if (use_min_max_filter_) { + double r; + pcl::PointCloud::Ptr tmp_ptr2(new pcl::PointCloud()); + 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_ptr2->points.push_back(p);} } + tmp_ptr = tmp_ptr2; + } - 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);} + }; auto imu_callback = @@ -282,6 +259,40 @@ void ScanMatcherComponent::initializePubSub() path_pub_ = create_publisher("path", rclcpp::QoS(10)); } +void ScanMatcherComponent::initializeMap(const pcl::PointCloud ::Ptr & tmp_ptr, const std_msgs::msg::Header & header) +{ + RCLCPP_INFO(get_logger(), "create a first map"); + pcl::PointCloud::Ptr cloud_ptr(new pcl::PointCloud()); + pcl::VoxelGrid 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::Ptr transformed_cloud_ptr( + new pcl::PointCloud()); + 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::ConstPtr & cloud_ptr, const rclcpp::Time stamp) @@ -474,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; } } @@ -515,17 +526,15 @@ void ScanMatcherComponent::receiveImu(const sensor_msgs::msg::Imu 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::Ptr map_ptr(new pcl::PointCloud); - for (auto & submap : map_array_msg_.submaps) { + for (auto & submap : map_array_msg.submaps) { pcl::PointCloud::Ptr submap_cloud_ptr(new pcl::PointCloud); pcl::PointCloud::Ptr transformed_submap_cloud_ptr( new pcl::PointCloud); pcl::fromROSMsg(submap.cloud, *submap_cloud_ptr); - + Eigen::Affine3d affine; tf2::fromMsg(submap.pose, affine); pcl::transformPointCloud( @@ -534,15 +543,15 @@ void ScanMatcherComponent::publishMap() *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(graphslam::ScanMatcherComponent)