From 1984aebc5160755718aa0ab9353b4ebd575cd63f Mon Sep 17 00:00:00 2001 From: Diego Palma Date: Fri, 8 Nov 2024 19:16:29 +0000 Subject: [PATCH] Draft: Publish likelihood_data Signed-off-by: Diego Palma --- .../include/beluga_amcl/ros2_common.hpp | 5 +- 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, 91 insertions(+), 3 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..c81686ed9 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 @@ -89,7 +90,7 @@ class BaseAMCLNode : public rclcpp_lifecycle::LifecycleNode { void autostart_callback(); /// User provided extra steps for the autostart process. - virtual void do_autostart_callback(){}; + virtual void do_autostart_callback() {}; // Configuration points for extra steps needed for the different AMCL variants. @@ -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_)) {