From 31951c3ca729e04b397bd41e0a51ed7e467fc047 Mon Sep 17 00:00:00 2001 From: Ryohei Sasaki Date: Mon, 27 Nov 2023 07:28:00 +0900 Subject: [PATCH] Refactoring similar to lidarslam_ros2 --- .../graph_based_slam_component.h | 2 +- .../src/graph_based_slam_component.cpp | 21 ++- .../scanmatcher/scanmatcher_component.h | 3 +- scanmatcher/param/lio_bigloop.yaml | 6 +- scanmatcher/src/scanmatcher_component.cpp | 149 ++++++++++-------- 5 files changed, 94 insertions(+), 87 deletions(-) diff --git a/graph_based_slam/include/graph_based_slam/graph_based_slam_component.h b/graph_based_slam/include/graph_based_slam/graph_based_slam_component.h index 289de95..1516b4b 100644 --- a/graph_based_slam/include/graph_based_slam/graph_based_slam_component.h +++ b/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 diff --git a/graph_based_slam/src/graph_based_slam_component.cpp b/graph_based_slam/src/graph_based_slam_component.cpp index 0295645..012ac46 100644 --- a/graph_based_slam/src/graph_based_slam_component.cpp +++ b/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> gicp(new pclomp::GeneralizedIterativeClosestPoint()); 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 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::max(); double distance_min_fitness_score = 0; @@ -231,11 +233,8 @@ 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_); @@ -243,9 +242,7 @@ void GraphBasedSlamComponent::searchLoop() 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(graphslam::GraphBasedSlamComponent) diff --git a/scanmatcher/include/scanmatcher/scanmatcher_component.h b/scanmatcher/include/scanmatcher/scanmatcher_component.h index 6d65128..b5a6116 100644 --- a/scanmatcher/include/scanmatcher/scanmatcher_component.h +++ b/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 ::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, diff --git a/scanmatcher/param/lio_bigloop.yaml b/scanmatcher/param/lio_bigloop.yaml index 455c652..6dd7245 100644 --- a/scanmatcher/param/lio_bigloop.yaml +++ b/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 diff --git a/scanmatcher/src/scanmatcher_component.cpp b/scanmatcher/src/scanmatcher_component.cpp index 9b7c580..4aef96b 100644 --- a/scanmatcher/src/scanmatcher_component.cpp +++ b/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> gicp(new pclomp::GeneralizedIterativeClosestPoint()); 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::Ptr tmp_ptr(new pcl::PointCloud()); - pcl::fromROSMsg(transformed_msg, *tmp_ptr); - - if (use_min_max_filter_) { - double r; - pcl::PointCloud 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::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); - - //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::Ptr tmp_ptr(new pcl::PointCloud()); + pcl::fromROSMsg(transformed_msg, *tmp_ptr); + if (use_min_max_filter_) { + double r; + pcl::PointCloud 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 ::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) @@ -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::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( *submap_cloud_ptr, *transformed_submap_cloud_ptr, affine.matrix().cast()); + *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)