Fix ARM64 ABI notes: eliminate std::pair<double,double> from function signatures in bitbots_localization

.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
 

src/bitbots_navigation/bitbots_localization/include/bitbots_localization/StateDistribution.hpp

@@ -16,7 +16,8 @@ namespace bitbots_localization {
 class RobotStateDistribution : public particle_filter::StateDistribution<RobotState> {
  public:
   RobotStateDistribution(particle_filter::CRandomNumberGenerator& random_number_generator,
-                         std::pair<double, double> initial_robot_pose, std::pair<double, double> field_size);
+                         const std::pair<double, double>& initial_robot_pose,
+                         const std::pair<double, double>& field_size);
 
   const RobotState draw() const override;
 
@@ -31,20 +32,21 @@ class RobotStateDistribution : public particle_filter::StateDistribution<RobotSt
 
 class RobotStateDistributionStartLeft : public particle_filter::StateDistribution<RobotState> {
  public:
-  RobotStateDistributionStartLeft(particle_filter::CRandomNumberGenerator& random_number_generator,
-                                  std::pair<double, double> field_size);
+  RobotStateDistributionStartLeft(particle_filter::CRandomNumberGenerator& random_number_generator, double field_size_x,
+                                  double field_size_y);
 
   const RobotState draw() const override;
 
  private:
   particle_filter::CRandomNumberGenerator random_number_generator_;
-  std::pair<double, double> field_size;
+  double field_size_x_;
+  double field_size_y_;
 };
 
 class RobotStateDistributionOwnSideline : public particle_filter::StateDistribution<RobotState> {
  public:
   RobotStateDistributionOwnSideline(particle_filter::CRandomNumberGenerator& random_number_generator,
-                                    std::pair<double, double> field_size);
+                                    const std::pair<double, double>& field_size);
 
   const RobotState draw() const override;
 
@@ -56,7 +58,7 @@ class RobotStateDistributionOwnSideline : public particle_filter::StateDistribut
 class RobotStateDistributionOpponentHalf : public particle_filter::StateDistribution<RobotState> {
  public:
   RobotStateDistributionOpponentHalf(particle_filter::CRandomNumberGenerator& random_number_generator,
-                                     std::pair<double, double> field_size);
+                                     const std::pair<double, double>& field_size);
 
   const RobotState draw() const override;
 
@@ -71,7 +73,7 @@ class RobotStateDistributionOpponentHalf : public particle_filter::StateDistribu
 class RobotStateDistributionOwnHalf : public particle_filter::StateDistribution<RobotState> {
  public:
   RobotStateDistributionOwnHalf(particle_filter::CRandomNumberGenerator& random_number_generator,
-                                std::pair<double, double> field_size);
+                                const std::pair<double, double>& field_size);
 
   const RobotState draw() const override;
 

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<double, double> getObservationCoordinatesInMapFrame(std::pair<double, double> 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);
 

src/bitbots_navigation/bitbots_localization/include/bitbots_localization/tools.hpp

@@ -8,8 +8,8 @@
 #include <vector>
 
 namespace bitbots_localization {
-std::pair<double, double> cartesianToPolar(double x, double y);
-std::pair<double, double> 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

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);

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<float> iter_xyz(measurement, "x"); iter_xyz != iter_xyz.end(); ++iter_xyz) {
-    std::pair<double, double> 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<double, double> 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);
   }
 }
 

src/bitbots_navigation/bitbots_localization/src/StateDistribution.cpp

@@ -7,24 +7,23 @@
 namespace bitbots_localization {
 
 RobotStateDistributionStartLeft::RobotStateDistributionStartLeft(
-    particle_filter::CRandomNumberGenerator& random_number_generator, std::pair<double, double> field_size) {
-  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));
   }
 }
 
 RobotStateDistributionOwnSideline::RobotStateDistributionOwnSideline(
-    particle_filter::CRandomNumberGenerator& random_number_generator, std::pair<double, double> field_size) {
+    particle_filter::CRandomNumberGenerator& random_number_generator, const std::pair<double, double>& field_size) {
   field_x = field_size.first;
   field_y = field_size.second;
 }
@@ -42,7 +41,7 @@ const RobotState RobotStateDistributionOwnSideline::draw() const {
 }
 
 RobotStateDistributionOpponentHalf::RobotStateDistributionOpponentHalf(
-    particle_filter::CRandomNumberGenerator& random_number_generator, std::pair<double, double> field_size)
+    particle_filter::CRandomNumberGenerator& random_number_generator, const std::pair<double, double>& field_size)
     : random_number_generator_(random_number_generator) {
   // only own half
   min_x_ = (field_size.first / 2.0) + 0.5;
@@ -58,7 +57,7 @@ const RobotState RobotStateDistributionOpponentHalf::draw() const {
 }
 
 RobotStateDistributionOwnHalf::RobotStateDistributionOwnHalf(
-    particle_filter::CRandomNumberGenerator& random_number_generator, std::pair<double, double> field_size)
+    particle_filter::CRandomNumberGenerator& random_number_generator, const std::pair<double, double>& field_size)
     : random_number_generator_(random_number_generator) {
   // only own half
   min_x_ = -field_size.first / 2.0;

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<double, double>{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<double, double>{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<double, double>{field_dimensions_.x, field_dimensions_.y}));
 
   // Create the resampling strategy
   resampling_.reset(
@@ -517,15 +517,15 @@ void Localization::publish_debug_rating(const std::vector<std::pair<double, doub
 
   for (const std::pair<double, double>& measurement : measurements) {
     // lines are in polar form!
-    std::pair<double, double> 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;

src/bitbots_navigation/bitbots_localization/src/map.cpp

@@ -53,32 +53,32 @@ std::vector<double> Map::provideRating(const RobotState& state,
   std::vector<double> rating;
   for (const std::pair<double, double>& observation : observations) {
     // lines are in polar form!
-    std::pair<double, double> 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<double, double> Map::getObservationCoordinatesInMapFrame(std::pair<double, double> 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<double, double> 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<double, double> observationRelative =
-      std::make_pair(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) {

src/bitbots_navigation/bitbots_localization/src/tools.cpp

@@ -2,19 +2,14 @@
 
 namespace bitbots_localization {
 
-std::pair<double, double> cartesianToPolar(double x, double y) {
-  double r = hypot(x, y);
-
-  double t = atan2(y, x);
-
-  return std::make_pair(t, r);
+void cartesianToPolar(double x, double y, double& angle, double& radius) {
+  radius = hypot(x, y);
+  angle = atan2(y, x);
 }
 
-std::pair<double, double> polarToCartesian(double t, double r) {
-  double x = r * std::cos(t);
-  double y = r * std::sin(t);
-
-  return std::make_pair(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) {

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_)
     {