From 856aeb6d697d6d4a8fe94c2c2021775a6e4ba363 Mon Sep 17 00:00:00 2001 From: Nahuel Espinosa Date: Mon, 5 Dec 2022 13:33:41 +0000 Subject: [PATCH 1/3] Implement likelihood estimation --- beluga_amcl/CMakeLists.txt | 6 +- beluga_amcl/include/beluga_amcl/amcl_node.hpp | 18 ++- beluga_amcl/include/beluga_amcl/models.hpp | 112 ++++++++++++++---- beluga_amcl/package.xml | 4 + beluga_amcl/src/amcl_node.cpp | 72 ++++++++++- beluga_example/config/params.yaml | 2 + beluga_example/models/turtlebot.model.yaml | 19 +-- 7 files changed, 188 insertions(+), 45 deletions(-) diff --git a/beluga_amcl/CMakeLists.txt b/beluga_amcl/CMakeLists.txt index 122f838c3..22abeb7ed 100644 --- a/beluga_amcl/CMakeLists.txt +++ b/beluga_amcl/CMakeLists.txt @@ -13,6 +13,8 @@ find_package(nav2_msgs REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) find_package(rclcpp_lifecycle REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) add_executable(amcl_node) target_sources(amcl_node PUBLIC src/main.cpp src/amcl_node.cpp) @@ -26,7 +28,9 @@ ament_target_dependencies(amcl_node PUBLIC nav2_msgs rclcpp rclcpp_components - rclcpp_lifecycle) + rclcpp_lifecycle + sensor_msgs + tf2_geometry_msgs) install(TARGETS amcl_node DESTINATION lib/${PROJECT_NAME}) diff --git a/beluga_amcl/include/beluga_amcl/amcl_node.hpp b/beluga_amcl/include/beluga_amcl/amcl_node.hpp index 526bfa45c..87e5e95a5 100644 --- a/beluga_amcl/include/beluga_amcl/amcl_node.hpp +++ b/beluga_amcl/include/beluga_amcl/amcl_node.hpp @@ -15,6 +15,12 @@ #ifndef BELUGA_AMCL__AMCL_NODE_HPP_ #define BELUGA_AMCL__AMCL_NODE_HPP_ +#include +#include +#include +#include +#include + #include #include @@ -24,6 +30,7 @@ #include #include #include +#include namespace beluga_amcl { @@ -32,7 +39,7 @@ class AmclNode : public rclcpp_lifecycle::LifecycleNode { public: using rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; - using ParticleFilter = beluga::AMCL; + using ParticleFilter = beluga::AMCL; explicit AmclNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); virtual ~AmclNode(); @@ -46,6 +53,7 @@ class AmclNode : public rclcpp_lifecycle::LifecycleNode void map_callback(nav_msgs::msg::OccupancyGrid::SharedPtr); void timer_callback(); + void laser_callback(sensor_msgs::msg::LaserScan::ConstSharedPtr); std::unique_ptr particle_filter_; std::unique_ptr bond_; @@ -56,6 +64,14 @@ class AmclNode : public rclcpp_lifecycle::LifecycleNode rclcpp_lifecycle::LifecyclePublisher::SharedPtr likelihood_field_pub_; rclcpp::Subscription::SharedPtr map_sub_; + + std::unique_ptr tf_buffer_; + std::unique_ptr tf_broadcaster_; + std::unique_ptr tf_listener_; + std::unique_ptr> laser_scan_filter_; + std::unique_ptr> laser_scan_sub_; + message_filters::Connection laser_scan_connection_; }; } // namespace beluga_amcl diff --git a/beluga_amcl/include/beluga_amcl/models.hpp b/beluga_amcl/include/beluga_amcl/models.hpp index cbfbbd1d6..76ff06e96 100644 --- a/beluga_amcl/include/beluga_amcl/models.hpp +++ b/beluga_amcl/include/beluga_amcl/models.hpp @@ -23,6 +23,9 @@ #include #include +#include +#include + #include #include #include @@ -32,8 +35,10 @@ #include #include +#include #include #include +#include namespace beluga_amcl { @@ -62,19 +67,37 @@ struct Pose // TODO(nahuel): Replace this with a real motion model. template -class MockMotionModel : public Mixin +class StationaryMotionModel : public Mixin { public: template - explicit MockMotionModel(Args && ... args) + explicit StationaryMotionModel(Args && ... args) : Mixin(std::forward(args)...) {} - [[nodiscard]] auto apply_motion(const Pose & state) {return state;} + [[nodiscard]] auto apply_motion(const Pose & state) const + { + static thread_local std::mt19937 generator{std::random_device()()}; + auto distribution = std::normal_distribution<>{0, 0.02}; + auto pose = state; + pose.x += distribution(generator); + pose.y += distribution(generator); + pose.theta = wrap_angle(pose.theta + distribution(generator)); + return pose; + } + +private: + static double wrap_angle(double angle) + { + // Wrap angle between -pi and pi + angle = std::fmod(angle + M_PI, 2.0 * M_PI); + return angle <= 0.0 ? angle + M_PI : angle - M_PI; + } }; struct LikelihoodSensorModelParam { double max_obstacle_distance; + double min_laser_distance; double max_laser_distance; double z_hit; double z_random; @@ -95,34 +118,82 @@ class LikelihoodSensorModel : public Mixin 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 &) + [[nodiscard]] double importance_weight(const Pose & pose) const { - // TODO(nahuel): Implement likelihood estimation. - return 1.; + auto x_offset = pose.x - map_metadata_.origin.position.x; + auto y_offset = pose.y - map_metadata_.origin.position.y; + const auto lock = std::shared_lock(points_mutex_); + /* *INDENT-OFF* Avoid uncrustify reformat */ + return std::transform_reduce( + points_.cbegin(), points_.cend(), 0.0, std::plus{}, + [this, pose, x_offset, y_offset](const auto & point) { + auto x = point.first * std::cos(pose.theta) + point.second * std::sin(pose.theta); + auto y = point.first * std::sin(pose.theta) + point.second * std::cos(pose.theta); + x = (x + x_offset) / map_metadata_.resolution; + y = (y + y_offset) / map_metadata_.resolution; + auto x_index = static_cast(std::floor(x)); + auto y_index = static_cast(std::floor(y)); + if (x_index >= map_metadata_.width || y_index >= map_metadata_.height) { + return 0.; + } + return likelihood_field_[x_index + y_index * map_metadata_.width]; + }); + /* *INDENT-ON* */ } template - [[nodiscard]] auto generate_random_state(Generator & generator) + [[nodiscard]] auto generate_random_state(Generator & generator) const { + auto index_distribution = std::uniform_int_distribution{0, free_cells_.size() - 1}; + auto theta_distribution = std::uniform_real_distribution{-M_PI, M_PI}; 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); + auto index = free_cells_[index_distribution(generator)]; + pose.x = static_cast(index % map_metadata_.width) + 0.5; + pose.y = static_cast(index / map_metadata_.width) + 0.5; + pose.x = pose.x * map_metadata_.resolution + map_metadata_.origin.position.x; + pose.y = pose.y * map_metadata_.resolution + map_metadata_.origin.position.y; + pose.theta = theta_distribution(generator); return pose; } + void update_sensor( + const sensor_msgs::msg::LaserScan & laser_scan, + const geometry_msgs::msg::TransformStamped & laser_transform) + { + auto base_transform = tf2::Transform{}; + tf2::convert(laser_transform.transform, base_transform); + auto range_min = std::max(laser_scan.range_min, static_cast(params_.min_laser_distance)); + auto range_max = std::min(laser_scan.range_max, static_cast(params_.max_laser_distance)); + auto point_buffer = decltype(points_) {}; + point_buffer.reserve(laser_scan.ranges.size()); + for (std::size_t index = 0; index < laser_scan.ranges.size(); ++index) { + auto range = laser_scan.ranges[index]; + if (std::isnan(range) || range < range_min || range > range_max) { + continue; + } + // Store points in the robot's reference frame + using ScalarType = decltype(laser_scan.angle_increment); + auto angle = laser_scan.angle_min + static_cast(index) * + laser_scan.angle_increment; + auto point = base_transform * tf2::Vector3{ + range * std::cos(angle), + range * std::sin(angle), + 0.0}; + point_buffer.emplace_back(point.x(), point.y()); + } + const auto lock = std::lock_guard(points_mutex_); + points_ = std::move(point_buffer); + } + auto get_likelihood_field_as_gridmap() const { + const std::size_t size = map_metadata_.width * map_metadata_.height; 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); } @@ -133,10 +204,10 @@ class LikelihoodSensorModel : public Mixin 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}; std::vector likelihood_field_; + std::vector> points_; + mutable std::shared_mutex points_mutex_; static std::vector make_free_cells_list(const nav_msgs::msg::OccupancyGrid & map) { @@ -203,15 +274,6 @@ class LikelihoodSensorModel : public Mixin 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.width) + 0.5) * info.resolution + info.origin.position.x, - (static_cast(index / info.width) + 0.5) * info.resolution + info.origin.position.y); - } }; } // namespace beluga_amcl diff --git a/beluga_amcl/package.xml b/beluga_amcl/package.xml index 64453f951..ca963e195 100644 --- a/beluga_amcl/package.xml +++ b/beluga_amcl/package.xml @@ -20,6 +20,10 @@ rclcpp rclcpp_components rclcpp_lifecycle + sensor_msgs + tf2 + tf2_geometry_msgs + tf2_ros ament_cmake_gmock ament_cmake_gtest diff --git a/beluga_amcl/src/amcl_node.cpp b/beluga_amcl/src/amcl_node.cpp index 3b2ff24f1..97d4783e3 100644 --- a/beluga_amcl/src/amcl_node.cpp +++ b/beluga_amcl/src/amcl_node.cpp @@ -14,12 +14,18 @@ #include +#include +#include +#include + #include #include #include +#include #include #include +#include namespace beluga_amcl { @@ -36,6 +42,18 @@ AmclNode::AmclNode(const rclcpp::NodeOptions & options) declare_parameter("global_frame_id", rclcpp::ParameterValue("map"), descriptor); } + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = "The name of the coordinate frame to use for odometry."; + declare_parameter("odom_frame_id", rclcpp::ParameterValue("odom"), descriptor); + } + + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = "The name of the coordinate frame to use for the robot base."; + declare_parameter("base_frame_id", rclcpp::ParameterValue("base_footprint"), descriptor); + } + { auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); descriptor.description = @@ -43,6 +61,13 @@ AmclNode::AmclNode(const rclcpp::NodeOptions & options) declare_parameter("map_topic", rclcpp::ParameterValue("map"), descriptor); } + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = + "Topic to subscribe to in order to receive the laser scan for localization."; + declare_parameter("scan_topic", rclcpp::ParameterValue("scan"), descriptor); + } + { auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); descriptor.description = "Minimum allowed number of particles."; @@ -264,6 +289,27 @@ AmclNode::CallbackReturn AmclNode::on_activate(const rclcpp_lifecycle::State &) RCLCPP_INFO(get_logger(), "Subscribed to map_topic: %s", map_sub_->get_topic_name()); + tf_buffer_ = std::make_unique(get_clock()); + tf_buffer_->setCreateTimerInterface( + std::make_shared( + get_node_base_interface(), + get_node_timers_interface())); + tf_broadcaster_ = std::make_unique(shared_from_this()); + tf_listener_ = std::make_unique(*tf_buffer_); + + laser_scan_sub_ = std::make_unique>( + shared_from_this(), get_parameter("scan_topic").as_string(), rmw_qos_profile_sensor_data); + + laser_scan_filter_ = std::make_unique>( + *laser_scan_sub_, *tf_buffer_, get_parameter("base_frame_id").as_string(), 50, + get_node_logging_interface(), + get_node_clock_interface(), + tf2::durationFromSec(1.0)); + + laser_scan_connection_ = laser_scan_filter_->registerCallback( + std::bind(&AmclNode::laser_callback, this, std::placeholders::_1)); + return CallbackReturn::SUCCESS; } @@ -272,9 +318,12 @@ AmclNode::CallbackReturn AmclNode::on_deactivate(const rclcpp_lifecycle::State & RCLCPP_INFO(get_logger(), "Deactivating"); particle_cloud_pub_->on_deactivate(); likelihood_field_pub_->on_deactivate(); + laser_scan_sub_.reset(); map_sub_.reset(); bond_.reset(); - + tf_listener_.reset(); + tf_broadcaster_.reset(); + tf_buffer_.reset(); return CallbackReturn::SUCCESS; } @@ -309,6 +358,7 @@ void AmclNode::map_callback(nav_msgs::msg::OccupancyGrid::SharedPtr map) auto likelihood_model_params = LikelihoodSensorModelParam{ get_parameter("laser_likelihood_max_dist").as_double(), + get_parameter("laser_min_range").as_double(), get_parameter("laser_max_range").as_double(), get_parameter("z_hit").as_double(), get_parameter("z_rand").as_double(), @@ -353,6 +403,26 @@ void AmclNode::timer_callback() } } +void AmclNode::laser_callback(sensor_msgs::msg::LaserScan::ConstSharedPtr laser_scan) +{ + if (!particle_filter_) { + return; + } + + try { + using namespace std::chrono_literals; + auto transform_stamped = tf_buffer_->lookupTransform( + get_parameter("base_frame_id").as_string(), + laser_scan->header.frame_id, + laser_scan->header.stamp, + 0s); + particle_filter_->update_sensor(*laser_scan, transform_stamped); + particle_filter_->update(); + } catch (const tf2::TransformException & error) { + RCLCPP_ERROR(get_logger(), "Could not transform laser: %s", error.what()); + } +} + } // namespace beluga_amcl RCLCPP_COMPONENTS_REGISTER_NODE(beluga_amcl::AmclNode) diff --git a/beluga_example/config/params.yaml b/beluga_example/config/params.yaml index 377ece65c..2c440362b 100644 --- a/beluga_example/config/params.yaml +++ b/beluga_example/config/params.yaml @@ -1,6 +1,8 @@ amcl: ros__parameters: global_frame_id: map + odom_frame_id: odom + base_frame_id: base map_topic: map max_particles: 2000 min_particles: 500 diff --git a/beluga_example/models/turtlebot.model.yaml b/beluga_example/models/turtlebot.model.yaml index f7dfaa88c..cd9e51d91 100644 --- a/beluga_example/models/turtlebot.model.yaml +++ b/beluga_example/models/turtlebot.model.yaml @@ -9,29 +9,14 @@ bodies: center: [-1, 0.0] density: 1.0 - - type: polygon - sensor: true - points: [[-.45, -.05], [-.45, 0.05], [-.35, 0.05], [-.35, -0.05]] - layers: [] - density: 1.0 - - - type: polygon - sensor: true - points: [[-.125, -.4], [-.125, -.3], [.125, -.3], [.125, -.4]] - density: 1.0 - - - type: polygon - sensor: true - points: [[-.125, .4], [-.125, .3], [.125, .3], [.125, .4]] - density: 100000 - plugins: - type: DiffDrive name: turtlebot_drive body: base twist_sub: cmd_vel - odom_frame_id: map + odom_frame_id: odom pub_rate: 10 + enable_odom_pub: true - type: ModelTfPublisher name: tf_publisher From 876224c4f6d3eacde33415d7821f73a6e22a01d6 Mon Sep 17 00:00:00 2001 From: Nahuel Espinosa Date: Wed, 7 Dec 2022 12:59:27 +0000 Subject: [PATCH 2/3] Address code review comments --- beluga_amcl/include/beluga_amcl/models.hpp | 37 ++++++++++++---------- 1 file changed, 21 insertions(+), 16 deletions(-) diff --git a/beluga_amcl/include/beluga_amcl/models.hpp b/beluga_amcl/include/beluga_amcl/models.hpp index 76ff06e96..0cc311583 100644 --- a/beluga_amcl/include/beluga_amcl/models.hpp +++ b/beluga_amcl/include/beluga_amcl/models.hpp @@ -123,19 +123,19 @@ class LikelihoodSensorModel : public Mixin [[nodiscard]] double importance_weight(const Pose & pose) const { - auto x_offset = pose.x - map_metadata_.origin.position.x; - auto y_offset = pose.y - map_metadata_.origin.position.y; + const auto x_offset = pose.x - map_metadata_.origin.position.x; + const auto y_offset = pose.y - map_metadata_.origin.position.y; const auto lock = std::shared_lock(points_mutex_); /* *INDENT-OFF* Avoid uncrustify reformat */ return std::transform_reduce( points_.cbegin(), points_.cend(), 0.0, std::plus{}, [this, pose, x_offset, y_offset](const auto & point) { - auto x = point.first * std::cos(pose.theta) + point.second * std::sin(pose.theta); + auto x = point.first * std::cos(pose.theta) - point.second * std::sin(pose.theta); auto y = point.first * std::sin(pose.theta) + point.second * std::cos(pose.theta); x = (x + x_offset) / map_metadata_.resolution; y = (y + y_offset) / map_metadata_.resolution; - auto x_index = static_cast(std::floor(x)); - auto y_index = static_cast(std::floor(y)); + const auto x_index = static_cast(std::floor(x)); + const auto y_index = static_cast(std::floor(y)); if (x_index >= map_metadata_.width || y_index >= map_metadata_.height) { return 0.; } @@ -150,7 +150,7 @@ class LikelihoodSensorModel : public Mixin auto index_distribution = std::uniform_int_distribution{0, free_cells_.size() - 1}; auto theta_distribution = std::uniform_real_distribution{-M_PI, M_PI}; auto pose = Pose{}; - auto index = free_cells_[index_distribution(generator)]; + const auto index = free_cells_[index_distribution(generator)]; pose.x = static_cast(index % map_metadata_.width) + 0.5; pose.y = static_cast(index / map_metadata_.width) + 0.5; pose.x = pose.x * map_metadata_.resolution + map_metadata_.origin.position.x; @@ -165,20 +165,21 @@ class LikelihoodSensorModel : public Mixin { auto base_transform = tf2::Transform{}; tf2::convert(laser_transform.transform, base_transform); - auto range_min = std::max(laser_scan.range_min, static_cast(params_.min_laser_distance)); - auto range_max = std::min(laser_scan.range_max, static_cast(params_.max_laser_distance)); + const float range_min = + std::max(laser_scan.range_min, static_cast(params_.min_laser_distance)); + const float range_max = + std::min(laser_scan.range_max, static_cast(params_.max_laser_distance)); auto point_buffer = decltype(points_) {}; point_buffer.reserve(laser_scan.ranges.size()); for (std::size_t index = 0; index < laser_scan.ranges.size(); ++index) { - auto range = laser_scan.ranges[index]; + float range = laser_scan.ranges[index]; if (std::isnan(range) || range < range_min || range > range_max) { continue; } // Store points in the robot's reference frame - using ScalarType = decltype(laser_scan.angle_increment); - auto angle = laser_scan.angle_min + static_cast(index) * - laser_scan.angle_increment; - auto point = base_transform * tf2::Vector3{ + const float angle = laser_scan.angle_min + + static_cast(index) * laser_scan.angle_increment; + const auto point = base_transform * tf2::Vector3{ range * std::cos(angle), range * std::sin(angle), 0.0}; @@ -206,6 +207,8 @@ class LikelihoodSensorModel : public Mixin std::vector free_cells_; std::vector likelihood_field_; + + // TODO(nahuel): Create a mixin type to ensure thread safety for sensor and motion models. std::vector> points_; mutable std::shared_mutex points_mutex_; @@ -238,9 +241,9 @@ class LikelihoodSensorModel : public Mixin 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 = + const auto delta_x = static_cast(first % width) - static_cast(second % width); - auto delta_y = + const 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, @@ -271,7 +274,9 @@ class LikelihoodSensorModel : public Mixin return amplitude * std::exp(-squared_distance / two_squared_sigma) + offset; }; - auto distance_map = nearest_obstacle_distance_map(obstacle_map, squared_distance, neighbors); + const auto distance_map = nearest_obstacle_distance_map( + obstacle_map, squared_distance, + neighbors); return distance_map | ranges::views::transform(to_likelihood) | ranges::to; } }; From ccd7eff12cb6206ea1a8ba648580275c5b37f11e Mon Sep 17 00:00:00 2001 From: Nahuel Espinosa Date: Wed, 7 Dec 2022 13:09:01 +0000 Subject: [PATCH 3/3] Scale map coordinates outside the loop --- beluga_amcl/include/beluga_amcl/models.hpp | 29 +++++++++++----------- beluga_amcl/src/amcl_node.cpp | 4 +-- 2 files changed, 16 insertions(+), 17 deletions(-) diff --git a/beluga_amcl/include/beluga_amcl/models.hpp b/beluga_amcl/include/beluga_amcl/models.hpp index 0cc311583..731d33c43 100644 --- a/beluga_amcl/include/beluga_amcl/models.hpp +++ b/beluga_amcl/include/beluga_amcl/models.hpp @@ -123,17 +123,16 @@ class LikelihoodSensorModel : public Mixin [[nodiscard]] double importance_weight(const Pose & pose) const { - const auto x_offset = pose.x - map_metadata_.origin.position.x; - const auto y_offset = pose.y - map_metadata_.origin.position.y; + // Compute offset in scaled map coordinates + const auto x_offset = (pose.x - map_metadata_.origin.position.x) / map_metadata_.resolution; + const auto y_offset = (pose.y - map_metadata_.origin.position.y) / map_metadata_.resolution; const auto lock = std::shared_lock(points_mutex_); /* *INDENT-OFF* Avoid uncrustify reformat */ return std::transform_reduce( points_.cbegin(), points_.cend(), 0.0, std::plus{}, [this, pose, x_offset, y_offset](const auto & point) { - auto x = point.first * std::cos(pose.theta) - point.second * std::sin(pose.theta); - auto y = point.first * std::sin(pose.theta) + point.second * std::cos(pose.theta); - x = (x + x_offset) / map_metadata_.resolution; - y = (y + y_offset) / map_metadata_.resolution; + const auto x = point.first * std::cos(pose.theta) - point.second * std::sin(pose.theta) + x_offset; + const auto y = point.first * std::sin(pose.theta) + point.second * std::cos(pose.theta) + y_offset; const auto x_index = static_cast(std::floor(x)); const auto y_index = static_cast(std::floor(y)); if (x_index >= map_metadata_.width || y_index >= map_metadata_.height) { @@ -161,10 +160,10 @@ class LikelihoodSensorModel : public Mixin void update_sensor( const sensor_msgs::msg::LaserScan & laser_scan, - const geometry_msgs::msg::TransformStamped & laser_transform) + const geometry_msgs::msg::TransformStamped & base_to_laser_stamped) { - auto base_transform = tf2::Transform{}; - tf2::convert(laser_transform.transform, base_transform); + auto base_to_laser_transform = tf2::Transform{}; + tf2::convert(base_to_laser_stamped.transform, base_to_laser_transform); const float range_min = std::max(laser_scan.range_min, static_cast(params_.min_laser_distance)); const float range_max = @@ -176,14 +175,16 @@ class LikelihoodSensorModel : public Mixin if (std::isnan(range) || range < range_min || range > range_max) { continue; } - // Store points in the robot's reference frame + // Store points in the robot's reference frame with scaled coordinates const float angle = laser_scan.angle_min + static_cast(index) * laser_scan.angle_increment; - const auto point = base_transform * tf2::Vector3{ + const auto point = base_to_laser_transform * tf2::Vector3{ range * std::cos(angle), range * std::sin(angle), 0.0}; - point_buffer.emplace_back(point.x(), point.y()); + point_buffer.emplace_back( + point.x() / map_metadata_.resolution, + point.y() / map_metadata_.resolution); } const auto lock = std::lock_guard(points_mutex_); points_ = std::move(point_buffer); @@ -205,7 +206,6 @@ class LikelihoodSensorModel : public Mixin LikelihoodSensorModelParam params_; nav_msgs::msg::MapMetaData map_metadata_; std::vector free_cells_; - std::vector likelihood_field_; // TODO(nahuel): Create a mixin type to ensure thread safety for sensor and motion models. @@ -275,8 +275,7 @@ class LikelihoodSensorModel : public Mixin }; const auto distance_map = nearest_obstacle_distance_map( - obstacle_map, squared_distance, - neighbors); + obstacle_map, squared_distance, neighbors); return distance_map | ranges::views::transform(to_likelihood) | ranges::to; } }; diff --git a/beluga_amcl/src/amcl_node.cpp b/beluga_amcl/src/amcl_node.cpp index 97d4783e3..1bdbe35d2 100644 --- a/beluga_amcl/src/amcl_node.cpp +++ b/beluga_amcl/src/amcl_node.cpp @@ -411,12 +411,12 @@ void AmclNode::laser_callback(sensor_msgs::msg::LaserScan::ConstSharedPtr laser_ try { using namespace std::chrono_literals; - auto transform_stamped = tf_buffer_->lookupTransform( + const auto laser_transform = tf_buffer_->lookupTransform( get_parameter("base_frame_id").as_string(), laser_scan->header.frame_id, laser_scan->header.stamp, 0s); - particle_filter_->update_sensor(*laser_scan, transform_stamped); + particle_filter_->update_sensor(*laser_scan, laser_transform); particle_filter_->update(); } catch (const tf2::TransformException & error) { RCLCPP_ERROR(get_logger(), "Could not transform laser: %s", error.what());