From 27fd1358b0d6f1d9cc865747a3d43fa3f011e042 Mon Sep 17 00:00:00 2001 From: Diego Palma Date: Sun, 20 Oct 2024 18:51:40 -0500 Subject: [PATCH 1/3] Expose model_unknown_space parameter to ROS2 Signed-off-by: Diego Palma --- beluga_amcl/config/Amcl.cfg | 6 ++++++ beluga_amcl/src/amcl_node.cpp | 7 +++++++ beluga_amcl/src/amcl_nodelet.cpp | 1 + beluga_amcl/test/test_amcl_node.cpp | 2 ++ beluga_example/params/default.ros2.yaml | 2 ++ 5 files changed, 18 insertions(+) diff --git a/beluga_amcl/config/Amcl.cfg b/beluga_amcl/config/Amcl.cfg index 0be236dcd..c4c4c36cd 100755 --- a/beluga_amcl/config/Amcl.cfg +++ b/beluga_amcl/config/Amcl.cfg @@ -263,6 +263,12 @@ gen.add( default=0.05, min=0., max=1. ) +gen.add( + "model_unknown_space", bool_t, 0, + "Whether to model unknown space or assume it free.", + default=False, +) + gen.add( "laser_z_max", double_t, 0, "Mixture weight for the probability of getting max range measurements.", diff --git a/beluga_amcl/src/amcl_node.cpp b/beluga_amcl/src/amcl_node.cpp index 9e7ad3754..7eec0ca71 100644 --- a/beluga_amcl/src/amcl_node.cpp +++ b/beluga_amcl/src/amcl_node.cpp @@ -120,6 +120,12 @@ AmclNode::AmclNode(const rclcpp::NodeOptions& options) : BaseAMCLNode{"amcl", "" declare_parameter("z_rand", rclcpp::ParameterValue(0.5), descriptor); } + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = "If false, Likelihood Field sensor model won't model unknown space."; + declare_parameter("model_unknown_space", false, descriptor); + } + { auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); descriptor.description = "Mixture weight for the probability of getting max range measurements."; @@ -304,6 +310,7 @@ auto AmclNode::get_sensor_model(std::string_view name, nav_msgs::msg::OccupancyG params.z_hit = get_parameter("z_hit").as_double(); params.z_random = get_parameter("z_rand").as_double(); params.sigma_hit = get_parameter("sigma_hit").as_double(); + params.model_unknown_space = get_parameter("model_unknown_space").as_bool(); return beluga::LikelihoodFieldModel{params, beluga_ros::OccupancyGrid{map}}; } if (name == kBeamSensorModelName) { diff --git a/beluga_amcl/src/amcl_nodelet.cpp b/beluga_amcl/src/amcl_nodelet.cpp index 347376f78..d84c86801 100644 --- a/beluga_amcl/src/amcl_nodelet.cpp +++ b/beluga_amcl/src/amcl_nodelet.cpp @@ -194,6 +194,7 @@ auto AmclNodelet::get_sensor_model(std::string_view name, const nav_msgs::Occupa params.z_hit = config_.laser_z_hit; params.z_random = config_.laser_z_rand; params.sigma_hit = config_.laser_sigma_hit; + params.model_unknown_space = config_.model_unknown_space; return beluga::LikelihoodFieldModel{params, beluga_ros::OccupancyGrid{map}}; } if (name == kBeamSensorModelName) { diff --git a/beluga_amcl/test/test_amcl_node.cpp b/beluga_amcl/test/test_amcl_node.cpp index a4a92f8f2..6f08aee55 100644 --- a/beluga_amcl/test/test_amcl_node.cpp +++ b/beluga_amcl/test/test_amcl_node.cpp @@ -733,6 +733,8 @@ INSTANTIATE_TEST_SUITE_P( rclcpp::Parameter("z_hit", 2.0), rclcpp::Parameter("z_rand", -1.0), rclcpp::Parameter("z_rand", 2.0), + rclcpp::Parameter("model_unknown_space", false), + rclcpp::Parameter("model_unknown_space", true), rclcpp::Parameter("z_max", -1.0), rclcpp::Parameter("z_max", 2.0), rclcpp::Parameter("z_short", -1.0), diff --git a/beluga_example/params/default.ros2.yaml b/beluga_example/params/default.ros2.yaml index 5c4788fec..a40d276af 100644 --- a/beluga_example/params/default.ros2.yaml +++ b/beluga_example/params/default.ros2.yaml @@ -61,6 +61,8 @@ amcl: z_hit: 0.5 # Weight used to combine the probability of random noise in perception. z_rand: 0.5 + # Whether to model unknown space or assume it free. + model_unknown_space : false # Weight used to combine the probability of getting short readings. z_short: 0.05 # Weight used to combine the probability of getting max range readings. From 14697825d6b19189d474817c48ff9f0214e134fb Mon Sep 17 00:00:00 2001 From: Diego Palma Date: Fri, 8 Nov 2024 14:21:44 -0500 Subject: [PATCH 2/3] Draft: Publish likelihood_data Signed-off-by: Diego Palma --- .../include/beluga_amcl/ros2_common.hpp | 3 ++ beluga_amcl/src/amcl_node.cpp | 10 +++- beluga_amcl/src/ros2_common.cpp | 4 ++ beluga_example/params/default.ros.yaml | 2 + beluga_example/params/default.ros2.yaml | 2 +- beluga_ros/include/beluga_ros/amcl.hpp | 16 ++++++ .../include/beluga_ros/likelihood_field.hpp | 50 +++++++++++++++++++ beluga_ros/src/amcl.cpp | 5 ++ 8 files changed, 90 insertions(+), 2 deletions(-) create mode 100644 beluga_ros/include/beluga_ros/likelihood_field.hpp diff --git a/beluga_amcl/include/beluga_amcl/ros2_common.hpp b/beluga_amcl/include/beluga_amcl/ros2_common.hpp index 010699039..668e53879 100644 --- a/beluga_amcl/include/beluga_amcl/ros2_common.hpp +++ b/beluga_amcl/include/beluga_amcl/ros2_common.hpp @@ -24,6 +24,7 @@ #include #include +#include #include #include @@ -118,6 +119,8 @@ class BaseAMCLNode : public rclcpp_lifecycle::LifecycleNode { rclcpp_lifecycle::LifecyclePublisher::SharedPtr particle_markers_pub_; /// Estimated pose publisher. rclcpp_lifecycle::LifecyclePublisher::SharedPtr pose_pub_; + /// Likelihood field publisher + rclcpp_lifecycle::LifecyclePublisher::SharedPtr likelihood_field_pub_; /// Node bond with the lifecycle manager. std::unique_ptr bond_; /// Timer for periodic particle cloud updates. diff --git a/beluga_amcl/src/amcl_node.cpp b/beluga_amcl/src/amcl_node.cpp index 7eec0ca71..bb100b334 100644 --- a/beluga_amcl/src/amcl_node.cpp +++ b/beluga_amcl/src/amcl_node.cpp @@ -63,6 +63,7 @@ #include #include #include +#include #include #include #include @@ -122,7 +123,7 @@ AmclNode::AmclNode(const rclcpp::NodeOptions& options) : BaseAMCLNode{"amcl", "" { auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); - descriptor.description = "If false, Likelihood Field sensor model won't model unknown space."; + descriptor.description = "Whether to model unknown space or assume it free."; declare_parameter("model_unknown_space", false, descriptor); } @@ -418,6 +419,13 @@ void AmclNode::do_periodic_timer_callback() { beluga_ros::stamp_message(get_parameter("global_frame_id").as_string(), now(), message); particle_markers_pub_->publish(message); } + + if (likelihood_field_pub_->get_subscription_count() > 0) { + auto message = beluga_ros::msg::OccupancyGrid{}; + beluga_ros::assign_likelihood_field(particle_filter_->get_likelihood_field(), message); + beluga_ros::stamp_message(get_parameter("global_frame_id").as_string(), now(), message); + likelihood_field_pub_->publish(message); + } } void AmclNode::laser_callback(sensor_msgs::msg::LaserScan::ConstSharedPtr laser_scan) { diff --git a/beluga_amcl/src/ros2_common.cpp b/beluga_amcl/src/ros2_common.cpp index 65e73aa29..1ac855f83 100644 --- a/beluga_amcl/src/ros2_common.cpp +++ b/beluga_amcl/src/ros2_common.cpp @@ -408,6 +408,8 @@ BaseAMCLNode::CallbackReturn BaseAMCLNode::on_configure(const rclcpp_lifecycle:: particle_markers_pub_ = create_publisher("particle_markers", rclcpp::SystemDefaultsQoS()); pose_pub_ = create_publisher("pose", rclcpp::SystemDefaultsQoS()); + likelihood_field_pub_ = + create_publisher("likelihood_field", rclcpp::SystemDefaultsQoS()); do_configure(state); return CallbackReturn::SUCCESS; }; @@ -417,6 +419,7 @@ BaseAMCLNode::CallbackReturn BaseAMCLNode::on_deactivate(const rclcpp_lifecycle: particle_cloud_pub_->on_deactivate(); particle_markers_pub_->on_deactivate(); pose_pub_->on_deactivate(); + likelihood_field_pub_->on_deactivate(); initial_pose_sub_.reset(); tf_listener_.reset(); tf_broadcaster_.reset(); @@ -451,6 +454,7 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn BaseAM particle_cloud_pub_->on_activate(); particle_markers_pub_->on_activate(); pose_pub_->on_activate(); + likelihood_field_pub_->on_activate(); { initial_pose_sub_ = create_subscription( get_parameter("initial_pose_topic").as_string(), rclcpp::SystemDefaultsQoS(), diff --git a/beluga_example/params/default.ros.yaml b/beluga_example/params/default.ros.yaml index 4bc6da9df..254f75a09 100644 --- a/beluga_example/params/default.ros.yaml +++ b/beluga_example/params/default.ros.yaml @@ -63,6 +63,8 @@ laser_max_beams: 100 laser_z_hit: 0.5 # Weight used to combine the probability of random noise in perception. laser_z_rand: 0.5 +# Whether to model unknown space or assume it free. +model_unknown_space: false # Weight used to combine the probability of getting short readings. laser_z_short: 0.05 # Weight used to combine the probability of getting max range readings. diff --git a/beluga_example/params/default.ros2.yaml b/beluga_example/params/default.ros2.yaml index a40d276af..e8d8762c2 100644 --- a/beluga_example/params/default.ros2.yaml +++ b/beluga_example/params/default.ros2.yaml @@ -62,7 +62,7 @@ amcl: # Weight used to combine the probability of random noise in perception. z_rand: 0.5 # Whether to model unknown space or assume it free. - model_unknown_space : false + model_unknown_space: true # Weight used to combine the probability of getting short readings. z_short: 0.05 # Weight used to combine the probability of getting max range readings. diff --git a/beluga_ros/include/beluga_ros/amcl.hpp b/beluga_ros/include/beluga_ros/amcl.hpp index 7ab81ffd4..0f4d46d7c 100644 --- a/beluga_ros/include/beluga_ros/amcl.hpp +++ b/beluga_ros/include/beluga_ros/amcl.hpp @@ -31,6 +31,7 @@ #include #include #include +#include #include #include @@ -133,6 +134,9 @@ class Amcl { /// Returns a reference to the current set of particles. [[nodiscard]] const auto& particles() const { return particles_; } + /// Returns a reference to the current likelihood field. + [[nodiscard]] const auto& get_likelihood_field() const { return likelihood_field_; } + /// Initialize particles using a custom distribution. template void initialize(Distribution distribution) { @@ -178,6 +182,7 @@ class Amcl { private: beluga::TupleVector particles_; + beluga::ValueGrid2 likelihood_field_ = createCleanValueGrid2(0, 2, 2); // Dummy initialization AmclParams params_; beluga::MultivariateUniformDistribution map_distribution_; @@ -193,6 +198,17 @@ class Amcl { beluga::RollingWindow control_action_window_; bool force_update_{true}; + + // Created to initialize likelihood_field_ + template + beluga::ValueGrid2 + createCleanValueGrid2(T default_value, std::size_t width, std::size_t height, double resolution = 1.0) { + // Create a data vector with default_value for each cell + std::vector data(width * height, default_value); + + // Construct and return the ValueGrid2 object + return beluga::ValueGrid2(std::move(data), width, resolution); + } }; } // namespace beluga_ros diff --git a/beluga_ros/include/beluga_ros/likelihood_field.hpp b/beluga_ros/include/beluga_ros/likelihood_field.hpp new file mode 100644 index 000000000..e8fe36abb --- /dev/null +++ b/beluga_ros/include/beluga_ros/likelihood_field.hpp @@ -0,0 +1,50 @@ +// Copyright 2024 Ekumen, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef BELUGA_ROS_LIKELIHOOD_FIELD_HPP +#define BELUGA_ROS_LIKELIHOOD_FIELD_HPP + +#include + +#include +#include + +/** + * \file + * \brief Utilities for likelihood data over ROS interfaces. + */ + +namespace beluga_ros { + +inline void assign_likelihood_field( + const beluga::ValueGrid2& likelihood_field, + nav_msgs::msg::OccupancyGrid& message) { + // Set metadata + message.info.width = static_cast(likelihood_field.width()); + message.info.height = static_cast(likelihood_field.height()); + message.info.resolution = static_cast(likelihood_field.resolution()); + + // Populate the data field with the grid data + message.data.resize(likelihood_field.size()); + const auto& grid_data = likelihood_field.data(); + + for (std::size_t i = 0; i < likelihood_field.size(); ++i) { + // Convert T to int8_t if necessary + message.data[i] = static_cast(grid_data[i]); + } +} + +} // namespace beluga_ros + +#endif // BELUGA_ROS_LIKELIHOOD_FIELD_HPP diff --git a/beluga_ros/src/amcl.cpp b/beluga_ros/src/amcl.cpp index 4f52f9afc..215833a08 100644 --- a/beluga_ros/src/amcl.cpp +++ b/beluga_ros/src/amcl.cpp @@ -76,6 +76,11 @@ auto Amcl::update(Sophus::SE2d base_pose_in_odom, beluga_ros::LaserScan laser_sc }, execution_policy_, motion_model_, sensor_model_); + if (std::holds_alternative>(sensor_model_)) { + likelihood_field_ = + std::get>(sensor_model_).likelihood_field(); + } + const double random_state_probability = random_probability_estimator_(particles_); if (resample_policy_(particles_)) { From 9e6e7f88a49b5eb3f9a4ddb91883052435a0055c Mon Sep 17 00:00:00 2001 From: Diego Palma Date: Sun, 10 Nov 2024 13:24:14 -0500 Subject: [PATCH 3/3] Add thresholds for conversion from likelihood_data to ROS2 message Signed-off-by: Diego Palma --- beluga_ros/include/beluga_ros/likelihood_field.hpp | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/beluga_ros/include/beluga_ros/likelihood_field.hpp b/beluga_ros/include/beluga_ros/likelihood_field.hpp index e8fe36abb..c2dc62f0a 100644 --- a/beluga_ros/include/beluga_ros/likelihood_field.hpp +++ b/beluga_ros/include/beluga_ros/likelihood_field.hpp @@ -27,9 +27,12 @@ namespace beluga_ros { +/// Convert from likelihood data to ROS2 message. inline void assign_likelihood_field( const beluga::ValueGrid2& likelihood_field, - nav_msgs::msg::OccupancyGrid& message) { + nav_msgs::msg::OccupancyGrid& message, + double free_threshold = 0.2, + double occupied_threshold = 0.25) { // Set metadata message.info.width = static_cast(likelihood_field.width()); message.info.height = static_cast(likelihood_field.height()); @@ -40,8 +43,13 @@ inline void assign_likelihood_field( const auto& grid_data = likelihood_field.data(); for (std::size_t i = 0; i < likelihood_field.size(); ++i) { - // Convert T to int8_t if necessary - message.data[i] = static_cast(grid_data[i]); + if (grid_data[i] <= free_threshold) { + message.data[i] = 0; // Free + } else if (grid_data[i] >= occupied_threshold) { + message.data[i] = 100; // Occupied + } else { + message.data[i] = -1; // Unknown + } } }