diff --git a/cabot_navigation2/CMakeLists.txt b/cabot_navigation2/CMakeLists.txt index c503c4d6..72865fda 100644 --- a/cabot_navigation2/CMakeLists.txt +++ b/cabot_navigation2/CMakeLists.txt @@ -18,6 +18,8 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) find_package(geometry_msgs REQUIRED) +find_package(grid_map_msgs REQUIRED) +find_package(grid_map_ros REQUIRED) find_package(nav2_lifecycle_manager REQUIRED) find_package(nav2_costmap_2d REQUIRED) find_package(nav2_core REQUIRED) @@ -49,23 +51,25 @@ find_package(rosbag2_cpp REQUIRED) find_package(rosbag2_transport REQUIRED) find_package(rosbag2_storage REQUIRED) find_package(rosbag2_compression REQUIRED) -#find_package(OpenMP REQUIRED) +find_package(OpenMP REQUIRED) include_directories( include -# ${OPENMP_INCLUDES} + ${OPENMP_INCLUDES} ${OpenCV_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS} + ${grid_map_core_INCLUDE_DIRS} ) -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}") -#set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") if(NOT DEBUG) set(dependencies geometry_msgs + grid_map_msgs + grid_map_ros rclcpp nav2_lifecycle_manager nav2_costmap_2d @@ -83,7 +87,7 @@ set(dependencies diagnostic_updater visualization_msgs yaml_cpp_vendor -# OpenMP + OpenMP ) ### dwb critics @@ -178,6 +182,7 @@ ament_target_dependencies(clip_ground_filter_node pcl_conversions pcl_ros tf2_ros + visualization_msgs ) target_link_libraries(clip_ground_filter_node @@ -203,6 +208,28 @@ target_link_libraries(ransac_ground_filter_node ${PCL_LIBRARIES} ) +### grid map ground filter +add_executable(grid_map_ground_filter_node + src/abstract_ground_filter_node.cpp + src/grid_map_ground_filter_node.cpp +) + +ament_target_dependencies(grid_map_ground_filter_node + rclcpp + sensor_msgs + pcl_conversions + pcl_ros + tf2_ros + visualization_msgs + grid_map_msgs + grid_map_ros +) + +target_link_libraries(grid_map_ground_filter_node + ${PCL_LIBRARIES} + ${OPENMP_LIBRARIES} +) + ### cabot lifecycle manager add_executable(cabot_lifecycle_manager src/cabot_lifecycle_manager.cpp @@ -234,6 +261,7 @@ install(TARGETS cabot_lifecycle_manager clip_ground_filter_node ransac_ground_filter_node + grid_map_ground_filter_node ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION lib/${PROJECT_NAME} diff --git a/cabot_navigation2/include/cabot_navigation2/abstract_ground_filter_node.hpp b/cabot_navigation2/include/cabot_navigation2/abstract_ground_filter_node.hpp index c141a487..5103e2dc 100644 --- a/cabot_navigation2/include/cabot_navigation2/abstract_ground_filter_node.hpp +++ b/cabot_navigation2/include/cabot_navigation2/abstract_ground_filter_node.hpp @@ -64,9 +64,17 @@ class AbstractGroundFilterNode : public rclcpp::Node ~AbstractGroundFilterNode(); protected: - virtual void filterGround(const pcl::PointCloud::Ptr input, pcl::PointCloud::Ptr ground, pcl::PointCloud::Ptr filtered) = 0; + virtual void filterGround(const rclcpp::Time & time, const pcl::PointCloud::Ptr input, pcl::PointCloud::Ptr ground, pcl::PointCloud::Ptr filtered) = 0; std::string target_frame_; + float min_range_; + float max_range_; + bool publish_debug_ground_; + std::string output_debug_ground_topic_; + float ground_distance_threshold_; + + tf2_ros::TransformListener * tf_listener_; + tf2_ros::Buffer * tf_buffer_; private: void pointCloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr input); @@ -76,11 +84,6 @@ class AbstractGroundFilterNode : public rclcpp::Node std::string input_topic_; std::string output_ground_topic_; std::string output_filtered_topic_; - float min_range_; - float max_range_; - - tf2_ros::TransformListener * tf_listener_; - tf2_ros::Buffer * tf_buffer_; rclcpp::Subscription::SharedPtr pointcloud_sub_; rclcpp::Publisher::SharedPtr ground_pointcloud_pub_; diff --git a/cabot_navigation2/include/cabot_navigation2/clip_ground_filter_node.hpp b/cabot_navigation2/include/cabot_navigation2/clip_ground_filter_node.hpp index 9c5aca72..78de9c4a 100644 --- a/cabot_navigation2/include/cabot_navigation2/clip_ground_filter_node.hpp +++ b/cabot_navigation2/include/cabot_navigation2/clip_ground_filter_node.hpp @@ -21,6 +21,8 @@ #ifndef CABOT_NAVIGATION2__CLIP_GROUND_FILTER_NODE_HPP_ #define CABOT_NAVIGATION2__CLIP_GROUND_FILTER_NODE_HPP_ +#include + #include "cabot_navigation2/abstract_ground_filter_node.hpp" namespace cabot_navigation2 @@ -32,10 +34,10 @@ class ClipGroundFilterNode : public AbstractGroundFilterNode explicit ClipGroundFilterNode(const rclcpp::NodeOptions & options); protected: - void filterGround(const pcl::PointCloud::Ptr input, pcl::PointCloud::Ptr ground, pcl::PointCloud::Ptr filtered) override; + void filterGround(const rclcpp::Time & time, const pcl::PointCloud::Ptr input, pcl::PointCloud::Ptr ground, pcl::PointCloud::Ptr filtered) override; private: - float clip_height_; + rclcpp::Publisher::SharedPtr debug_plane_pub_; }; // class ClipGroundFilterNode } // namespace cabot_navigation2 diff --git a/cabot_navigation2/include/cabot_navigation2/grid_map_ground_filter_node.hpp b/cabot_navigation2/include/cabot_navigation2/grid_map_ground_filter_node.hpp new file mode 100644 index 00000000..8f53aa8c --- /dev/null +++ b/cabot_navigation2/include/cabot_navigation2/grid_map_ground_filter_node.hpp @@ -0,0 +1,92 @@ +// Copyright (c) 2024 Carnegie Mellon University, IBM Corporation, and others +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#ifndef CABOT_NAVIGATION2__GRID_MAP_GROUND_FILTER_NODE_HPP_ +#define CABOT_NAVIGATION2__GRID_MAP_GROUND_FILTER_NODE_HPP_ + +#include +#include +#include + +#include +#include +#include +#include + +#include "cabot_navigation2/abstract_ground_filter_node.hpp" + +namespace cabot_navigation2 +{ + +class GridMapGroundFilterNode : public AbstractGroundFilterNode +{ +public: + explicit GridMapGroundFilterNode(const rclcpp::NodeOptions & options); + +protected: + void filterGround(const rclcpp::Time & time, const pcl::PointCloud::Ptr input, pcl::PointCloud::Ptr ground, pcl::PointCloud::Ptr filtered) override; + +private: + void odomCallback(const nav_msgs::msg::Odometry::SharedPtr input); + int calcLivoxGridEstimatedNumPoints(float distance, float resolution); + bool isVisibleAngle(const grid_map::Position & check_position, const grid_map::Position & sensor_position, double sensor_yaw); + + int num_threads_; + std::string odom_topic_; + float grid_resolution_; + float grid_length_; + std::vector grid_patch_sizes_; + std::vector grid_patch_change_distances_; + int grid_occupied_inflate_size_; + int grid_num_points_min_threshold_; + float grid_num_points_raio_threshold_; + float grid_var_threshold_; + float grid_prob_prior_; + float grid_prob_free_; + float grid_prob_occupied_; + float grid_prob_forget_rate_; + float grid_prob_free_threshold_; + float grid_prob_occupied_threshold_; + float outlier_old_ground_threshold_; + float outlier_los_ground_threshold_; + float ground_estimate_angle_min_; + float ground_estimate_angle_max_; + float ground_slope_threshold_; + float ground_confidence_interpolate_decay_; + + static const int livox_num_points_; + static const float lixox_tan_fov_angle_; + + std::vector grid_half_patch_sizes_; + float log_odds_prior_; + float log_odds_free_; + float log_odds_occupied_; + float ground_estimate_radius_; + + std::recursive_mutex grid_map_mutex_; + std::shared_ptr grid_map_ptr_; + + rclcpp::Subscription::SharedPtr odom_sub_; + rclcpp::Publisher::SharedPtr debug_outlier_pointcloud_pub_; + rclcpp::Publisher::SharedPtr debug_grid_map_pub_; +}; // class GridMapGroundFilterNode + +} // namespace cabot_navigation2 +#endif // CABOT_NAVIGATION2__GRID_MAP_GROUND_FILTER_NODE_HPP_ diff --git a/cabot_navigation2/include/cabot_navigation2/ransac_ground_filter_node.hpp b/cabot_navigation2/include/cabot_navigation2/ransac_ground_filter_node.hpp index a85953c5..3dd4f67d 100644 --- a/cabot_navigation2/include/cabot_navigation2/ransac_ground_filter_node.hpp +++ b/cabot_navigation2/include/cabot_navigation2/ransac_ground_filter_node.hpp @@ -36,7 +36,7 @@ class RansacGroundFilterNode : public AbstractGroundFilterNode explicit RansacGroundFilterNode(const rclcpp::NodeOptions & options); protected: - void filterGround(const pcl::PointCloud::Ptr input, pcl::PointCloud::Ptr ground, pcl::PointCloud::Ptr filtered) override; + void filterGround(const rclcpp::Time & time, const pcl::PointCloud::Ptr input, pcl::PointCloud::Ptr ground, pcl::PointCloud::Ptr filtered) override; private: int ransac_max_iteration_; @@ -45,9 +45,6 @@ class RansacGroundFilterNode : public AbstractGroundFilterNode float ransac_input_min_height_; float ransac_input_max_height_; float ransac_inlier_threshold_; - float ground_distance_threshold_; - bool debug_; - std::string debug_output_plane_topic_; rclcpp::Publisher::SharedPtr debug_plane_pub_; }; // class RansacGroundFilterNode diff --git a/cabot_navigation2/launch/bringup_launch.py b/cabot_navigation2/launch/bringup_launch.py index 5c26c0e6..bf3d813e 100755 --- a/cabot_navigation2/launch/bringup_launch.py +++ b/cabot_navigation2/launch/bringup_launch.py @@ -55,7 +55,9 @@ def generate_launch_description(): offset = LaunchConfiguration('offset') cabot_side = LaunchConfiguration('cabot_side') low_obstacle_detect_version = LaunchConfiguration('low_obstacle_detect_version') - use_low_obstacle_detect = PythonExpression([low_obstacle_detect_version, ' > 0']) + publish_low_obstacle_ground = LaunchConfiguration('publish_low_obstacle_ground') + + use_low_obstacle_detect = PythonExpression([low_obstacle_detect_version, " > 0"]) remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] @@ -162,7 +164,11 @@ def generate_launch_description(): DeclareLaunchArgument( 'low_obstacle_detect_version', default_value='0', - description='0: do not detect, 1: remove ground by fixed height, 2: remove groud by RANSAC'), + description='0: do not detect, 1: remove ground by fixed height, 2: remove groud by RANSAC, 3: remove groud by grid map'), + + DeclareLaunchArgument( + 'publish_low_obstacle_ground', default_value='false', + description='publish ground to detect low obstacles only for debug purpose'), # default navigator Node( @@ -364,16 +370,18 @@ def generate_launch_description(): parameters=[{ 'use_sim_time': use_sim_time, 'target_frame': 'livox_footprint', + 'min_range': 0.05, + 'max_range': 5.0, + 'publish_debug_ground': publish_low_obstacle_ground, + 'output_debug_ground_topic': '/ground_filter_ground', + 'ground_distance_threshold': 0.05, 'xfer_format': PythonExpression(["2 if '", use_sim_time, "'=='true' else 0"]), 'ignore_noise': True, 'input_topic': '/livox/points', 'output_ground_topic': '/livox/points_ground', - 'output_filtered_topic': '/livox/points_filtered', - 'min_range': 0.05, - 'max_range': 5.0, - 'clip_height': 0.05 + 'output_filtered_topic': '/livox/points_filtered' }], - condition=IfCondition(PythonExpression([low_obstacle_detect_version, ' == 1'])) + condition=IfCondition(PythonExpression([low_obstacle_detect_version, " == 1"])) ), Node( @@ -384,24 +392,77 @@ def generate_launch_description(): parameters=[{ 'use_sim_time': use_sim_time, 'target_frame': 'livox_footprint', + 'min_range': 0.05, + 'max_range': 5.0, + 'publish_debug_ground': publish_low_obstacle_ground, + 'output_debug_ground_topic': '/ground_filter_ground', + 'ground_distance_threshold': 0.05, 'xfer_format': PythonExpression(["2 if '", use_sim_time, "'=='true' else 0"]), 'ignore_noise': True, 'input_topic': '/livox/points', 'output_ground_topic': '/livox/points_ground', 'output_filtered_topic': '/livox/points_filtered', - 'min_range': 0.05, - 'max_range': 5.0, 'ransac_max_iteration': 10000, 'ransac_probability': 0.999, 'ransac_eps_angle': 5.0, 'ransac_input_min_height': -0.50, 'ransac_input_max_height': 0.50, - 'ransac_inlier_threshold': 0.01, + 'ransac_inlier_threshold': 0.01 + }], + condition=IfCondition(PythonExpression([low_obstacle_detect_version, " == 2"])) + ), + + Node( + package='cabot_navigation2', + executable='grid_map_ground_filter_node', + namespace='', + name='grid_map_ground_filter_node', + parameters=[{ + 'use_sim_time': use_sim_time, + 'target_frame': 'livox_footprint', + 'min_range': 0.05, + 'max_range': 5.0, + 'publish_debug_ground': publish_low_obstacle_ground, + 'output_debug_ground_topic': '/ground_filter_ground', 'ground_distance_threshold': 0.05, - 'debug': False, - 'debug_output_plane_topic': '/ground_filter/ransac_plane' + 'xfer_format': PythonExpression(["2 if '", use_sim_time, "'=='true' else 0"]), + 'ignore_noise': True, + 'input_topic': '/livox/points', + 'output_ground_topic': '/livox/points_ground', + 'output_filtered_topic': '/livox/points_filtered', + 'num_threads': 2, + 'odom_topic': '/odom', + 'grid_resolution': 0.05, + 'grid_length': 10.0, + 'grid_patch_sizes': [3, 5], + 'grid_patch_change_distances': [1.0], + 'grid_occupied_inflate_size': 5, + 'grid_num_points_min_threshold': 5, + 'grid_num_points_raio_threshold': 0.1, + 'grid_var_threshold': 0.001, + 'grid_prob_prior': 0.5, + 'grid_prob_free': 0.1, + 'grid_prob_occupied': 0.9, + 'grid_prob_forget_rate': 0.2, + 'grid_prob_free_threshold': 0.15, + 'grid_prob_occupied_threshold': 0.55, + 'outlier_old_ground_threshold': 0.05, + 'outlier_los_ground_threshold': 0.05, + 'ground_estimate_angle_min': -0.614, # -35.2*M_PI/180 + 'ground_estimate_angle_max': 0.614, # 35.2*M_PI/180 + 'ground_slope_threshold': 0.262, # 15.0*M_PI/180 + 'ground_confidence_interpolate_decay': 0.5 }], - condition=IfCondition(PythonExpression([low_obstacle_detect_version, ' == 2'])) + condition=IfCondition(PythonExpression([low_obstacle_detect_version, " == 3"])) + ), + + Node( + package='grid_map_visualization', + executable='grid_map_visualization', + namespace='', + name='grid_map_visualization', + parameters=[configured_params], + condition=IfCondition(PythonExpression([low_obstacle_detect_version, " == 3 and '", publish_low_obstacle_ground, "' == 'true'"])) ), # others diff --git a/cabot_navigation2/package.xml b/cabot_navigation2/package.xml index f18b6050..fd2bcbb2 100644 --- a/cabot_navigation2/package.xml +++ b/cabot_navigation2/package.xml @@ -31,6 +31,8 @@ MIT geometry_msgs + grid_map_msgs + grid_map_ros nav2_costmap_2d nav2_core angles diff --git a/cabot_navigation2/params/nav2_params.yaml b/cabot_navigation2/params/nav2_params.yaml index c0508684..7e5625a5 100644 --- a/cabot_navigation2/params/nav2_params.yaml +++ b/cabot_navigation2/params/nav2_params.yaml @@ -46,6 +46,53 @@ people_vis: people_topic: "people" vis_topic: "people_vis" +grid_map_visualization: + ros__parameters: + grid_map_topic: /ground_filter_ground + grid_map_visualizations: [ground_filter_is_observed_enough_points, ground_filter_is_observed_ground_grid, ground_filter_is_free_grid, ground_filter_is_occupied_grid, ground_filter_observed_ground_z_points, ground_filter_valid_ground_z_points, ground_filter_estimated_ground_z_points, ground_filter_ground_confidence] + ground_filter_is_observed_enough_points: + type: occupancy_grid + params: + layer: is_observed_enough_points + data_min: 0.0 + data_max: 1.0 + ground_filter_is_observed_ground_grid: + type: occupancy_grid + params: + layer: is_observed_ground + data_min: 0.0 + data_max: 1.0 + ground_filter_is_free_grid: + type: occupancy_grid + params: + layer: is_free + data_min: 0.0 + data_max: 1.0 + ground_filter_is_occupied_grid: + type: occupancy_grid + params: + layer: is_occupied + data_min: 0.0 + data_max: 1.0 + ground_filter_observed_ground_z_points: + type: point_cloud + params: + layer: observed_ground_z + ground_filter_valid_ground_z_points: + type: point_cloud + params: + layer: valid_ground_z + ground_filter_estimated_ground_z_points: + type: point_cloud + params: + layer: estimated_ground_z + ground_filter_ground_confidence: + type: occupancy_grid + params: + layer: ground_confidence + data_min: 0.0 + data_max: 1.0 + amcl: ros__parameters: diff --git a/cabot_navigation2/src/abstract_ground_filter_node.cpp b/cabot_navigation2/src/abstract_ground_filter_node.cpp index a2b159f9..8b31925d 100644 --- a/cabot_navigation2/src/abstract_ground_filter_node.cpp +++ b/cabot_navigation2/src/abstract_ground_filter_node.cpp @@ -29,19 +29,26 @@ namespace cabot_navigation2 AbstractGroundFilterNode::AbstractGroundFilterNode(const std::string & filter_name, const rclcpp::NodeOptions & options) : rclcpp::Node(filter_name, options), target_frame_("livox_footprint"), + min_range_(0.05), + max_range_(5.0), + publish_debug_ground_(false), + output_debug_ground_topic_("/ground_filter_ground"), + ground_distance_threshold_(0.05), xfer_format_(POINTCLOUD2_XYZRTL), ignore_noise_(true), input_topic_("/livox/points"), output_ground_topic_("/livox/points_ground"), - output_filtered_topic_("/livox/points_filtered"), - min_range_(0.05), - max_range_(10.0) + output_filtered_topic_("/livox/points_filtered") { RCLCPP_INFO(get_logger(), "NodeClass Constructor"); RCLCPP_INFO(get_logger(), "Livox Point Cloud Node - %s", __FUNCTION__); target_frame_ = declare_parameter("target_frame", target_frame_); - + min_range_ = declare_parameter("min_range", min_range_); + max_range_ = declare_parameter("max_range", max_range_); + publish_debug_ground_ = declare_parameter("publish_debug_ground", publish_debug_ground_); + output_debug_ground_topic_ = declare_parameter("output_debug_ground_topic", output_debug_ground_topic_); + ground_distance_threshold_ = declare_parameter("ground_distance_threshold", ground_distance_threshold_); xfer_format_ = declare_parameter("xfer_format", xfer_format_); if (xfer_format_ != POINTCLOUD2_XYZRTL && xfer_format_ != POINTCLOUD2_XYZI) { RCLCPP_ERROR(get_logger(), "Invalid format, specify 0 (Livox pointcloud2 format, PointXYZRTL) or 2 (Standard pointcloud2 format, pcl :: PointXYZI)."); @@ -50,8 +57,6 @@ AbstractGroundFilterNode::AbstractGroundFilterNode(const std::string & filter_na input_topic_ = declare_parameter("input_topic", input_topic_); output_ground_topic_ = declare_parameter("output_ground_topic", output_ground_topic_); output_filtered_topic_ = declare_parameter("output_filtered_topic", output_filtered_topic_); - min_range_ = declare_parameter("min_range", min_range_); - max_range_ = declare_parameter("max_range", max_range_); tf_buffer_ = new tf2_ros::Buffer(get_clock()); tf_listener_ = new tf2_ros::TransformListener(*tf_buffer_, this); @@ -126,7 +131,7 @@ void AbstractGroundFilterNode::pointCloudCallback(const sensor_msgs::msg::PointC // filter ground point cloud pcl::PointCloud::Ptr ground_points(new pcl::PointCloud); pcl::PointCloud::Ptr filtered_points(new pcl::PointCloud); - filterGround(valid_transformed_normal_points, ground_points, filtered_points); + filterGround(input->header.stamp, valid_transformed_normal_points, ground_points, filtered_points); // publish ground point cloud sensor_msgs::msg::PointCloud2 ground_points_msg; diff --git a/cabot_navigation2/src/clip_ground_filter_node.cpp b/cabot_navigation2/src/clip_ground_filter_node.cpp index 0625faa1..c74d5c96 100644 --- a/cabot_navigation2/src/clip_ground_filter_node.cpp +++ b/cabot_navigation2/src/clip_ground_filter_node.cpp @@ -18,27 +18,65 @@ // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE // SOFTWARE. +#include + #include "cabot_navigation2/clip_ground_filter_node.hpp" namespace cabot_navigation2 { ClipGroundFilterNode::ClipGroundFilterNode(const rclcpp::NodeOptions & options) -: AbstractGroundFilterNode("clip_ground_filter_node", options), - clip_height_(0.05) +: AbstractGroundFilterNode("clip_ground_filter_node", options) { - clip_height_ = declare_parameter("clip_height", clip_height_); + if (publish_debug_ground_) { + debug_plane_pub_ = create_publisher(output_debug_ground_topic_, 1); + } } -void ClipGroundFilterNode::filterGround(const pcl::PointCloud::Ptr input, pcl::PointCloud::Ptr ground, pcl::PointCloud::Ptr filtered) +void ClipGroundFilterNode::filterGround( + const rclcpp::Time & time, const pcl::PointCloud::Ptr input, pcl::PointCloud::Ptr ground, + pcl::PointCloud::Ptr filtered) { - for (const auto & p : input->points) { - if (p.z > clip_height_) { - filtered->push_back(p); - } else if (abs(p.z) <= clip_height_) { - ground->push_back(p); + pcl::PointIndices ground_indices; + pcl::PointIndices filtered_indices; + for (unsigned int i = 0; i < input->points.size(); i++) { + const auto & p = input->points[i]; + if (p.z > ground_distance_threshold_) { + filtered_indices.indices.push_back(i); + } else if (abs(p.z) <= ground_distance_threshold_) { + ground_indices.indices.push_back(i); } } + + pcl::ExtractIndices ground_extract_indices; + ground_extract_indices.setIndices(pcl::make_shared(ground_indices)); + ground_extract_indices.setInputCloud(input); + ground_extract_indices.filter(*ground); + + pcl::ExtractIndices filtered_extract_indices; + filtered_extract_indices.setIndices(pcl::make_shared(filtered_indices)); + filtered_extract_indices.setInputCloud(input); + filtered_extract_indices.filter(*filtered); + + if (publish_debug_ground_) { + visualization_msgs::msg::Marker plane_marker; + plane_marker.header.frame_id = target_frame_; + plane_marker.ns = "clip_plane"; + plane_marker.id = 0; + plane_marker.type = visualization_msgs::msg::Marker::CUBE; + plane_marker.action = visualization_msgs::msg::Marker::MODIFY; + plane_marker.pose.position.x = 0; + plane_marker.pose.position.y = 0; + plane_marker.pose.position.z = ground_distance_threshold_; + plane_marker.scale.x = max_range_ * 2.0; + plane_marker.scale.y = max_range_ * 2.0; + plane_marker.scale.z = 0.0; + plane_marker.color.r = 1.0f; + plane_marker.color.g = 0.0f; + plane_marker.color.b = 0.0f; + plane_marker.color.a = 0.5; + debug_plane_pub_->publish(plane_marker); + } } } // namespace cabot_navigation2 diff --git a/cabot_navigation2/src/grid_map_ground_filter_node.cpp b/cabot_navigation2/src/grid_map_ground_filter_node.cpp new file mode 100644 index 00000000..06f04084 --- /dev/null +++ b/cabot_navigation2/src/grid_map_ground_filter_node.cpp @@ -0,0 +1,657 @@ +// Copyright (c) 2024 Carnegie Mellon University, IBM Corporation, and others +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#include +#include +#include + +#include +#include +#include +#include + +#include "cabot_navigation2/grid_map_ground_filter_node.hpp" + +namespace cabot_navigation2 +{ + +// https://www.livoxtech.com/mid-70/specs +const int GridMapGroundFilterNode::livox_num_points_ = 100000; +const float GridMapGroundFilterNode::lixox_tan_fov_angle_ = std::tan(M_PI * (70.4 / 2.0) / 180.0); + +GridMapGroundFilterNode::GridMapGroundFilterNode(const rclcpp::NodeOptions & options) +: AbstractGroundFilterNode("grid_map_ground_filter_node", options), + num_threads_(2), + odom_topic_("/odom"), + grid_resolution_(0.05), + grid_length_(10.0), + grid_patch_sizes_({3, 5}), + grid_patch_change_distances_({1.0}), + grid_occupied_inflate_size_(5), + grid_num_points_min_threshold_(5), + grid_num_points_raio_threshold_(0.1), + grid_var_threshold_(0.001), + grid_prob_prior_(0.5), + grid_prob_free_(0.1), + grid_prob_occupied_(0.9), + grid_prob_forget_rate_(0.2), + grid_prob_free_threshold_(0.15), + grid_prob_occupied_threshold_(0.55), + outlier_old_ground_threshold_(0.05), + outlier_los_ground_threshold_(0.05), + ground_estimate_angle_min_(-0.614), + ground_estimate_angle_max_(0.614), + ground_slope_threshold_(0.262), + ground_confidence_interpolate_decay_(0.5) +{ + num_threads_ = declare_parameter("num_threads", num_threads_); + odom_topic_ = declare_parameter("odom_topic", odom_topic_); + grid_resolution_ = declare_parameter("grid_resolution", grid_resolution_); + grid_length_ = declare_parameter("grid_length", grid_length_); + grid_patch_sizes_ = declare_parameter("grid_patch_sizes", grid_patch_sizes_); + grid_patch_change_distances_ = declare_parameter("grid_patch_change_distances", grid_patch_change_distances_); + if (grid_patch_sizes_.size() - 1 != grid_patch_change_distances_.size()) { + RCLCPP_ERROR(get_logger(), "Invalid grid patch parameters, set length of patch sizes should be one value larger than length of patch change distances."); + } + grid_occupied_inflate_size_ = declare_parameter("grid_occupied_inflate_size", grid_occupied_inflate_size_); + grid_num_points_min_threshold_ = declare_parameter("grid_num_points_min_threshold", grid_num_points_min_threshold_); + grid_num_points_raio_threshold_ = declare_parameter("grid_num_points_raio_threshold", grid_num_points_raio_threshold_); + grid_var_threshold_ = declare_parameter("grid_var_threshold", grid_var_threshold_); + grid_prob_prior_ = declare_parameter("grid_prob_prior", grid_prob_prior_); + grid_prob_free_ = declare_parameter("grid_prob_free", grid_prob_free_); + grid_prob_occupied_ = declare_parameter("grid_prob_occupied", grid_prob_occupied_); + grid_prob_forget_rate_ = declare_parameter("grid_prob_forget_rate", grid_prob_forget_rate_); + grid_prob_free_threshold_ = declare_parameter("grid_prob_free_threshold", grid_prob_free_threshold_); + grid_prob_occupied_threshold_ = declare_parameter("grid_prob_occupied_threshold", grid_prob_occupied_threshold_); + outlier_old_ground_threshold_ = declare_parameter("outlier_old_ground_threshold", outlier_old_ground_threshold_); + outlier_los_ground_threshold_ = declare_parameter("outlier_los_ground_threshold", outlier_los_ground_threshold_); + ground_estimate_angle_min_ = declare_parameter("ground_estimate_angle_min", ground_estimate_angle_min_); + ground_estimate_angle_max_ = declare_parameter("ground_estimate_angle_max", ground_estimate_angle_max_); + ground_slope_threshold_ = declare_parameter("ground_slope_threshold", ground_slope_threshold_); + ground_confidence_interpolate_decay_ = declare_parameter("ground_confidence_interpolate_decay", ground_confidence_interpolate_decay_); + + for (auto patch_size : grid_patch_sizes_) { + grid_half_patch_sizes_.push_back(std::floor(patch_size / 2)); + } + log_odds_prior_ = std::log(grid_prob_prior_ / (1.0 - grid_prob_prior_)); + log_odds_free_ = std::log(grid_prob_free_ / (1.0 - grid_prob_free_)); + log_odds_occupied_ = std::log(grid_prob_occupied_ / (1.0 - grid_prob_occupied_)); + ground_estimate_radius_ = grid_length_ / 2.0; + + odom_sub_ = create_subscription(odom_topic_, 10, std::bind(&GridMapGroundFilterNode::odomCallback, this, std::placeholders::_1)); + + if (publish_debug_ground_) { + debug_outlier_pointcloud_pub_ = create_publisher(output_debug_ground_topic_ + "/outlier_pointcloud", rclcpp::QoS(1).transient_local()); + debug_grid_map_pub_ = create_publisher(output_debug_ground_topic_, rclcpp::QoS(1).transient_local()); + } +} + +void GridMapGroundFilterNode::odomCallback(const nav_msgs::msg::Odometry::SharedPtr input) +{ + grid_map::Position input_pos(input->pose.pose.position.x, input->pose.pose.position.y); + + std::unique_lock lock(grid_map_mutex_); + + if (!grid_map_ptr_) { + grid_map_ptr_ = + std::make_shared>( + {"num_points", "mean_points_z", "m2_points_z", "var_points_z", "is_observed_enough_points", "is_observed_ground", "log_odds", "occupancy", "is_free", "is_occupied", + "observed_ground_z", "valid_ground_z", "estimated_ground_z", "ground_confidence"}); + grid_map_ptr_->setFrameId("odom"); + grid_map_ptr_->setGeometry(grid_map::Length(grid_length_, grid_length_), grid_resolution_, input_pos); + grid_map_ptr_->move(input_pos); + (*grid_map_ptr_)["num_points"].setZero(); + (*grid_map_ptr_)["mean_points_z"].setZero(); + (*grid_map_ptr_)["m2_points_z"].setZero(); + (*grid_map_ptr_)["var_points_z"].setZero(); + (*grid_map_ptr_)["is_observed_enough_points"].setZero(); + (*grid_map_ptr_)["is_observed_ground"].setZero(); + (*grid_map_ptr_)["log_odds"].setConstant(log_odds_prior_); + (*grid_map_ptr_)["occupancy"].setConstant(grid_prob_prior_); + (*grid_map_ptr_)["is_free"].setZero(); + (*grid_map_ptr_)["is_occupied"].setZero(); + (*grid_map_ptr_)["observed_ground_z"].setConstant(std::numeric_limits::max()); + (*grid_map_ptr_)["valid_ground_z"].setConstant(std::numeric_limits::max()); + (*grid_map_ptr_)["estimated_ground_z"].setConstant(std::numeric_limits::max()); + (*grid_map_ptr_)["ground_confidence"].setZero(); + return; + } + + std::vector new_regions; + grid_map_ptr_->move(input_pos, new_regions); + + for (auto region : new_regions) { + for (auto iterator = grid_map::SubmapIterator(*grid_map_ptr_, region); !iterator.isPastEnd(); ++iterator) { + grid_map::Index gmap_index = *iterator; + + grid_map_ptr_->at("num_points", gmap_index) = 0.0; + grid_map_ptr_->at("mean_points_z", gmap_index) = 0.0; + grid_map_ptr_->at("m2_points_z", gmap_index) = 0.0; + grid_map_ptr_->at("var_points_z", gmap_index) = 0.0; + grid_map_ptr_->at("is_observed_enough_points", gmap_index) = 0.0; + grid_map_ptr_->at("is_observed_ground", gmap_index) = 0.0; + grid_map_ptr_->at("log_odds", gmap_index) = log_odds_prior_; + grid_map_ptr_->at("occupancy", gmap_index) = grid_prob_prior_; + grid_map_ptr_->at("is_free", gmap_index) = 0.0; + grid_map_ptr_->at("is_occupied", gmap_index) = 0.0; + grid_map_ptr_->at("observed_ground_z", gmap_index) = std::numeric_limits::max(); + grid_map_ptr_->at("valid_ground_z", gmap_index) = std::numeric_limits::max(); + grid_map_ptr_->at("estimated_ground_z", gmap_index) = std::numeric_limits::max(); + grid_map_ptr_->at("ground_confidence", gmap_index) = 0.0; + } + } + + grid_map_ptr_->convertToDefaultStartIndex(); +} + +int GridMapGroundFilterNode::calcLivoxGridEstimatedNumPoints(float distance, float resolution) +{ + const float fov_radius = distance * lixox_tan_fov_angle_; + const float fov_area = M_PI * std::pow(fov_radius, 2.0); + return livox_num_points_ * std::pow(resolution, 2.0) / fov_area; +} + +bool GridMapGroundFilterNode::isVisibleAngle(const grid_map::Position & check_position, const grid_map::Position & sensor_position, double sensor_yaw) +{ + float angle = std::atan2(check_position.y() - sensor_position.y(), check_position.x() - sensor_position.x()) - sensor_yaw; + if (angle < -M_PI) { + angle += 2.0 * M_PI; + } else if (angle > M_PI) { + angle -= 2.0 * M_PI; + } + + if ((angle >= ground_estimate_angle_min_) && (angle <= ground_estimate_angle_max_)) { + return true; + } else { + return false; + } +} + +void GridMapGroundFilterNode::filterGround( + const rclcpp::Time & time, const pcl::PointCloud::Ptr input, pcl::PointCloud::Ptr ground, + pcl::PointCloud::Ptr filtered) +{ + // auto t0 = std::chrono::system_clock::now(); + + // transform point cloud to map frame + geometry_msgs::msg::PoseStamped gmap_origin_pose; + pcl::PointCloud::Ptr transformed_input(new pcl::PointCloud); + try { + geometry_msgs::msg::TransformStamped tf_stamped = tf_buffer_->lookupTransform( + "odom", target_frame_, + rclcpp::Time(0), rclcpp::Duration(std::chrono::duration(1.0))); + + pcl_ros::transformPointCloud(*input, *transformed_input, tf_stamped); + + geometry_msgs::msg::PoseStamped origin_pose; + origin_pose.header.frame_id = target_frame_; + origin_pose.header.stamp = time; + origin_pose.pose.position.x = 0.0; + origin_pose.pose.position.y = 0.0; + origin_pose.pose.position.z = 0.0; + origin_pose.pose.orientation.x = 0.0; + origin_pose.pose.orientation.y = 0.0; + origin_pose.pose.orientation.z = 0.0; + origin_pose.pose.orientation.w = 1.0; + tf2::doTransform(origin_pose, gmap_origin_pose, tf_stamped); + } catch (tf2::TransformException & ex) { + RCLCPP_WARN(get_logger(), "%s", ex.what()); + return; + } + + // auto t1 = std::chrono::system_clock::now(); + // auto ms1 = std::chrono::duration_cast(t1 - t0); + // RCLCPP_INFO(get_logger(), "duration to find transform odom: %ld ms", ms1.count()); + + std::unique_lock lock(grid_map_mutex_); + + if (!grid_map_ptr_) { + RCLCPP_WARN(get_logger(), "grid map is not ready"); + return; + } + + // auto t2 = std::chrono::system_clock::now(); + // auto ms2 = std::chrono::duration_cast(t2 - t1); + // RCLCPP_INFO(get_logger(), "duration to lock map: %ld ms", ms2.count()); + + grid_map::Position gmap_origin_pose_position(gmap_origin_pose.pose.position.x, gmap_origin_pose.pose.position.y); + if (!grid_map_ptr_->isInside(gmap_origin_pose_position)) { + RCLCPP_WARN(get_logger(), "robot is not inside grid map"); + return; + } + + // auto t3 = std::chrono::system_clock::now(); + // auto ms3 = std::chrono::duration_cast(t3 - t2); + // RCLCPP_INFO(get_logger(), "duration to check inside map: %ld ms", ms3.count()); + + // collect visible area grid map indices in spiral order + std::vector visible_grid_spiral_indices; + std::vector invisible_grid_spiral_indices; + grid_map::Index gmap_origin_pose_index; + grid_map_ptr_->getIndex(gmap_origin_pose_position, gmap_origin_pose_index); + const double gmap_origin_pose_yaw = tf2::getYaw(gmap_origin_pose.pose.orientation); + for (auto iterator = grid_map::SpiralIterator(*grid_map_ptr_, gmap_origin_pose_position, ground_estimate_radius_); !iterator.isPastEnd(); ++iterator) { + grid_map::Index gmap_index = *iterator; + + if ((gmap_index(0) == gmap_origin_pose_index(0)) && (gmap_index(1) == gmap_origin_pose_index(1))) { + // collect sensor position as invisible area + invisible_grid_spiral_indices.push_back(gmap_index); + } else { + grid_map::Position gmap_position; + grid_map_ptr_->getPosition(gmap_index, gmap_position); + + if (grid_map_ptr_->isInside(gmap_position)) { + if (isVisibleAngle(gmap_position, gmap_origin_pose_position, gmap_origin_pose_yaw)) { + visible_grid_spiral_indices.push_back(gmap_index); + } else { + invisible_grid_spiral_indices.push_back(gmap_index); + } + } + } + } + + // collect patch sizes for visible areas + std::vector visible_grid_spiral_patch_sizes; + std::vector visible_grid_spiral_half_patch_sizes; + visible_grid_spiral_patch_sizes.reserve(visible_grid_spiral_indices.size()); + visible_grid_spiral_half_patch_sizes.reserve(visible_grid_spiral_indices.size()); + const float gmap_resolution = grid_map_ptr_->getResolution(); + for (unsigned int i = 0, j = 0; i < visible_grid_spiral_indices.size(); i++) { + grid_map::Index gmap_index = visible_grid_spiral_indices[i]; + float gmap_distance = std::hypot(gmap_index(0) - gmap_origin_pose_index(0), gmap_index(1) - gmap_origin_pose_index(1)) * gmap_resolution; + if ((gmap_distance > grid_patch_change_distances_[j]) && (j < grid_patch_sizes_.size() - 1)) { + j += 1; + } + visible_grid_spiral_patch_sizes.push_back(grid_patch_sizes_[j]); + visible_grid_spiral_half_patch_sizes.push_back(grid_half_patch_sizes_[j]); + } + + // auto t4 = std::chrono::system_clock::now(); + // auto ms4 = std::chrono::duration_cast(t4 - t3); + // RCLCPP_INFO(get_logger(), "duration to collect grid map indices in spiral order: %ld ms", ms4.count()); + + // add points to grid map + std::vector::Ptr>> grid_map_pointcloud; + const grid_map::Size gmap_size = grid_map_ptr_->getSize(); + grid_map_pointcloud.resize(gmap_size(0)); +#pragma omp parallel for num_threads(num_threads_) schedule(dynamic, 1) + for (int row = 0; row < gmap_size(0); row++) { + grid_map_pointcloud[row].resize(gmap_size(1)); + for (int col = 0; col < gmap_size(1); col++) { + grid_map_pointcloud[row][col].reset(new pcl::PointCloud); + } + } + pcl::PointCloud::Ptr outlier_points(new pcl::PointCloud); + for (const auto & p : transformed_input->points) { + const auto & p_pos = grid_map::Position(p.x, p.y); + if (!grid_map_ptr_->isInside(p_pos)) { + continue; + } + + grid_map::Index p_index; + grid_map_ptr_->getIndex(p_pos, p_index); + + // check outlier by previous ground height in line of sight + bool is_outlier = false; + if (p.z < (*grid_map_ptr_)["estimated_ground_z"](p_index(0), p_index(1)) - outlier_old_ground_threshold_) { + // point is enough below previous estimated ground height + float z_slope = (p.z - gmap_origin_pose.pose.position.z) / std::hypot(p.x - gmap_origin_pose.pose.position.x, p.y - gmap_origin_pose.pose.position.y); + + auto los_iterator = grid_map::LineIterator(*grid_map_ptr_, gmap_origin_pose_position, p_pos); + ++los_iterator; + for (; !los_iterator.isPastEnd(); ++los_iterator) { + grid_map::Index los_gmap_index = *los_iterator; + + if ((*grid_map_ptr_)["valid_ground_z"](los_gmap_index(0), los_gmap_index(1)) != std::numeric_limits::max()) { + float los_dist = std::hypot(los_gmap_index(0) - gmap_origin_pose_index(0), los_gmap_index(1) - gmap_origin_pose_index(1)) * gmap_resolution; + float los_z = gmap_origin_pose.pose.position.z + z_slope * los_dist; + if (los_z < (*grid_map_ptr_)["valid_ground_z"](los_gmap_index(0), los_gmap_index(1)) - outlier_los_ground_threshold_) { + // line of sight is enough below previous validated ground height + is_outlier = true; + break; + } + } + } + } + + if (!is_outlier) { + grid_map_pointcloud[p_index(0)][p_index(1)]->push_back(p); + } else { + outlier_points->push_back(p); + } + } + + // auto t5 = std::chrono::system_clock::now(); + // auto ms5 = std::chrono::duration_cast(t5 - t4); + // RCLCPP_INFO(get_logger(), "duration to add points: %ld ms", ms5.count()); + + // initialize grid map except log odds + (*grid_map_ptr_)["num_points"].setZero(); + (*grid_map_ptr_)["mean_points_z"].setZero(); + (*grid_map_ptr_)["m2_points_z"].setZero(); + (*grid_map_ptr_)["var_points_z"].setZero(); + (*grid_map_ptr_)["is_observed_enough_points"].setZero(); + (*grid_map_ptr_)["is_observed_ground"].setZero(); + (*grid_map_ptr_)["occupancy"].setConstant(grid_prob_prior_); + (*grid_map_ptr_)["is_free"].setZero(); + (*grid_map_ptr_)["is_occupied"].setZero(); + (*grid_map_ptr_)["observed_ground_z"].setConstant(std::numeric_limits::max()); + (*grid_map_ptr_)["valid_ground_z"].setConstant(std::numeric_limits::max()); + (*grid_map_ptr_)["estimated_ground_z"].setConstant(std::numeric_limits::max()); + (*grid_map_ptr_)["ground_confidence"].setZero(); + + // auto t6 = std::chrono::system_clock::now(); + // auto ms6 = std::chrono::duration_cast(t6 - t5); + // RCLCPP_INFO(get_logger(), "duration to initialize map: %ld ms", ms6.count()); + + // calculate mean, variance in each grid cell +#pragma omp parallel for num_threads(num_threads_) schedule(dynamic, 1) + for (auto iterator = visible_grid_spiral_indices.begin(); iterator != visible_grid_spiral_indices.end(); ++iterator) { + grid_map::Index gmap_index = *iterator; + + for (const auto & p : grid_map_pointcloud[gmap_index(0)][gmap_index(1)]->points) { + const float num_points = (*grid_map_ptr_)["num_points"](gmap_index(0), gmap_index(1)); + const float mean_points_z = (*grid_map_ptr_)["mean_points_z"](gmap_index(0), gmap_index(1)); + const float m2_points_z = (*grid_map_ptr_)["m2_points_z"](gmap_index(0), gmap_index(1)); + + const float new_num_points = num_points + 1; + const float new_mean_points_z = mean_points_z + (p.z - mean_points_z) / new_num_points; + (*grid_map_ptr_)["num_points"](gmap_index(0), gmap_index(1)) = new_num_points; + (*grid_map_ptr_)["mean_points_z"](gmap_index(0), gmap_index(1)) = new_mean_points_z; + (*grid_map_ptr_)["m2_points_z"](gmap_index(0), gmap_index(1)) = m2_points_z + (p.z - new_mean_points_z) * (p.z - mean_points_z); + } + const float num_points = (*grid_map_ptr_)["num_points"](gmap_index(0), gmap_index(1)); + if (num_points > 1) { + (*grid_map_ptr_)["var_points_z"](gmap_index(0), gmap_index(1)) = (*grid_map_ptr_)["m2_points_z"](gmap_index(0), gmap_index(1)) / (num_points - 1); + } + } + + // auto t7 = std::chrono::system_clock::now(); + // auto ms7 = std::chrono::duration_cast(t7 - t6); + // RCLCPP_INFO(get_logger(), "duration to calculate mean, variance in each grid cell: %ld ms", ms7.count()); + + // update occupancy in visible area +#pragma omp parallel for num_threads(num_threads_) schedule(dynamic, 1) + for (unsigned int i = 0; i < visible_grid_spiral_indices.size(); i++) { + grid_map::Index gmap_index = visible_grid_spiral_indices[i]; + + const float grid_distance = std::hypot(gmap_index(0) - (gmap_size(0) / 2.0), gmap_index(1) - (gmap_size(1) / 2.0)) * gmap_resolution; + + const int grid_estimated_num_points = calcLivoxGridEstimatedNumPoints(grid_distance, gmap_resolution); + const int grid_num_points_threshold = std::max(static_cast(grid_num_points_raio_threshold_ * grid_estimated_num_points), grid_num_points_min_threshold_); + + bool is_enough_points = false; + float var_points_z = 0.0; + int num_points_grid = (*grid_map_ptr_)["num_points"](gmap_index(0), gmap_index(1)); + if (num_points_grid > grid_num_points_threshold) { + // observe enough points in the grid, use points only in the grid + is_enough_points = true; + var_points_z = (*grid_map_ptr_)["var_points_z"](gmap_index(0), gmap_index(1)); + } else { + // do not observe enough points in the grid, use points in patch + const int patch_size = visible_grid_spiral_patch_sizes[i]; + const int half_patch_size = visible_grid_spiral_half_patch_sizes[i]; + const auto & num_points_patch = + (*grid_map_ptr_)["num_points"].block(gmap_index(0) - half_patch_size, gmap_index(1) - half_patch_size, patch_size, patch_size); + const auto & var_points_z_patch = + (*grid_map_ptr_)["var_points_z"].block(gmap_index(0) - half_patch_size, gmap_index(1) - half_patch_size, patch_size, patch_size); + + const float & num_points_patch_sum = num_points_patch.sum(); + if (num_points_patch_sum > grid_num_points_threshold) { + is_enough_points = true; + var_points_z = var_points_z_patch.cwiseProduct(num_points_patch).sum() / num_points_patch_sum; + } + } + + if (is_enough_points) { + grid_map_ptr_->at("is_observed_enough_points", gmap_index) = 1.0; + if (var_points_z > grid_var_threshold_) { + // update log odds as occupied area + grid_map_ptr_->at("log_odds", gmap_index) = grid_map_ptr_->at("log_odds", gmap_index) - log_odds_prior_ + log_odds_occupied_; + } else { + grid_map_ptr_->at("is_observed_ground", gmap_index) = 1.0; + // update log odds as free area + grid_map_ptr_->at("log_odds", gmap_index) = grid_map_ptr_->at("log_odds", gmap_index) - log_odds_prior_ + log_odds_free_; + } + } else { + // do not observe enough point, forget previous updates of log odds + grid_map_ptr_->at("log_odds", gmap_index) = log_odds_prior_ + (grid_map_ptr_->at("log_odds", gmap_index) - log_odds_prior_) * (1.0 - grid_prob_forget_rate_); + } + + grid_map_ptr_->at("occupancy", gmap_index) = 1.0 - 1.0 / (1.0 + std::exp(grid_map_ptr_->at("log_odds", gmap_index))); + grid_map_ptr_->at("is_free", gmap_index) = (grid_map_ptr_->at("occupancy", gmap_index) < grid_prob_free_threshold_) ? 1.0 : 0.0; + grid_map_ptr_->at("is_occupied", gmap_index) = (grid_map_ptr_->at("occupancy", gmap_index) > grid_prob_occupied_threshold_) ? 1.0 : 0.0; + } + + // update occupancy in invisible area +#pragma omp parallel for num_threads(num_threads_) schedule(dynamic, 1) + for (auto iterator = invisible_grid_spiral_indices.begin(); iterator != invisible_grid_spiral_indices.end(); ++iterator) { + grid_map::Index gmap_index = *iterator; + + // invisible area, forget previous updates of log odds + grid_map_ptr_->at("log_odds", gmap_index) = log_odds_prior_ + (grid_map_ptr_->at("log_odds", gmap_index) - log_odds_prior_) * (1.0 - grid_prob_forget_rate_); + + grid_map_ptr_->at("occupancy", gmap_index) = 1.0 - 1.0 / (1.0 + std::exp(grid_map_ptr_->at("log_odds", gmap_index))); + grid_map_ptr_->at("is_free", gmap_index) = (grid_map_ptr_->at("occupancy", gmap_index) < grid_prob_free_threshold_) ? 1.0 : 0.0; + grid_map_ptr_->at("is_occupied", gmap_index) = (grid_map_ptr_->at("occupancy", gmap_index) > grid_prob_occupied_threshold_) ? 1.0 : 0.0; + } + + // auto t8 = std::chrono::system_clock::now(); + // auto ms8 = std::chrono::duration_cast(t8 - t7); + // RCLCPP_INFO(get_logger(), "duration to update free area: %ld ms", ms8.count()); + + // inflate binary occupied area + cv::Mat is_occupied_mat; + grid_map::GridMapCvConverter::toImage(*grid_map_ptr_, "is_occupied", CV_8UC1, 0.0, 1.0, is_occupied_mat); + + cv::Mat is_inflate_occupied_mat; + cv::dilate(is_occupied_mat, is_inflate_occupied_mat, cv::Mat::ones(grid_occupied_inflate_size_, grid_occupied_inflate_size_, CV_8UC1), cv::Point(-1, -1), 1); + grid_map::GridMapCvConverter::addLayerFromImage(is_inflate_occupied_mat, "is_occupied", *grid_map_ptr_, 0.0, 1.0); + + // auto t9 = std::chrono::system_clock::now(); + // auto ms9 = std::chrono::duration_cast(t9 - t8); + // RCLCPP_INFO(get_logger(), "duration to inflate occupied area: %ld ms", ms9.count()); + + // set ground height for robot position + grid_map_ptr_->at("observed_ground_z", gmap_origin_pose_index) = gmap_origin_pose.pose.position.z; + grid_map_ptr_->at("valid_ground_z", gmap_origin_pose_index) = gmap_origin_pose.pose.position.z; + grid_map_ptr_->at("estimated_ground_z", gmap_origin_pose_index) = gmap_origin_pose.pose.position.z; + grid_map_ptr_->at("ground_confidence", gmap_origin_pose_index) = (1.0 - grid_map_ptr_->at("occupancy", gmap_origin_pose_index)); + + // calculate ground height for free observed area +#pragma omp parallel for num_threads(num_threads_) schedule(dynamic, 1) + for (unsigned int i = 0; i < visible_grid_spiral_indices.size(); i++) { + grid_map::Index gmap_index = visible_grid_spiral_indices[i]; + + int num_observed_ground_z = 0; + float sum_observed_ground_z = 0.0; + if ((grid_map_ptr_->at("is_observed_ground", gmap_index) && (grid_map_ptr_->at("is_free", gmap_index)) && (!grid_map_ptr_->at("is_occupied", gmap_index)))) { + // calculate ground height by averaging minimum point height in the patch + const int patch_size = visible_grid_spiral_patch_sizes[i]; + const int half_patch_size = visible_grid_spiral_half_patch_sizes[i]; + const auto & is_observed_ground_patch = + (*grid_map_ptr_)["is_observed_ground"].block(gmap_index(0) - half_patch_size, gmap_index(1) - half_patch_size, patch_size, patch_size); + const auto & is_free_patch = + (*grid_map_ptr_)["is_free"].block(gmap_index(0) - half_patch_size, gmap_index(1) - half_patch_size, patch_size, patch_size); + const auto & is_occupied_patch = + (*grid_map_ptr_)["is_occupied"].block(gmap_index(0) - half_patch_size, gmap_index(1) - half_patch_size, patch_size, patch_size); + const auto & mean_points_z_patch = + (*grid_map_ptr_)["mean_points_z"].block(gmap_index(0) - half_patch_size, gmap_index(1) - half_patch_size, patch_size, patch_size); + const auto & num_points_patch = + (*grid_map_ptr_)["num_points"].block(gmap_index(0) - half_patch_size, gmap_index(1) - half_patch_size, patch_size, patch_size); + for (int row = 0; row < is_free_patch.rows(); row++) { + for (int col = 0; col < is_free_patch.cols(); col++) { + if ((is_observed_ground_patch(row, col) == 1.0) && (is_free_patch(row, col) == 1.0) && (is_occupied_patch(row, col) == 0.0)) { + sum_observed_ground_z += mean_points_z_patch(row, col) * num_points_patch(row, col); + num_observed_ground_z += num_points_patch(row, col); + } + } + } + } + + if (num_observed_ground_z > 0) { + grid_map_ptr_->at("observed_ground_z", gmap_index) = sum_observed_ground_z / num_observed_ground_z; + } + } + + // auto t10 = std::chrono::system_clock::now(); + // auto ms10 = std::chrono::duration_cast(t10 - t9); + // RCLCPP_INFO(get_logger(), "duration to calculate observed ground height: %ld ms", ms10.count()); + + // validate ground height for free observed area from center of grid map in spiral order + for (auto iterator = visible_grid_spiral_indices.begin(); iterator != visible_grid_spiral_indices.end(); ++iterator) { + grid_map::Index gmap_index = *iterator; + + float observed_ground_z = grid_map_ptr_->at("observed_ground_z", gmap_index); + if (observed_ground_z != std::numeric_limits::max()) { + // check if ground height is valid by calculating slope to the closest observed ground height in line of sight + bool is_valid = false; + grid_map::Position gmap_position; + grid_map_ptr_->getPosition(gmap_index, gmap_position); + + auto los_iterator = grid_map::LineIterator(*grid_map_ptr_, gmap_position, gmap_origin_pose_position); + ++los_iterator; + for (; !los_iterator.isPastEnd(); ++los_iterator) { + grid_map::Index los_gmap_index = *los_iterator; + + float los_valid_ground_z = grid_map_ptr_->at("valid_ground_z", los_gmap_index); + if (los_valid_ground_z != std::numeric_limits::max()) { + grid_map::Position los_gmap_position; + grid_map_ptr_->getPosition(los_gmap_index, los_gmap_position); + double dist = std::hypot(gmap_position.x() - los_gmap_position.x(), gmap_position.y() - los_gmap_position.y()); + double slope = std::atan2(observed_ground_z - los_valid_ground_z, dist); + if (std::abs(slope) < ground_slope_threshold_) { + is_valid = true; + } + break; + } + } + if (is_valid) { + grid_map_ptr_->at("valid_ground_z", gmap_index) = observed_ground_z; + grid_map_ptr_->at("estimated_ground_z", gmap_index) = observed_ground_z; + grid_map_ptr_->at("ground_confidence", gmap_index) = (1.0 - grid_map_ptr_->at("occupancy", gmap_index)); + } + } + } + + // auto t11 = std::chrono::system_clock::now(); + // auto ms11 = std::chrono::duration_cast(t11 - t10); + // RCLCPP_INFO(get_logger(), "duration to validate observed ground height: %ld ms", ms11.count()); + + // estimate ground height from center of grid map in spiral order + for (unsigned int i = 0; i < visible_grid_spiral_indices.size(); i++) { + grid_map::Index gmap_index = visible_grid_spiral_indices[i]; + + if ((*grid_map_ptr_)["estimated_ground_z"](gmap_index(0), gmap_index(1)) == std::numeric_limits::max()) { + // estimate ground height by surrounding free area + const int patch_size = visible_grid_spiral_patch_sizes[i]; + const int half_patch_size = visible_grid_spiral_half_patch_sizes[i]; + const auto & estimated_ground_z_patch = + (*grid_map_ptr_)["estimated_ground_z"].block(gmap_index(0) - half_patch_size, gmap_index(1) - half_patch_size, patch_size, patch_size); + const auto & ground_confidence_patch = + (*grid_map_ptr_)["ground_confidence"].block(gmap_index(0) - half_patch_size, gmap_index(1) - half_patch_size, patch_size, patch_size); + + // calculate weighted average of estimated ground height + float sum_estimated_ground_z = 0.0; + float sum_ground_confidence = 0.0; + int num_averaged_grid = 0; + for (int row = 0; row < estimated_ground_z_patch.rows(); row++) { + for (int col = 0; col < estimated_ground_z_patch.cols(); col++) { + if ((estimated_ground_z_patch(row, col) != std::numeric_limits::max()) && (ground_confidence_patch(row, col) > 0.0)) { + sum_estimated_ground_z += estimated_ground_z_patch(row, col) * ground_confidence_patch(row, col); + sum_ground_confidence += ground_confidence_patch(row, col); + num_averaged_grid += 1; + } + } + } + + if (sum_ground_confidence > 0) { + (*grid_map_ptr_)["estimated_ground_z"](gmap_index(0), gmap_index(1)) = sum_estimated_ground_z / sum_ground_confidence; + (*grid_map_ptr_)["ground_confidence"](gmap_index(0), gmap_index(1)) = (sum_ground_confidence / num_averaged_grid) * ground_confidence_interpolate_decay_; + } + } + } + + // auto t12 = std::chrono::system_clock::now(); + // auto ms12 = std::chrono::duration_cast(t12 - t11); + // RCLCPP_INFO(get_logger(), "duration to estimate unobserved ground height: %ld ms", ms12.count()); + + // filter point cloud by intepolated ground height + pcl::PointIndices ground_indices; + pcl::PointIndices filtered_indices; + for (unsigned int i = 0; i < transformed_input->points.size(); i++) { + const auto & p = transformed_input->points[i]; + const auto & p_pos = grid_map::Position(p.x, p.y); + if (!grid_map_ptr_->isInside(p_pos)) { + continue; + } + + grid_map::Index p_index; + grid_map_ptr_->getIndex(p_pos, p_index); + + // ignore points in sparse areas to remove noise + if (grid_map_ptr_->at("is_observed_enough_points", p_index)) { + const float estimated_ground_z = (*grid_map_ptr_)["estimated_ground_z"](p_index(0), p_index(1)); + const float signed_dist = p.z - estimated_ground_z; + if (signed_dist > ground_distance_threshold_) { + filtered_indices.indices.push_back(i); + } else if (std::abs(signed_dist) <= ground_distance_threshold_) { + ground_indices.indices.push_back(i); + } + } + } + + pcl::ExtractIndices ground_extract_indices; + ground_extract_indices.setIndices(pcl::make_shared(ground_indices)); + ground_extract_indices.setInputCloud(input); + ground_extract_indices.filter(*ground); + + pcl::ExtractIndices filtered_extract_indices; + filtered_extract_indices.setIndices(pcl::make_shared(filtered_indices)); + filtered_extract_indices.setInputCloud(input); + filtered_extract_indices.filter(*filtered); + + // auto t13 = std::chrono::system_clock::now(); + // auto ms13 = std::chrono::duration_cast(t13 - t12); + // RCLCPP_INFO(get_logger(), "duration to filter points: %ld ms", ms13.count()); + + // auto ms_all = std::chrono::duration_cast(t13 - t0); + // RCLCPP_INFO(get_logger(), "duration for all filter ground steps: %ld ms", ms_all.count()); + + if (publish_debug_ground_) { + sensor_msgs::msg::PointCloud2 outlier_points_msg; + pcl::toROSMsg(*outlier_points, outlier_points_msg); + outlier_points_msg.header.frame_id = "odom"; + debug_outlier_pointcloud_pub_->publish(outlier_points_msg); + + auto grid_map_msg = grid_map::GridMapRosConverter::toMessage(*grid_map_ptr_); + debug_grid_map_pub_->publish(std::move(grid_map_msg)); + } +} + +} // namespace cabot_navigation2 + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared(rclcpp::NodeOptions())); + rclcpp::shutdown(); + return 0; +} diff --git a/cabot_navigation2/src/ransac_ground_filter_node.cpp b/cabot_navigation2/src/ransac_ground_filter_node.cpp index 51234a91..3bf8097e 100644 --- a/cabot_navigation2/src/ransac_ground_filter_node.cpp +++ b/cabot_navigation2/src/ransac_ground_filter_node.cpp @@ -18,6 +18,7 @@ // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE // SOFTWARE. +#include #include #include #include @@ -34,10 +35,7 @@ RansacGroundFilterNode::RansacGroundFilterNode(const rclcpp::NodeOptions & optio ransac_eps_angle_(5.0), ransac_input_min_height_(-0.50), ransac_input_max_height_(0.50), - ransac_inlier_threshold_(0.01), - ground_distance_threshold_(0.05), - debug_(false), - debug_output_plane_topic_("/ground_filter/ransac_plane") + ransac_inlier_threshold_(0.01) { ransac_max_iteration_ = declare_parameter("ransac_max_iteration", ransac_max_iteration_); ransac_probability_ = declare_parameter("ransac_probability", ransac_probability_); @@ -45,16 +43,15 @@ RansacGroundFilterNode::RansacGroundFilterNode(const rclcpp::NodeOptions & optio ransac_input_min_height_ = declare_parameter("ransac_input_min_height", ransac_input_min_height_); ransac_input_max_height_ = declare_parameter("ransac_input_max_height", ransac_input_max_height_); ransac_inlier_threshold_ = declare_parameter("ransac_inlier_threshold", ransac_inlier_threshold_); - ground_distance_threshold_ = declare_parameter("ground_distance_threshold", ground_distance_threshold_); - debug_ = declare_parameter("debug", debug_); - debug_output_plane_topic_ = declare_parameter("debug_output_plane_topic", debug_output_plane_topic_); - if (debug_) { - debug_plane_pub_ = create_publisher(debug_output_plane_topic_, 1); + if (publish_debug_ground_) { + debug_plane_pub_ = create_publisher(output_debug_ground_topic_, 1); } } -void RansacGroundFilterNode::filterGround(const pcl::PointCloud::Ptr input, pcl::PointCloud::Ptr ground, pcl::PointCloud::Ptr filtered) +void RansacGroundFilterNode::filterGround( + const rclcpp::Time & time, const pcl::PointCloud::Ptr input, pcl::PointCloud::Ptr ground, + pcl::PointCloud::Ptr filtered) { // select points which are used to estimate the ground plane pcl::PointCloud::Ptr ransac_input(new pcl::PointCloud); @@ -80,29 +77,43 @@ void RansacGroundFilterNode::filterGround(const pcl::PointCloud:: seg.setInputCloud(ransac_input); seg.segment(*inliers, *coefficients); + pcl::PointIndices ground_indices; + pcl::PointIndices filtered_indices; if (inliers->indices.size() > 0) { // select points above the ground plane - for (const auto & p : input->points) { - float signed_dist = pcl::pointToPlaneDistanceSigned(p, coefficients->values[0], coefficients->values[1], coefficients->values[2], coefficients->values[3]); + for (unsigned int i = 0; i < input->points.size(); i++) { + const auto & p = input->points[i]; + const float signed_dist = pcl::pointToPlaneDistanceSigned(p, coefficients->values[0], coefficients->values[1], coefficients->values[2], coefficients->values[3]); if (signed_dist > ground_distance_threshold_) { - filtered->push_back(p); + filtered_indices.indices.push_back(i); } else if (abs(signed_dist) <= ground_distance_threshold_) { - ground->push_back(p); + ground_indices.indices.push_back(i); } } } else { // if ground plane is not found, select points by fixed height RCLCPP_WARN(get_logger(), "failed to estimate the ground plane"); - for (const auto & p : input->points) { + for (unsigned int i = 0; i < input->points.size(); i++) { + const auto & p = input->points[i]; if (p.z > ground_distance_threshold_) { - filtered->push_back(p); + filtered_indices.indices.push_back(i); } else if (abs(p.z) <= ground_distance_threshold_) { - ground->push_back(p); + ground_indices.indices.push_back(i); } } } - if (debug_) { + pcl::ExtractIndices ground_extract_indices; + ground_extract_indices.setIndices(pcl::make_shared(ground_indices)); + ground_extract_indices.setInputCloud(input); + ground_extract_indices.filter(*ground); + + pcl::ExtractIndices filtered_extract_indices; + filtered_extract_indices.setIndices(pcl::make_shared(filtered_indices)); + filtered_extract_indices.setInputCloud(input); + filtered_extract_indices.filter(*filtered); + + if (publish_debug_ground_) { visualization_msgs::msg::Marker plane_marker; plane_marker.header.frame_id = target_frame_; plane_marker.ns = "ransac_plane"; @@ -111,8 +122,8 @@ void RansacGroundFilterNode::filterGround(const pcl::PointCloud:: plane_marker.action = visualization_msgs::msg::Marker::MODIFY; plane_marker.pose.position.x = 0; plane_marker.pose.position.y = 0; - plane_marker.scale.x = 20.0; - plane_marker.scale.y = 20.0; + plane_marker.scale.x = max_range_ * 2.0; + plane_marker.scale.y = max_range_ * 2.0; plane_marker.scale.z = 0.0; plane_marker.color.r = 1.0f; plane_marker.color.g = 0.0f; diff --git a/docker-compose-common.yaml b/docker-compose-common.yaml index 38c4d5e2..56846763 100644 --- a/docker-compose-common.yaml +++ b/docker-compose-common.yaml @@ -36,6 +36,7 @@ services: - CABOT_SITE - CABOT_LANG - CABOT_LOW_OBSTABLE_DETECT_VERSION + - CABOT_PUBLISH_LOW_OBSTABLE_GROUND - CABOT_OFFSET - CABOT_ANNOUNCE_NO_TOUCH - CABOT_INIT_SPEED diff --git a/docker/ros2/Dockerfile b/docker/ros2/Dockerfile index 6ce3e996..1ac290aa 100644 --- a/docker/ros2/Dockerfile +++ b/docker/ros2/Dockerfile @@ -84,6 +84,7 @@ RUN apt update && \ qtbase5-dev \ ros-$ROS_DISTRO-diagnostic-aggregator \ ros-$ROS_DISTRO-gazebo-msgs \ + ros-$ROS_DISTRO-grid-map \ ros-$ROS_DISTRO-image-transport-plugins \ ros-$ROS_DISTRO-joint-state-publisher \ ros-$ROS_DISTRO-joint-state-publisher-gui \ diff --git a/script/cabot_ros2.sh b/script/cabot_ros2.sh index d6c3c99a..82ac3bb2 100755 --- a/script/cabot_ros2.sh +++ b/script/cabot_ros2.sh @@ -95,6 +95,7 @@ export CABOT_INITAR=$(echo "$CABOT_INITA * 3.1415926535 / 180.0" | bc -l) : ${CABOT_USE_HANDLE_SIMULATOR:=0} : ${CABOT_LOW_OBSTABLE_DETECT_VERSION:=0} +: ${CABOT_PUBLISH_LOW_OBSTABLE_GROUND:=0} # optional variables # TODO @@ -132,6 +133,8 @@ fi pid= use_sim_time=false if [ $CABOT_GAZEBO -eq 1 ]; then use_sim_time=true; fi +publish_low_obstacle_ground=false +if [ $CABOT_PUBLISH_LOW_OBSTABLE_GROUND -eq 1 ]; then publish_low_obstacle_ground=true; fi # configuration for cabot site scripts gazebo=$CABOT_GAZEBO # read by config script @@ -236,6 +239,7 @@ com="$command_prefix ros2 launch cabot_navigation2 bringup_launch.py \ record_bt_log:=false \ cabot_side:=$CABOT_SIDE \ low_obstacle_detect_version:=$CABOT_LOW_OBSTABLE_DETECT_VERSION \ + publish_low_obstacle_ground:=$publish_low_obstacle_ground \ $command_postfix" echo $com eval $com