From 880ae55ef4c89c1d9af189ea796f34ecce0ff217 Mon Sep 17 00:00:00 2001 From: Jan Gutsche Date: Sun, 24 May 2026 17:22:50 +0200 Subject: [PATCH 1/6] Enhance CI workflow to support ARM64 architecture by adding a matrix strategy for OS --- .github/workflows/ci.yml | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 6318af683..fa3c508d0 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -6,7 +6,10 @@ on: jobs: build: - runs-on: ubuntu-latest + strategy: + matrix: + os: [ubuntu-latest, ubuntu-24.04-arm] + runs-on: ${{ matrix.os }} env: PSModulePath: "" # Otherwise colcon might think that PowerShell is being used From 58895eff314b9966409d281e9279df823b37131e Mon Sep 17 00:00:00 2001 From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com> Date: Mon, 25 May 2026 10:15:41 +0000 Subject: [PATCH 2/6] Fix ARM CI warnings in yesense and localization Agent-Logs-Url: https://github.com/bit-bots/bitbots_main/sessions/506bd666-9895-407c-b056-607f1ffb4b08 Co-authored-by: jaagut <34797331+jaagut@users.noreply.github.com> --- .../bitbots_localization/src/localization.cpp | 6 +++--- .../src/yesense_ros/yesense/src/yesense_driver.cpp | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/bitbots_navigation/bitbots_localization/src/localization.cpp b/src/bitbots_navigation/bitbots_localization/src/localization.cpp index 212dcb83c..72068570d 100644 --- a/src/bitbots_navigation/bitbots_localization/src/localization.cpp +++ b/src/bitbots_navigation/bitbots_localization/src/localization.cpp @@ -121,11 +121,11 @@ void Localization::updateParams(bool force_reload) { // Create standard particle probability distributions (e.g. for the initialization at the start of the game) robot_state_distribution_own_sidelines.reset(new RobotStateDistributionOwnSideline( - random_number_generator_, std::make_pair(field_dimensions_.x, field_dimensions_.y))); + random_number_generator_, std::pair{field_dimensions_.x, field_dimensions_.y})); robot_state_distribution_opponent_half.reset(new RobotStateDistributionOpponentHalf( - random_number_generator_, std::make_pair(field_dimensions_.x, field_dimensions_.y))); + random_number_generator_, std::pair{field_dimensions_.x, field_dimensions_.y})); robot_state_distribution_own_half_.reset(new RobotStateDistributionOwnHalf( - random_number_generator_, std::make_pair(field_dimensions_.x, field_dimensions_.y))); + random_number_generator_, std::pair{field_dimensions_.x, field_dimensions_.y})); // Create the resampling strategy resampling_.reset( diff --git a/src/lib/livelybot_hardware_sdk/src/yesense_ros/yesense/src/yesense_driver.cpp b/src/lib/livelybot_hardware_sdk/src/yesense_ros/yesense/src/yesense_driver.cpp index 073f5bcdd..49dae757e 100644 --- a/src/lib/livelybot_hardware_sdk/src/yesense_ros/yesense/src/yesense_driver.cpp +++ b/src/lib/livelybot_hardware_sdk/src/yesense_ros/yesense/src/yesense_driver.cpp @@ -1261,7 +1261,7 @@ void YesenseDriver::spin() uint16_t tid = 0x00; uint16_t prev_tid = 0x00; - uint32_t gps_header_sum; + uint32_t gps_header_sum = 0; while(configured_) { From 02e8a7b68483403fb137fabe9b8d5527ffa24f2d Mon Sep 17 00:00:00 2001 From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com> Date: Mon, 25 May 2026 11:17:48 +0000 Subject: [PATCH 3/6] Fix remaining ARM localization pair ABI notes Agent-Logs-Url: https://github.com/bit-bots/bitbots_main/sessions/5026fd65-fe24-4049-b9c6-6a5b176eee87 Co-authored-by: jaagut <34797331+jaagut@users.noreply.github.com> --- .../bitbots_localization/StateDistribution.hpp | 11 ++++++----- .../include/bitbots_localization/map.hpp | 4 ++-- .../bitbots_localization/src/StateDistribution.cpp | 8 ++++---- .../bitbots_localization/src/map.cpp | 8 ++++---- .../bitbots_localization/src/tools.cpp | 4 ++-- 5 files changed, 18 insertions(+), 17 deletions(-) diff --git a/src/bitbots_navigation/bitbots_localization/include/bitbots_localization/StateDistribution.hpp b/src/bitbots_navigation/bitbots_localization/include/bitbots_localization/StateDistribution.hpp index 9339d0f09..de9bf22d7 100644 --- a/src/bitbots_navigation/bitbots_localization/include/bitbots_localization/StateDistribution.hpp +++ b/src/bitbots_navigation/bitbots_localization/include/bitbots_localization/StateDistribution.hpp @@ -16,7 +16,8 @@ namespace bitbots_localization { class RobotStateDistribution : public particle_filter::StateDistribution { public: RobotStateDistribution(particle_filter::CRandomNumberGenerator& random_number_generator, - std::pair initial_robot_pose, std::pair field_size); + const std::pair& initial_robot_pose, + const std::pair& field_size); const RobotState draw() const override; @@ -32,7 +33,7 @@ class RobotStateDistribution : public particle_filter::StateDistribution { public: RobotStateDistributionStartLeft(particle_filter::CRandomNumberGenerator& random_number_generator, - std::pair field_size); + const std::pair& field_size); const RobotState draw() const override; @@ -44,7 +45,7 @@ class RobotStateDistributionStartLeft : public particle_filter::StateDistributio class RobotStateDistributionOwnSideline : public particle_filter::StateDistribution { public: RobotStateDistributionOwnSideline(particle_filter::CRandomNumberGenerator& random_number_generator, - std::pair field_size); + const std::pair& field_size); const RobotState draw() const override; @@ -56,7 +57,7 @@ class RobotStateDistributionOwnSideline : public particle_filter::StateDistribut class RobotStateDistributionOpponentHalf : public particle_filter::StateDistribution { public: RobotStateDistributionOpponentHalf(particle_filter::CRandomNumberGenerator& random_number_generator, - std::pair field_size); + const std::pair& field_size); const RobotState draw() const override; @@ -71,7 +72,7 @@ class RobotStateDistributionOpponentHalf : public particle_filter::StateDistribu class RobotStateDistributionOwnHalf : public particle_filter::StateDistribution { public: RobotStateDistributionOwnHalf(particle_filter::CRandomNumberGenerator& random_number_generator, - std::pair field_size); + const std::pair& field_size); const RobotState draw() const override; diff --git a/src/bitbots_navigation/bitbots_localization/include/bitbots_localization/map.hpp b/src/bitbots_navigation/bitbots_localization/include/bitbots_localization/map.hpp index e32edb572..6fd5d9bf1 100644 --- a/src/bitbots_navigation/bitbots_localization/include/bitbots_localization/map.hpp +++ b/src/bitbots_navigation/bitbots_localization/include/bitbots_localization/map.hpp @@ -45,8 +45,8 @@ class Map { double get_occupancy(double x, double y); - std::pair getObservationCoordinatesInMapFrame(std::pair observation, double stateX, - double stateY, double stateT); + std::pair getObservationCoordinatesInMapFrame(const std::pair& observation, + double stateX, double stateY, double stateT); nav_msgs::msg::OccupancyGrid get_map_msg(std::string frame_id, int threshold = -1); diff --git a/src/bitbots_navigation/bitbots_localization/src/StateDistribution.cpp b/src/bitbots_navigation/bitbots_localization/src/StateDistribution.cpp index 0e8ba1d49..d31d18170 100644 --- a/src/bitbots_navigation/bitbots_localization/src/StateDistribution.cpp +++ b/src/bitbots_navigation/bitbots_localization/src/StateDistribution.cpp @@ -7,7 +7,7 @@ namespace bitbots_localization { RobotStateDistributionStartLeft::RobotStateDistributionStartLeft( - particle_filter::CRandomNumberGenerator& random_number_generator, std::pair field_size) { + particle_filter::CRandomNumberGenerator& random_number_generator, const std::pair& field_size) { field_size = field_size; } @@ -24,7 +24,7 @@ const RobotState RobotStateDistributionStartLeft::draw() const { } RobotStateDistributionOwnSideline::RobotStateDistributionOwnSideline( - particle_filter::CRandomNumberGenerator& random_number_generator, std::pair field_size) { + particle_filter::CRandomNumberGenerator& random_number_generator, const std::pair& field_size) { field_x = field_size.first; field_y = field_size.second; } @@ -42,7 +42,7 @@ const RobotState RobotStateDistributionOwnSideline::draw() const { } RobotStateDistributionOpponentHalf::RobotStateDistributionOpponentHalf( - particle_filter::CRandomNumberGenerator& random_number_generator, std::pair field_size) + particle_filter::CRandomNumberGenerator& random_number_generator, const std::pair& field_size) : random_number_generator_(random_number_generator) { // only own half min_x_ = (field_size.first / 2.0) + 0.5; @@ -58,7 +58,7 @@ const RobotState RobotStateDistributionOpponentHalf::draw() const { } RobotStateDistributionOwnHalf::RobotStateDistributionOwnHalf( - particle_filter::CRandomNumberGenerator& random_number_generator, std::pair field_size) + particle_filter::CRandomNumberGenerator& random_number_generator, const std::pair& field_size) : random_number_generator_(random_number_generator) { // only own half min_x_ = -field_size.first / 2.0; diff --git a/src/bitbots_navigation/bitbots_localization/src/map.cpp b/src/bitbots_navigation/bitbots_localization/src/map.cpp index b990313a2..681a95b65 100644 --- a/src/bitbots_navigation/bitbots_localization/src/map.cpp +++ b/src/bitbots_navigation/bitbots_localization/src/map.cpp @@ -64,8 +64,8 @@ std::vector Map::provideRating(const RobotState& state, return rating; } -std::pair Map::getObservationCoordinatesInMapFrame(std::pair observation, double stateX, - double stateY, double stateT) { +std::pair Map::getObservationCoordinatesInMapFrame(const std::pair& observation, + double stateX, double stateY, double stateT) { // queries the Cartesian metric map coordinates for a given observation (in polar coordinates) // taken relative to a given state (in Cartesian coordinates) // Input: Observation coordinates in polar coordinates, state coordinates in Cartesian coordinates @@ -75,8 +75,8 @@ std::pair Map::getObservationCoordinatesInMapFrame(std::pair observationWithTheta = polarToCartesian(observation.first + stateT, observation.second); // add to particle - std::pair observationRelative = - std::make_pair(stateX + observationWithTheta.first, stateY + observationWithTheta.second); + std::pair observationRelative{ + stateX + observationWithTheta.first, stateY + observationWithTheta.second}; return observationRelative; // in cartesian } diff --git a/src/bitbots_navigation/bitbots_localization/src/tools.cpp b/src/bitbots_navigation/bitbots_localization/src/tools.cpp index abcc12116..921f6e677 100644 --- a/src/bitbots_navigation/bitbots_localization/src/tools.cpp +++ b/src/bitbots_navigation/bitbots_localization/src/tools.cpp @@ -7,14 +7,14 @@ std::pair cartesianToPolar(double x, double y) { double t = atan2(y, x); - return std::make_pair(t, r); + return {t, r}; } std::pair polarToCartesian(double t, double r) { double x = r * std::cos(t); double y = r * std::sin(t); - return std::make_pair(x, y); + return {x, y}; } double signedAngle(double angle) { From 35031cecc324b00034e7e16269646efa2c4c2e3c Mon Sep 17 00:00:00 2001 From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com> Date: Tue, 26 May 2026 20:12:53 +0000 Subject: [PATCH 4/6] Fix StartLeft constructor member initialization --- .../bitbots_localization/src/StateDistribution.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/bitbots_navigation/bitbots_localization/src/StateDistribution.cpp b/src/bitbots_navigation/bitbots_localization/src/StateDistribution.cpp index d31d18170..0521bb4de 100644 --- a/src/bitbots_navigation/bitbots_localization/src/StateDistribution.cpp +++ b/src/bitbots_navigation/bitbots_localization/src/StateDistribution.cpp @@ -7,9 +7,8 @@ namespace bitbots_localization { RobotStateDistributionStartLeft::RobotStateDistributionStartLeft( - particle_filter::CRandomNumberGenerator& random_number_generator, const std::pair& field_size) { - field_size = field_size; -} + particle_filter::CRandomNumberGenerator& random_number_generator, const std::pair& field_size) + : random_number_generator_(random_number_generator), field_size(field_size) {} const RobotState RobotStateDistributionStartLeft::draw() const { if (random_number_generator_.getUniform(0, 1) > 0.5) { From 11d219936aa8f29b2c00ec1adac4463a8d1b0ff9 Mon Sep 17 00:00:00 2001 From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com> Date: Tue, 26 May 2026 20:46:10 +0000 Subject: [PATCH 5/6] Fix ARM64 ABI notes: replace std::pair in function signatures --- .../StateDistribution.hpp | 5 +++-- .../include/bitbots_localization/map.hpp | 4 ++-- .../include/bitbots_localization/tools.hpp | 4 ++-- .../bitbots_localization/src/MotionModel.cpp | 8 +++++--- .../src/ObservationModel.cpp | 10 ++++++---- .../src/StateDistribution.cpp | 12 +++++------ .../bitbots_localization/src/localization.cpp | 12 +++++------ .../bitbots_localization/src/map.cpp | 20 +++++++++---------- .../bitbots_localization/src/tools.cpp | 17 ++++++---------- 9 files changed, 46 insertions(+), 46 deletions(-) diff --git a/src/bitbots_navigation/bitbots_localization/include/bitbots_localization/StateDistribution.hpp b/src/bitbots_navigation/bitbots_localization/include/bitbots_localization/StateDistribution.hpp index de9bf22d7..f5ecda7bc 100644 --- a/src/bitbots_navigation/bitbots_localization/include/bitbots_localization/StateDistribution.hpp +++ b/src/bitbots_navigation/bitbots_localization/include/bitbots_localization/StateDistribution.hpp @@ -33,13 +33,14 @@ class RobotStateDistribution : public particle_filter::StateDistribution { public: RobotStateDistributionStartLeft(particle_filter::CRandomNumberGenerator& random_number_generator, - const std::pair& field_size); + double field_size_x, double field_size_y); const RobotState draw() const override; private: particle_filter::CRandomNumberGenerator random_number_generator_; - std::pair field_size; + double field_size_x_; + double field_size_y_; }; class RobotStateDistributionOwnSideline : public particle_filter::StateDistribution { diff --git a/src/bitbots_navigation/bitbots_localization/include/bitbots_localization/map.hpp b/src/bitbots_navigation/bitbots_localization/include/bitbots_localization/map.hpp index 6fd5d9bf1..6553a29f7 100644 --- a/src/bitbots_navigation/bitbots_localization/include/bitbots_localization/map.hpp +++ b/src/bitbots_navigation/bitbots_localization/include/bitbots_localization/map.hpp @@ -45,8 +45,8 @@ class Map { double get_occupancy(double x, double y); - std::pair getObservationCoordinatesInMapFrame(const std::pair& observation, - double stateX, double stateY, double stateT); + void getObservationCoordinatesInMapFrame(double obs_angle, double obs_radius, double stateX, double stateY, + double stateT, double& result_x, double& result_y); nav_msgs::msg::OccupancyGrid get_map_msg(std::string frame_id, int threshold = -1); diff --git a/src/bitbots_navigation/bitbots_localization/include/bitbots_localization/tools.hpp b/src/bitbots_navigation/bitbots_localization/include/bitbots_localization/tools.hpp index 3c55b1d56..0b8c75db7 100644 --- a/src/bitbots_navigation/bitbots_localization/include/bitbots_localization/tools.hpp +++ b/src/bitbots_navigation/bitbots_localization/include/bitbots_localization/tools.hpp @@ -8,8 +8,8 @@ #include namespace bitbots_localization { -std::pair cartesianToPolar(double x, double y); -std::pair polarToCartesian(double t, double r); +void cartesianToPolar(double x, double y, double& angle, double& radius); +void polarToCartesian(double t, double r, double& x, double& y); double signedAngle(double angle_a, double angle_b); double signedAngle(double angle); } // namespace bitbots_localization diff --git a/src/bitbots_navigation/bitbots_localization/src/MotionModel.cpp b/src/bitbots_navigation/bitbots_localization/src/MotionModel.cpp index 85b385edb..809da15b7 100644 --- a/src/bitbots_navigation/bitbots_localization/src/MotionModel.cpp +++ b/src/bitbots_navigation/bitbots_localization/src/MotionModel.cpp @@ -20,7 +20,8 @@ RobotMotionModel::RobotMotionModel(const particle_filter::CRandomNumberGenerator void RobotMotionModel::drift(RobotState& state, geometry_msgs::msg::Vector3 linear_movement, geometry_msgs::msg::Vector3 rotational_movement) const { // Convert cartesian coordinates to polarcoordinates with an orientation - auto [polar_rot, polar_dist] = cartesianToPolar(linear_movement.x, linear_movement.y); + double polar_rot, polar_dist; + cartesianToPolar(linear_movement.x, linear_movement.y, polar_rot, polar_dist); // get the minimal absolute double orientation = signedAngle(rotational_movement.z); // Apply sample drift for odom data @@ -33,8 +34,9 @@ void RobotMotionModel::drift(RobotState& state, geometry_msgs::msg::Vector3 line // Convert polar coordinates with offset back to cartesian ones, while transforming it into the local frame of each // particle - auto [cartesian_with_offset_x, cartesian_with_offset_y] = - polarToCartesian(state.getTheta() + polar_rot_with_drift, polar_dist_with_drift); + double cartesian_with_offset_x, cartesian_with_offset_y; + polarToCartesian(state.getTheta() + polar_rot_with_drift, polar_dist_with_drift, cartesian_with_offset_x, + cartesian_with_offset_y); // Apply new values onto state state.setXPos(state.getXPos() + cartesian_with_offset_x); diff --git a/src/bitbots_navigation/bitbots_localization/src/ObservationModel.cpp b/src/bitbots_navigation/bitbots_localization/src/ObservationModel.cpp index 3a737f0f4..3a0f59c12 100644 --- a/src/bitbots_navigation/bitbots_localization/src/ObservationModel.cpp +++ b/src/bitbots_navigation/bitbots_localization/src/ObservationModel.cpp @@ -68,16 +68,18 @@ double RobotPoseObservationModel::measure(const RobotState& state) const { void RobotPoseObservationModel::set_measurement_lines_pc(sm::msg::PointCloud2 measurement) { // convert to polar for (sm::PointCloud2ConstIterator iter_xyz(measurement, "x"); iter_xyz != iter_xyz.end(); ++iter_xyz) { - std::pair linePolar = cartesianToPolar(iter_xyz[0], iter_xyz[1]); - last_measurement_lines_.push_back(linePolar); + double angle, radius; + cartesianToPolar(iter_xyz[0], iter_xyz[1], angle, radius); + last_measurement_lines_.emplace_back(angle, radius); } } void RobotPoseObservationModel::set_measurement_goalposts(sv3dm::msg::GoalpostArray measurement) { // convert to polar for (sv3dm::msg::Goalpost& post : measurement.posts) { - std::pair postPolar = cartesianToPolar(post.bb.center.position.x, post.bb.center.position.y); - last_measurement_goal_.push_back(postPolar); + double angle, radius; + cartesianToPolar(post.bb.center.position.x, post.bb.center.position.y, angle, radius); + last_measurement_goal_.emplace_back(angle, radius); } } diff --git a/src/bitbots_navigation/bitbots_localization/src/StateDistribution.cpp b/src/bitbots_navigation/bitbots_localization/src/StateDistribution.cpp index 0521bb4de..cf8889dd0 100644 --- a/src/bitbots_navigation/bitbots_localization/src/StateDistribution.cpp +++ b/src/bitbots_navigation/bitbots_localization/src/StateDistribution.cpp @@ -7,17 +7,17 @@ namespace bitbots_localization { RobotStateDistributionStartLeft::RobotStateDistributionStartLeft( - particle_filter::CRandomNumberGenerator& random_number_generator, const std::pair& field_size) - : random_number_generator_(random_number_generator), field_size(field_size) {} + particle_filter::CRandomNumberGenerator& random_number_generator, double field_size_x, double field_size_y) + : random_number_generator_(random_number_generator), field_size_x_(field_size_x), field_size_y_(field_size_y) {} const RobotState RobotStateDistributionStartLeft::draw() const { if (random_number_generator_.getUniform(0, 1) > 0.5) { - return (RobotState(random_number_generator_.getUniform(field_size.first / 2, 0.0), - random_number_generator_.getGaussian(0.1) - field_size.second / 2 - 0.1, + return (RobotState(random_number_generator_.getUniform(field_size_x_ / 2, 0.0), + random_number_generator_.getGaussian(0.1) - field_size_y_ / 2 - 0.1, random_number_generator_.getGaussian(0.2) - 1.57)); } else { - return (RobotState(random_number_generator_.getUniform(field_size.first / 2, 0.0), - random_number_generator_.getGaussian(0.1) + field_size.second / 2 + 0.1, + return (RobotState(random_number_generator_.getUniform(field_size_x_ / 2, 0.0), + random_number_generator_.getGaussian(0.1) + field_size_y_ / 2 + 0.1, random_number_generator_.getGaussian(0.2) + 1.57)); } } diff --git a/src/bitbots_navigation/bitbots_localization/src/localization.cpp b/src/bitbots_navigation/bitbots_localization/src/localization.cpp index 72068570d..7a820a118 100644 --- a/src/bitbots_navigation/bitbots_localization/src/localization.cpp +++ b/src/bitbots_navigation/bitbots_localization/src/localization.cpp @@ -517,15 +517,15 @@ void Localization::publish_debug_rating(const std::vector& measurement : measurements) { // lines are in polar form! - std::pair observationRelative; + double obs_x, obs_y; - observationRelative = map->getObservationCoordinatesInMapFrame(measurement, best_estimate.getXPos(), - best_estimate.getYPos(), best_estimate.getTheta()); - double occupancy = map->get_occupancy(observationRelative.first, observationRelative.second); + map->getObservationCoordinatesInMapFrame(measurement.first, measurement.second, best_estimate.getXPos(), + best_estimate.getYPos(), best_estimate.getTheta(), obs_x, obs_y); + double occupancy = map->get_occupancy(obs_x, obs_y); geometry_msgs::msg::Point point; - point.x = observationRelative.first; - point.y = observationRelative.second; + point.x = obs_x; + point.y = obs_y; std_msgs::msg::ColorRGBA color; color.b = 1; diff --git a/src/bitbots_navigation/bitbots_localization/src/map.cpp b/src/bitbots_navigation/bitbots_localization/src/map.cpp index 681a95b65..fc94a4c8d 100644 --- a/src/bitbots_navigation/bitbots_localization/src/map.cpp +++ b/src/bitbots_navigation/bitbots_localization/src/map.cpp @@ -53,32 +53,32 @@ std::vector Map::provideRating(const RobotState& state, std::vector rating; for (const std::pair& observation : observations) { // lines are in polar form! - std::pair lineRelative; + double line_x, line_y; // get rating per line - lineRelative = getObservationCoordinatesInMapFrame(observation, state.getXPos(), state.getYPos(), state.getTheta()); - double occupancy = get_occupancy(lineRelative.first, lineRelative.second); + getObservationCoordinatesInMapFrame(observation.first, observation.second, state.getXPos(), state.getYPos(), + state.getTheta(), line_x, line_y); + double occupancy = get_occupancy(line_x, line_y); rating.push_back(occupancy); } return rating; } -std::pair Map::getObservationCoordinatesInMapFrame(const std::pair& observation, - double stateX, double stateY, double stateT) { +void Map::getObservationCoordinatesInMapFrame(double obs_angle, double obs_radius, double stateX, double stateY, + double stateT, double& result_x, double& result_y) { // queries the Cartesian metric map coordinates for a given observation (in polar coordinates) // taken relative to a given state (in Cartesian coordinates) // Input: Observation coordinates in polar coordinates, state coordinates in Cartesian coordinates // Output: Observation coordinates in Cartesian coordinates in the map frame // add theta and convert back to cartesian - std::pair observationWithTheta = polarToCartesian(observation.first + stateT, observation.second); + double cart_x, cart_y; + polarToCartesian(obs_angle + stateT, obs_radius, cart_x, cart_y); // add to particle - std::pair observationRelative{ - stateX + observationWithTheta.first, stateY + observationWithTheta.second}; - - return observationRelative; // in cartesian + result_x = stateX + cart_x; + result_y = stateY + cart_y; } nav_msgs::msg::OccupancyGrid Map::get_map_msg(std::string frame_id, int threshold) { diff --git a/src/bitbots_navigation/bitbots_localization/src/tools.cpp b/src/bitbots_navigation/bitbots_localization/src/tools.cpp index 921f6e677..82a3502ca 100644 --- a/src/bitbots_navigation/bitbots_localization/src/tools.cpp +++ b/src/bitbots_navigation/bitbots_localization/src/tools.cpp @@ -2,19 +2,14 @@ namespace bitbots_localization { -std::pair cartesianToPolar(double x, double y) { - double r = hypot(x, y); - - double t = atan2(y, x); - - return {t, r}; +void cartesianToPolar(double x, double y, double& angle, double& radius) { + radius = hypot(x, y); + angle = atan2(y, x); } -std::pair polarToCartesian(double t, double r) { - double x = r * std::cos(t); - double y = r * std::sin(t); - - return {x, y}; +void polarToCartesian(double t, double r, double& x, double& y) { + x = r * std::cos(t); + y = r * std::sin(t); } double signedAngle(double angle) { From 65a9fbc9353c64eb88ee7c87722b2144ff3a9209 Mon Sep 17 00:00:00 2001 From: Jan Gutsche Date: Tue, 26 May 2026 23:08:16 +0200 Subject: [PATCH 6/6] Format code --- .../include/bitbots_localization/StateDistribution.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/bitbots_navigation/bitbots_localization/include/bitbots_localization/StateDistribution.hpp b/src/bitbots_navigation/bitbots_localization/include/bitbots_localization/StateDistribution.hpp index f5ecda7bc..66d0e0933 100644 --- a/src/bitbots_navigation/bitbots_localization/include/bitbots_localization/StateDistribution.hpp +++ b/src/bitbots_navigation/bitbots_localization/include/bitbots_localization/StateDistribution.hpp @@ -32,8 +32,8 @@ class RobotStateDistribution : public particle_filter::StateDistribution { public: - RobotStateDistributionStartLeft(particle_filter::CRandomNumberGenerator& random_number_generator, - double field_size_x, double field_size_y); + RobotStateDistributionStartLeft(particle_filter::CRandomNumberGenerator& random_number_generator, double field_size_x, + double field_size_y); const RobotState draw() const override;