From 964a5c34e54ca956b2d46f7447bf8c8e09f5cb38 Mon Sep 17 00:00:00 2001 From: Nahuel Espinosa Date: Mon, 5 Dec 2022 13:35:12 +0000 Subject: [PATCH 1/6] Add likelihood field pre-computation --- beluga/include/beluga/algorithm.h | 1 + .../include/beluga/algorithm/distance_map.h | 62 ++++++++ beluga/test/beluga/CMakeLists.txt | 1 + .../beluga/algorithm/test_distance_map.cpp | 74 +++++++++ beluga_amcl/include/beluga_amcl/amcl_node.hpp | 2 + beluga_amcl/include/beluga_amcl/models.hpp | 145 ++++++++++++++---- beluga_amcl/src/amcl_node.cpp | 114 ++++++++++++-- beluga_example/config/params.yaml | 6 + beluga_example/rviz/rviz.rviz | 48 ++++-- 9 files changed, 394 insertions(+), 59 deletions(-) create mode 100644 beluga/include/beluga/algorithm/distance_map.h create mode 100644 beluga/test/beluga/algorithm/test_distance_map.cpp diff --git a/beluga/include/beluga/algorithm.h b/beluga/include/beluga/algorithm.h index 15afc903d..9239ab801 100644 --- a/beluga/include/beluga/algorithm.h +++ b/beluga/include/beluga/algorithm.h @@ -14,6 +14,7 @@ #pragma once +#include #include #include #include diff --git a/beluga/include/beluga/algorithm/distance_map.h b/beluga/include/beluga/algorithm/distance_map.h new file mode 100644 index 000000000..05c6770ed --- /dev/null +++ b/beluga/include/beluga/algorithm/distance_map.h @@ -0,0 +1,62 @@ +// Copyright 2022 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. + +#pragma once + +#include +#include + +template +auto nearest_obstacle_distance_map( + Range&& map, + DistanceFunction&& distance_function, + NeighborsFunction&& neighbors_function) { + struct IndexPair { + std::size_t nearest_obstacle_index; + std::size_t index; + }; + + using DistanceType = std::invoke_result_t; + auto distance_map = std::vector(map.size()); + auto visited = std::vector(map.size(), false); + + auto compare = [&distance_map](const IndexPair& first, const IndexPair& second) { + return distance_map[first.index] > distance_map[second.index]; + }; + auto queue = std::priority_queue, decltype(compare)>{compare}; + + auto begin = map.begin(); + for (std::size_t index = 0; index < map.size(); ++index) { + bool is_obstacle = *(begin + index); + if (is_obstacle) { + visited[index] = true; + distance_map[index] = 0; + queue.push(IndexPair{index, index}); // The nearest obstacle is itself + } + } + + while (!queue.empty()) { + auto parent = queue.top(); + queue.pop(); + for (std::size_t index : neighbors_function(parent.index)) { + if (!visited[index]) { + visited[index] = true; + distance_map[index] = distance_function(parent.nearest_obstacle_index, index); + queue.push(IndexPair{parent.nearest_obstacle_index, index}); + } + } + } + + return distance_map; +} diff --git a/beluga/test/beluga/CMakeLists.txt b/beluga/test/beluga/CMakeLists.txt index 0fb88e942..285e1e9d9 100644 --- a/beluga/test/beluga/CMakeLists.txt +++ b/beluga/test/beluga/CMakeLists.txt @@ -1,6 +1,7 @@ find_package(ament_cmake_gmock REQUIRED) ament_add_gmock(test_beluga + algorithm/test_distance_map.cpp algorithm/test_exponential_filter.cpp algorithm/test_particle_filter.cpp algorithm/test_sampling.cpp diff --git a/beluga/test/beluga/algorithm/test_distance_map.cpp b/beluga/test/beluga/algorithm/test_distance_map.cpp new file mode 100644 index 000000000..8f317110c --- /dev/null +++ b/beluga/test/beluga/algorithm/test_distance_map.cpp @@ -0,0 +1,74 @@ +// Copyright 2022 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. + +#include + +#include + +namespace { + +auto array_distance(std::size_t first, std::size_t second) { + return std::abs(static_cast(first) - static_cast(second)); +} + +auto make_neighbors_function(std::size_t size) { + return [=](std::size_t index) { + auto result = std::vector{}; + if (index > 0) { + result.push_back(index - 1); + } + if (index < size - 1) { + result.push_back(index + 1); + } + return result; + }; +} + +TEST(DistanceMap, None) { + auto map = std::vector{}; + auto distance_map = nearest_obstacle_distance_map(map, array_distance, make_neighbors_function(6)); + ASSERT_EQ(distance_map.size(), 0); +} + +TEST(DistanceMap, Empty) { + auto map = std::array{false, false, false, false, false, false}; + auto distance_map = nearest_obstacle_distance_map(map, array_distance, make_neighbors_function(6)); + ASSERT_THAT(distance_map, testing::ElementsAre(0, 0, 0, 0, 0, 0)); +} + +TEST(DistanceMap, Full) { + auto map = std::array{true, true, true, true, true, true}; + auto distance_map = nearest_obstacle_distance_map(map, array_distance, make_neighbors_function(6)); + ASSERT_THAT(distance_map, testing::ElementsAre(0, 0, 0, 0, 0, 0)); +} + +TEST(DistanceMap, Case1) { + auto map = std::array{false, true, false, false, false, true}; + auto distance_map = nearest_obstacle_distance_map(map, array_distance, make_neighbors_function(6)); + ASSERT_THAT(distance_map, testing::ElementsAre(1, 0, 1, 2, 1, 0)); +} + +TEST(DistanceMap, Case2) { + auto map = std::array{true, true, false, false, false, false}; + auto distance_map = nearest_obstacle_distance_map(map, array_distance, make_neighbors_function(6)); + ASSERT_THAT(distance_map, testing::ElementsAre(0, 0, 1, 2, 3, 4)); +} + +TEST(DistanceMap, Case3) { + auto map = std::array{false, false, false, false, false, true}; + auto distance_map = nearest_obstacle_distance_map(map, array_distance, make_neighbors_function(6)); + ASSERT_THAT(distance_map, testing::ElementsAre(5, 4, 3, 2, 1, 0)); +} + +} // namespace diff --git a/beluga_amcl/include/beluga_amcl/amcl_node.hpp b/beluga_amcl/include/beluga_amcl/amcl_node.hpp index 80bf71de4..526bfa45c 100644 --- a/beluga_amcl/include/beluga_amcl/amcl_node.hpp +++ b/beluga_amcl/include/beluga_amcl/amcl_node.hpp @@ -53,6 +53,8 @@ class AmclNode : public rclcpp_lifecycle::LifecycleNode rclcpp::TimerBase::SharedPtr timer_; rclcpp_lifecycle::LifecyclePublisher::SharedPtr particle_cloud_pub_; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr + likelihood_field_pub_; rclcpp::Subscription::SharedPtr map_sub_; }; diff --git a/beluga_amcl/include/beluga_amcl/models.hpp b/beluga_amcl/include/beluga_amcl/models.hpp index bc1bb9dc9..8f4c2ac99 100644 --- a/beluga_amcl/include/beluga_amcl/models.hpp +++ b/beluga_amcl/include/beluga_amcl/models.hpp @@ -20,9 +20,12 @@ #ifndef BELUGA_AMCL__MODELS_HPP_ #define BELUGA_AMCL__MODELS_HPP_ +#include #include +#include #include +#include #include #include #include @@ -30,6 +33,7 @@ #include #include +#include namespace beluga_amcl { @@ -68,6 +72,15 @@ class MockMotionModel : public Mixin [[nodiscard]] auto apply_motion(const Pose & state) {return state;} }; +struct LikelihoodSensorModelParam +{ + double max_obstacle_distance; + double max_laser_distance; + double z_hit; + double z_random; + double sigma_hit; +}; + // TODO(nahuel): Move this to the core package once it doesn't depend on // ROS message types, or if we decide to add that dependecy to the core. template @@ -75,21 +88,16 @@ class LikelihoodSensorModel : public Mixin { public: template - explicit LikelihoodSensorModel(const nav_msgs::msg::OccupancyGrid & map, Args && ... rest) - : Mixin(std::forward(rest)...) - { - const std::size_t size = map.info.width * map.info.height; - - free_cells_.reserve(size); - for (std::size_t index = 0; index < size; ++index) { - if (map.data[index] == 0) { - free_cells_.push_back(index); - } - } - - map_metadata_ = map.info; - index_distribution_ = std::uniform_int_distribution{0, free_cells_.size() - 1}; - } + explicit LikelihoodSensorModel( + const LikelihoodSensorModelParam & params, + const nav_msgs::msg::OccupancyGrid & map, Args && ... rest) + : Mixin(std::forward(rest)...), + params_{params}, + map_metadata_{map.info}, + free_cells_{make_free_cells_list(map)}, + index_distribution_{0, free_cells_.size() - 1}, + likelihood_field_{make_likelihood_field(params, map)} + {} [[nodiscard]] double importance_weight(const Pose &) { @@ -100,29 +108,108 @@ class LikelihoodSensorModel : public Mixin template [[nodiscard]] auto generate_random_state(Generator & generator) { - return pose_from_index_and_angle( - map_metadata_, - free_cells_[index_distribution_(generator)], - theta_distribution_(generator)); + auto pose = Pose{}; + auto [x, y] = xy_from_index(map_metadata_, free_cells_[index_distribution_(generator)]); + pose.x = x; + pose.y = y; + pose.theta = theta_distribution_(generator); + return pose; + } + + auto get_likelihood_field_as_gridmap() const + { + auto message = nav_msgs::msg::OccupancyGrid{}; + message.info = map_metadata_; + const std::size_t size = map_metadata_.width * map_metadata_.height; + message.data.resize(size); + + for (std::size_t index = 0; index < size; ++index) { + message.data[index] = static_cast(likelihood_field_[index] * 100); + } + return message; } private: + LikelihoodSensorModelParam params_; nav_msgs::msg::MapMetaData map_metadata_; std::vector free_cells_; std::uniform_int_distribution index_distribution_; std::uniform_real_distribution theta_distribution_{-M_PI, M_PI}; - static auto pose_from_index_and_angle( - const nav_msgs::msg::MapMetaData & metadata, - std::size_t index, double angle) + std::vector likelihood_field_; + + static std::vector make_free_cells_list(const nav_msgs::msg::OccupancyGrid & map) { - double delta_x = (static_cast(index % metadata.height) + 0.5) * metadata.resolution; - double delta_y = (static_cast(index / metadata.width) + 0.5) * metadata.resolution; - auto pose = Pose{}; - pose.x = delta_x + metadata.origin.position.x; - pose.y = delta_y + metadata.origin.position.y; - pose.theta = angle; - return pose; + const std::size_t size = map.info.width * map.info.height; + auto free_cells = std::vector{}; + free_cells.reserve(size); + for (std::size_t index = 0; index < size; ++index) { + if (map.data[index] == 0) { + free_cells.push_back(index); + } + } + return free_cells; + } + + static std::vector make_likelihood_field( + const LikelihoodSensorModelParam & params, + const nav_msgs::msg::OccupancyGrid & map) + { + auto && obstacle_map = map.data | ranges::views::transform( + [](std::int8_t value) -> bool { + return value > 65; + }); + + auto squared_distance = + [height = map.info.height, + width = map.info.width, + squared_resolution = map.info.resolution * map.info.resolution, + squared_max_distance = params.max_obstacle_distance * params.max_obstacle_distance + ](std::size_t first, std::size_t second) { + auto delta_x = + static_cast(first % height) - static_cast(second % height); + auto delta_y = + static_cast(first / width) - static_cast(second / width); + return std::min( + static_cast(delta_x * delta_x + delta_y * delta_y) * squared_resolution, + squared_max_distance); + }; + + auto neighbors = [&map](std::size_t index) { + auto result = std::vector{}; + if (index / map.info.width < map.info.height - 1) { + result.push_back(index + map.info.height); + } + if (index / map.info.width > 0) { + result.push_back(index - map.info.height); + } + if (index % map.info.height < map.info.width - 1) { + result.push_back(index + 1); + } + if (index % map.info.height > 0) { + result.push_back(index - 1); + } + return result; + }; + + auto to_likelihood = + [amplitude = params.z_hit / (params.sigma_hit * std::sqrt(2 * M_PI)), + two_squared_sigma = 2 * params.sigma_hit * params.sigma_hit, + offset = params.z_random / params.max_laser_distance](double squared_distance) { + return amplitude * std::exp(-squared_distance / two_squared_sigma) + offset; + }; + + auto distance_map = nearest_obstacle_distance_map(obstacle_map, squared_distance, neighbors); + return distance_map | ranges::views::transform(to_likelihood) | ranges::to; + } + + static auto xy_from_index( + const nav_msgs::msg::MapMetaData & info, + std::size_t index) + { + return std::make_pair( + (static_cast(index % info.height) + 0.5) * info.resolution + info.origin.position.x, + (static_cast(index / info.width) + 0.5) * info.resolution + info.origin.position.y); } }; diff --git a/beluga_amcl/src/amcl_node.cpp b/beluga_amcl/src/amcl_node.cpp index bda0f47c2..aa24eba8a 100644 --- a/beluga_amcl/src/amcl_node.cpp +++ b/beluga_amcl/src/amcl_node.cpp @@ -151,6 +151,67 @@ AmclNode::AmclNode(const rclcpp::NodeOptions & options) descriptor.floating_point_range[0].step = 0; declare_parameter("update_min_d", rclcpp::ParameterValue(0.25), descriptor); } + + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = + "Maximum distance to do obstacle inflation on map, used in likelihood field model."; + descriptor.floating_point_range.resize(1); + descriptor.floating_point_range[0].from_value = 0; + descriptor.floating_point_range[0].to_value = std::numeric_limits::max(); + descriptor.floating_point_range[0].step = 0; + declare_parameter("laser_likelihood_max_dist", rclcpp::ParameterValue(2.0), descriptor); + } + + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = "Maximum scan range to be considered."; + descriptor.floating_point_range.resize(1); + descriptor.floating_point_range[0].from_value = 0; + descriptor.floating_point_range[0].to_value = std::numeric_limits::max(); + descriptor.floating_point_range[0].step = 0; + declare_parameter("laser_max_range", rclcpp::ParameterValue(100.0), descriptor); + } + + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = "Minimum scan range to be considered."; + descriptor.floating_point_range.resize(1); + descriptor.floating_point_range[0].from_value = 0; + descriptor.floating_point_range[0].to_value = std::numeric_limits::max(); + descriptor.floating_point_range[0].step = 0; + declare_parameter("laser_min_range", rclcpp::ParameterValue(0.0), descriptor); + } + + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = "Mixin weight for the probability of hitting an obstacle."; + descriptor.floating_point_range.resize(1); + descriptor.floating_point_range[0].from_value = 0; + descriptor.floating_point_range[0].to_value = 1; + descriptor.floating_point_range[0].step = 0; + declare_parameter("z_hit", rclcpp::ParameterValue(0.5), descriptor); + } + + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = "Mixin weight for the probability of getting random measurements."; + descriptor.floating_point_range.resize(1); + descriptor.floating_point_range[0].from_value = 0; + descriptor.floating_point_range[0].to_value = 1; + descriptor.floating_point_range[0].step = 0; + declare_parameter("z_rand", rclcpp::ParameterValue(0.5), descriptor); + } + + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = "Standard deviation of the hit distribution."; + descriptor.floating_point_range.resize(1); + descriptor.floating_point_range[0].from_value = 0; + descriptor.floating_point_range[0].to_value = 1; + descriptor.floating_point_range[0].step = 0; + declare_parameter("sigma_hit", rclcpp::ParameterValue(0.2), descriptor); + } } AmclNode::~AmclNode() @@ -176,6 +237,10 @@ AmclNode::CallbackReturn AmclNode::on_configure(const rclcpp_lifecycle::State &) "particle_cloud", rclcpp::SensorDataQoS()); + likelihood_field_pub_ = create_publisher( + "likelihood_field", + rclcpp::SystemDefaultsQoS()); + map_sub_ = create_subscription( get_parameter("map_topic").as_string(), rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), @@ -189,6 +254,7 @@ AmclNode::CallbackReturn AmclNode::on_activate(const rclcpp_lifecycle::State &) { RCLCPP_INFO(get_logger(), "Activating"); particle_cloud_pub_->on_activate(); + likelihood_field_pub_->on_activate(); RCLCPP_INFO(get_logger(), "Creating bond (%s) to lifecycle manager.", get_name()); @@ -204,6 +270,7 @@ AmclNode::CallbackReturn AmclNode::on_deactivate(const rclcpp_lifecycle::State & { RCLCPP_INFO(get_logger(), "Deactivating"); particle_cloud_pub_->on_deactivate(); + likelihood_field_pub_->on_deactivate(); bond_.reset(); return CallbackReturn::SUCCESS; @@ -213,6 +280,7 @@ AmclNode::CallbackReturn AmclNode::on_cleanup(const rclcpp_lifecycle::State &) { RCLCPP_INFO(get_logger(), "Cleaning up"); particle_cloud_pub_.reset(); + likelihood_field_pub_.reset(); map_sub_.reset(); return CallbackReturn::SUCCESS; } @@ -238,8 +306,15 @@ void AmclNode::map_callback(nav_msgs::msg::OccupancyGrid::SharedPtr map) get_parameter("pf_err").as_double(), get_parameter("pf_z").as_double()}; + auto likelihood_model_params = LikelihoodSensorModelParam{ + get_parameter("laser_likelihood_max_dist").as_double(), + get_parameter("laser_max_range").as_double(), + get_parameter("z_hit").as_double(), + get_parameter("z_rand").as_double(), + get_parameter("sigma_hit").as_double()}; + particle_filter_ = std::make_unique( - generation_params, kld_resampling_params, *map); + generation_params, kld_resampling_params, likelihood_model_params, *map); RCLCPP_INFO( get_logger(), "Particle filter initialized with %ld particles", @@ -252,22 +327,29 @@ void AmclNode::timer_callback() return; } - auto message = nav2_msgs::msg::ParticleCloud{}; - message.header.stamp = now(); - message.header.frame_id = get_parameter("global_frame_id").as_string(); - - message.particles.resize(particle_filter_->particles().size()); - - ranges::transform( - particle_filter_->particles(), std::begin(message.particles), [](const auto & particle) { - auto message = nav2_msgs::msg::Particle{}; - message.pose = geometry_msgs::msg::Pose{beluga::state(particle)}; - message.weight = beluga::weight(particle); - return message; - }); + { + auto message = nav2_msgs::msg::ParticleCloud{}; + message.header.stamp = now(); + message.header.frame_id = get_parameter("global_frame_id").as_string(); + message.particles.resize(particle_filter_->particles().size()); + ranges::transform( + particle_filter_->particles(), std::begin(message.particles), [](const auto & particle) { + auto message = nav2_msgs::msg::Particle{}; + message.pose = geometry_msgs::msg::Pose{beluga::state(particle)}; + message.weight = beluga::weight(particle); + return message; + }); + RCLCPP_INFO(get_logger(), "Publishing %ld particles", message.particles.size()); + particle_cloud_pub_->publish(message); + } - RCLCPP_INFO(get_logger(), "Publishing %ld particles", message.particles.size()); - particle_cloud_pub_->publish(message); + { + auto message = particle_filter_->get_likelihood_field_as_gridmap(); + message.header.stamp = now(); + message.header.frame_id = get_parameter("global_frame_id").as_string(); + RCLCPP_INFO(get_logger(), "Publishing likelihood field"); + likelihood_field_pub_->publish(message); + } } } // namespace beluga_amcl diff --git a/beluga_example/config/params.yaml b/beluga_example/config/params.yaml index c2a99da12..377ece65c 100644 --- a/beluga_example/config/params.yaml +++ b/beluga_example/config/params.yaml @@ -11,3 +11,9 @@ amcl: resample_interval: 1 update_min_a: 0.2 update_min_d: 0.25 + laser_likelihood_max_dist: 2.0 + laser_max_range: 100.0 + laser_min_range: 0.0 + z_hit: 0.5 + z_rand: 0.5 + sigma_hit: 0.2 diff --git a/beluga_example/rviz/rviz.rviz b/beluga_example/rviz/rviz.rviz index c45814ef5..4af5bfc20 100644 --- a/beluga_example/rviz/rviz.rviz +++ b/beluga_example/rviz/rviz.rviz @@ -8,10 +8,9 @@ Panels: - /Status1 - /ParticleCloud1 - /Map1 - - /TF1 - - /LaserScan1 + - /Map2 Splitter Ratio: 0.5 - Tree Height: 1166 + Tree Height: 1555 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -67,10 +66,10 @@ Visualization Manager: Reliability Policy: Best Effort Value: /particle_cloud Value: true - - Alpha: 0.699999988079071 + - Alpha: 1 Class: rviz_default_plugins/Map Color Scheme: map - Draw Behind: false + Draw Behind: true Enabled: true Name: Map Topic: @@ -88,6 +87,27 @@ Visualization Manager: Value: /map_updates Use Timestamp: false Value: true + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: raw + Draw Behind: false + Enabled: true + Name: Map + Topic: + Depth: 5 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /likelihood_field + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /likelihood_field_updates + Use Timestamp: false + Value: true - Class: rviz_default_plugins/TF Enabled: true Frame Timeout: 15 @@ -191,7 +211,7 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 7.173450469970703 + Distance: 7.49760103225708 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -201,23 +221,23 @@ Visualization Manager: X: 0 Y: 0 Z: 0 - Focal Shape Fixed Size: true + Focal Shape Fixed Size: false Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 1.100398302078247 + Pitch: 0.674796462059021 Target Frame: Value: Orbit (rviz) - Yaw: 3.5103964805603027 + Yaw: 0.9922116994857788 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1536 + Height: 2032 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd0000000400000000000001dc00000553fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000553000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000014f00000553fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b00000553000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009b800000051fc0100000002fb0000000800540069006d00650100000000000009b80000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000006810000055300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000001dc00000701fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000ab00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000069000007010000017800fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000014f00000701fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000069000007010000012300fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000e7000000051fc0100000002fb0000000800540069006d0065010000000000000e700000046000fffffffb0000000800540069006d0065010000000000000450000000000000000000000b2d0000070100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -226,6 +246,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 2488 - X: 72 - Y: 27 + Width: 3696 + X: 144 + Y: 54 From a05a25471d39469d94b332e7b70ca7c5995a1bf8 Mon Sep 17 00:00:00 2001 From: Nahuel Espinosa Date: Mon, 5 Dec 2022 17:23:59 +0000 Subject: [PATCH 2/6] Rename function parameter --- beluga/include/beluga/algorithm/distance_map.h | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/beluga/include/beluga/algorithm/distance_map.h b/beluga/include/beluga/algorithm/distance_map.h index 05c6770ed..a0062ee87 100644 --- a/beluga/include/beluga/algorithm/distance_map.h +++ b/beluga/include/beluga/algorithm/distance_map.h @@ -19,7 +19,7 @@ template auto nearest_obstacle_distance_map( - Range&& map, + Range&& obstacle_map, DistanceFunction&& distance_function, NeighborsFunction&& neighbors_function) { struct IndexPair { @@ -28,16 +28,16 @@ auto nearest_obstacle_distance_map( }; using DistanceType = std::invoke_result_t; - auto distance_map = std::vector(map.size()); - auto visited = std::vector(map.size(), false); + auto distance_map = std::vector(obstacle_map.size()); + auto visited = std::vector(obstacle_map.size(), false); auto compare = [&distance_map](const IndexPair& first, const IndexPair& second) { return distance_map[first.index] > distance_map[second.index]; }; auto queue = std::priority_queue, decltype(compare)>{compare}; - auto begin = map.begin(); - for (std::size_t index = 0; index < map.size(); ++index) { + auto begin = obstacle_map.begin(); + for (std::size_t index = 0; index < obstacle_map.size(); ++index) { bool is_obstacle = *(begin + index); if (is_obstacle) { visited[index] = true; From c0f4e93467c123fbb84b8c2b76b47d733b7400f6 Mon Sep 17 00:00:00 2001 From: Nahuel Espinosa Date: Mon, 5 Dec 2022 19:23:48 +0000 Subject: [PATCH 3/6] Publish likelihood field once --- beluga_amcl/src/amcl_node.cpp | 31 ++++++++++++++++--------------- 1 file changed, 16 insertions(+), 15 deletions(-) diff --git a/beluga_amcl/src/amcl_node.cpp b/beluga_amcl/src/amcl_node.cpp index aa24eba8a..dcecaf6ac 100644 --- a/beluga_amcl/src/amcl_node.cpp +++ b/beluga_amcl/src/amcl_node.cpp @@ -241,12 +241,6 @@ AmclNode::CallbackReturn AmclNode::on_configure(const rclcpp_lifecycle::State &) "likelihood_field", rclcpp::SystemDefaultsQoS()); - map_sub_ = create_subscription( - get_parameter("map_topic").as_string(), - rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), - std::bind(&AmclNode::map_callback, this, std::placeholders::_1)); - - RCLCPP_INFO(get_logger(), "Subscribed to map_topic: %s", map_sub_->get_topic_name()); return CallbackReturn::SUCCESS; } @@ -263,6 +257,13 @@ AmclNode::CallbackReturn AmclNode::on_activate(const rclcpp_lifecycle::State &) bond_->setHeartbeatTimeout(4.0); bond_->start(); + map_sub_ = create_subscription( + get_parameter("map_topic").as_string(), + rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), + std::bind(&AmclNode::map_callback, this, std::placeholders::_1)); + + RCLCPP_INFO(get_logger(), "Subscribed to map_topic: %s", map_sub_->get_topic_name()); + return CallbackReturn::SUCCESS; } @@ -271,6 +272,7 @@ AmclNode::CallbackReturn AmclNode::on_deactivate(const rclcpp_lifecycle::State & RCLCPP_INFO(get_logger(), "Deactivating"); particle_cloud_pub_->on_deactivate(); likelihood_field_pub_->on_deactivate(); + map_sub_.reset(); bond_.reset(); return CallbackReturn::SUCCESS; @@ -281,7 +283,6 @@ AmclNode::CallbackReturn AmclNode::on_cleanup(const rclcpp_lifecycle::State &) RCLCPP_INFO(get_logger(), "Cleaning up"); particle_cloud_pub_.reset(); likelihood_field_pub_.reset(); - map_sub_.reset(); return CallbackReturn::SUCCESS; } @@ -319,6 +320,14 @@ void AmclNode::map_callback(nav_msgs::msg::OccupancyGrid::SharedPtr map) RCLCPP_INFO( get_logger(), "Particle filter initialized with %ld particles", particle_filter_->particles().size()); + + { + auto message = particle_filter_->get_likelihood_field_as_gridmap(); + message.header.stamp = now(); + message.header.frame_id = get_parameter("global_frame_id").as_string(); + RCLCPP_INFO(get_logger(), "Publishing likelihood field"); + likelihood_field_pub_->publish(message); + } } void AmclNode::timer_callback() @@ -342,14 +351,6 @@ void AmclNode::timer_callback() RCLCPP_INFO(get_logger(), "Publishing %ld particles", message.particles.size()); particle_cloud_pub_->publish(message); } - - { - auto message = particle_filter_->get_likelihood_field_as_gridmap(); - message.header.stamp = now(); - message.header.frame_id = get_parameter("global_frame_id").as_string(); - RCLCPP_INFO(get_logger(), "Publishing likelihood field"); - likelihood_field_pub_->publish(message); - } } } // namespace beluga_amcl From 59f1a9b2cf89b69075292493113c2a10c27545ca Mon Sep 17 00:00:00 2001 From: Nahuel Espinosa Date: Mon, 5 Dec 2022 19:34:37 +0000 Subject: [PATCH 4/6] Fix occupied value --- beluga_amcl/include/beluga_amcl/models.hpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/beluga_amcl/include/beluga_amcl/models.hpp b/beluga_amcl/include/beluga_amcl/models.hpp index 8f4c2ac99..3856909a0 100644 --- a/beluga_amcl/include/beluga_amcl/models.hpp +++ b/beluga_amcl/include/beluga_amcl/models.hpp @@ -155,9 +155,11 @@ class LikelihoodSensorModel : public Mixin const LikelihoodSensorModelParam & params, const nav_msgs::msg::OccupancyGrid & map) { - auto && obstacle_map = map.data | ranges::views::transform( + auto obstacle_map = map.data | ranges::views::transform( [](std::int8_t value) -> bool { - return value > 65; + // Map server occupied output value + // https://wiki.ros.org/map_server#Value_Interpretation + return value == 100; }); auto squared_distance = From ec63902bf16ea4a4bfde3631e691c77e61d390e6 Mon Sep 17 00:00:00 2001 From: Nahuel Espinosa Date: Mon, 5 Dec 2022 20:35:22 +0000 Subject: [PATCH 5/6] Fix index arithmetic --- beluga_amcl/include/beluga_amcl/models.hpp | 17 ++++++++--------- 1 file changed, 8 insertions(+), 9 deletions(-) diff --git a/beluga_amcl/include/beluga_amcl/models.hpp b/beluga_amcl/include/beluga_amcl/models.hpp index 3856909a0..cbfbbd1d6 100644 --- a/beluga_amcl/include/beluga_amcl/models.hpp +++ b/beluga_amcl/include/beluga_amcl/models.hpp @@ -163,13 +163,12 @@ class LikelihoodSensorModel : public Mixin }); auto squared_distance = - [height = map.info.height, - width = map.info.width, + [width = map.info.width, squared_resolution = map.info.resolution * map.info.resolution, squared_max_distance = params.max_obstacle_distance * params.max_obstacle_distance ](std::size_t first, std::size_t second) { auto delta_x = - static_cast(first % height) - static_cast(second % height); + static_cast(first % width) - static_cast(second % width); auto delta_y = static_cast(first / width) - static_cast(second / width); return std::min( @@ -179,16 +178,16 @@ class LikelihoodSensorModel : public Mixin auto neighbors = [&map](std::size_t index) { auto result = std::vector{}; - if (index / map.info.width < map.info.height - 1) { - result.push_back(index + map.info.height); + if (index / map.info.width < (map.info.height - 1)) { + result.push_back(index + map.info.width); } if (index / map.info.width > 0) { - result.push_back(index - map.info.height); + result.push_back(index - map.info.width); } - if (index % map.info.height < map.info.width - 1) { + if (index % map.info.width < (map.info.width - 1)) { result.push_back(index + 1); } - if (index % map.info.height > 0) { + if (index % map.info.width > 0) { result.push_back(index - 1); } return result; @@ -210,7 +209,7 @@ class LikelihoodSensorModel : public Mixin std::size_t index) { return std::make_pair( - (static_cast(index % info.height) + 0.5) * info.resolution + info.origin.position.x, + (static_cast(index % info.width) + 0.5) * info.resolution + info.origin.position.x, (static_cast(index / info.width) + 0.5) * info.resolution + info.origin.position.y); } }; From 394a468b1fc67d9a3ded55dd21604577ef59ad5c Mon Sep 17 00:00:00 2001 From: Nahuel Espinosa Date: Tue, 6 Dec 2022 18:00:23 +0000 Subject: [PATCH 6/6] Fix standard deviation parameter limit --- beluga_amcl/src/amcl_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/beluga_amcl/src/amcl_node.cpp b/beluga_amcl/src/amcl_node.cpp index dcecaf6ac..3b2ff24f1 100644 --- a/beluga_amcl/src/amcl_node.cpp +++ b/beluga_amcl/src/amcl_node.cpp @@ -208,7 +208,7 @@ AmclNode::AmclNode(const rclcpp::NodeOptions & options) descriptor.description = "Standard deviation of the hit distribution."; descriptor.floating_point_range.resize(1); descriptor.floating_point_range[0].from_value = 0; - descriptor.floating_point_range[0].to_value = 1; + descriptor.floating_point_range[0].to_value = std::numeric_limits::max(); descriptor.floating_point_range[0].step = 0; declare_parameter("sigma_hit", rclcpp::ParameterValue(0.2), descriptor); }