From c0431deb36bb0c9f8eace4e0c6cbf276e7ab5d4f Mon Sep 17 00:00:00 2001 From: Alpaca-zip Date: Sun, 12 Nov 2023 19:54:12 +0900 Subject: [PATCH 1/3] changed to declare parameters before accessing them --- .../include/ground_segmentation/ground_segmentation.h | 6 ++++++ .../src/ground_segmentation_node.cc | 6 ++++++ .../src/ground_segmentation_test_node.cc | 4 ++++ 3 files changed, 16 insertions(+) diff --git a/linefit_ground_segmentation/include/ground_segmentation/ground_segmentation.h b/linefit_ground_segmentation/include/ground_segmentation/ground_segmentation.h index 6dca5fa..f809e07 100644 --- a/linefit_ground_segmentation/include/ground_segmentation/ground_segmentation.h +++ b/linefit_ground_segmentation/include/ground_segmentation/ground_segmentation.h @@ -55,6 +55,12 @@ struct GroundSegmentationParams { double line_search_angle; // Number of threads. int n_threads; + // Minimum point distance. + double r_min; + // Maximum point distance. + double r_max; + // Maximum error of a point during line fit. + double max_fit_error; }; typedef pcl::PointCloud PointCloud; diff --git a/linefit_ground_segmentation_ros/src/ground_segmentation_node.cc b/linefit_ground_segmentation_ros/src/ground_segmentation_node.cc index 35c3f88..8ea4c2e 100644 --- a/linefit_ground_segmentation_ros/src/ground_segmentation_node.cc +++ b/linefit_ground_segmentation_ros/src/ground_segmentation_node.cc @@ -52,6 +52,10 @@ SegmentationNode::SegmentationNode(const rclcpp::NodeOptions &node_options) params_.line_search_angle = this->declare_parameter("line_search_angle", params_.line_search_angle); params_.n_threads = this->declare_parameter("n_threads", params_.n_threads); + params_.r_min = this->declare_parameter("r_min", params_.r_min); + params_.r_max = this->declare_parameter("r_max", params_.r_max); + params_.max_fit_error = + this->declare_parameter("max_fit_error", params_.max_fit_error); // Params that need to be squared. double r_min, r_max, max_fit_error; if (this->get_parameter("r_min", r_min)) { @@ -81,6 +85,8 @@ SegmentationNode::SegmentationNode(const rclcpp::NodeOptions &node_options) tf_buffer_ = std::make_shared(this->get_clock()); tf_listener_ = std::make_shared(*tf_buffer_); RCLCPP_INFO(this->get_logger(), "Segmentation node initialized"); + RCLCPP_INFO(this->get_logger(), "params_.r_min_square: %f", + params_.r_min_square); } void SegmentationNode::scanCallback( diff --git a/linefit_ground_segmentation_ros/src/ground_segmentation_test_node.cc b/linefit_ground_segmentation_ros/src/ground_segmentation_test_node.cc index 0ad4510..26d52c4 100644 --- a/linefit_ground_segmentation_ros/src/ground_segmentation_test_node.cc +++ b/linefit_ground_segmentation_ros/src/ground_segmentation_test_node.cc @@ -34,6 +34,10 @@ int main(int argc, char **argv) { params.line_search_angle = node->declare_parameter("line_search_angle", params.line_search_angle); params.n_threads = node->declare_parameter("n_threads", params.n_threads); + params.r_min = node->declare_parameter("r_min", params.r_min); + params.r_max = node->declare_parameter("r_max", params.r_max); + params.max_fit_error = + node->declare_parameter("max_fit_error", params.max_fit_error); // Params that need to be squared. double r_min, r_max, max_fit_error; if (node->get_parameter("r_min", r_min)) { From 4965b1cc08ed660afb6984250e7c65e63e75cd02 Mon Sep 17 00:00:00 2001 From: Alpaca-zip Date: Sun, 12 Nov 2023 19:55:58 +0900 Subject: [PATCH 2/3] remove unused parameter --- README.md | 1 - linefit_ground_segmentation_ros/launch/segmentation_params.yaml | 1 - 2 files changed, 2 deletions(-) diff --git a/README.md b/README.md index 1286b7b..c3aff15 100644 --- a/README.md +++ b/README.md @@ -63,5 +63,4 @@ The default parameters should work on the KITTI dataset. ### Other - **n_threads** Number of threads to use. -- **latch** Latch output point clouds in ROS node. - **visualize** Visualize the segmentation result. **ONLY FOR DEBUGGING.** Do not set true during online operation. diff --git a/linefit_ground_segmentation_ros/launch/segmentation_params.yaml b/linefit_ground_segmentation_ros/launch/segmentation_params.yaml index 0153f38..da7d9a6 100644 --- a/linefit_ground_segmentation_ros/launch/segmentation_params.yaml +++ b/linefit_ground_segmentation_ros/launch/segmentation_params.yaml @@ -20,7 +20,6 @@ ground_segmentation: gravity_aligned_frame: "" # Frame which has its z axis aligned with gravity. (Sensor frame if empty.) - latch: false # latch output topics or not visualize: false # visualize segmentation result - USE ONLY FOR DEBUGGING input_topic: "livox/lidar/pointcloud" From 16ee812bda0a68103e5a0bfbf11f1d30da4e7347 Mon Sep 17 00:00:00 2001 From: Alpaca-zip Date: Sun, 12 Nov 2023 19:57:26 +0900 Subject: [PATCH 3/3] comment out pcl_ros/point_cloud includes --- linefit_ground_segmentation_ros/src/ground_segmentation_node.cc | 2 +- .../src/ground_segmentation_test_node.cc | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/linefit_ground_segmentation_ros/src/ground_segmentation_node.cc b/linefit_ground_segmentation_ros/src/ground_segmentation_node.cc index 8ea4c2e..a2b3e82 100644 --- a/linefit_ground_segmentation_ros/src/ground_segmentation_node.cc +++ b/linefit_ground_segmentation_ros/src/ground_segmentation_node.cc @@ -4,7 +4,7 @@ #include #include #include -#include +// #include #include #include #include diff --git a/linefit_ground_segmentation_ros/src/ground_segmentation_test_node.cc b/linefit_ground_segmentation_ros/src/ground_segmentation_test_node.cc index 26d52c4..b58c6de 100644 --- a/linefit_ground_segmentation_ros/src/ground_segmentation_test_node.cc +++ b/linefit_ground_segmentation_ros/src/ground_segmentation_test_node.cc @@ -1,5 +1,5 @@ #include -#include +// #include #include #include