Add ARM-CI and fix compile warnings

.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,32 +32,34 @@ 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);
+                                    double field_size_x, double field_size_y);
 
   const RobotState draw() const override;
 
  private:
   particle_filter::CRandomNumberGenerator random_number_generator_;
-  double field_x, field_y;
+  double field_size_x_;
+  double field_size_y_;
 };
 
 class RobotStateDistributionOpponentHalf : public particle_filter::StateDistribution<RobotState> {
  public:
   RobotStateDistributionOpponentHalf(particle_filter::CRandomNumberGenerator& random_number_generator,
-                                     std::pair<double, double> field_size);
+                                     double field_size_x, double field_size_y);
 
   const RobotState draw() const override;
 
@@ -70,8 +73,8 @@ 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);
+  RobotStateDistributionOwnHalf(particle_filter::CRandomNumberGenerator& random_number_generator, double field_size_x,
+                                double field_size_y);
 
   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);
+  CartesianCoordinates getObservationCoordinatesInMapFrame(PolarCoordinates observation, double stateX, double stateY,
+                                                           double stateT);
 
   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,20 @@
 #include <vector>
 
 namespace bitbots_localization {
-std::pair<double, double> cartesianToPolar(double x, double y);
-std::pair<double, double> polarToCartesian(double t, double r);
+struct PolarCoordinates {
+  double angle;
+  double radius;
+};
+
+struct CartesianCoordinates {
+  double x;
+  double y;
+};
+
+PolarCoordinates cartesianToPolar(CartesianCoordinates coordinates);
+PolarCoordinates cartesianToPolar(double x, double y);
+CartesianCoordinates polarToCartesian(PolarCoordinates coordinates);
+CartesianCoordinates polarToCartesian(double angle, double radius);
 double signedAngle(double angle_a, double angle_b);
 double signedAngle(double angle);
 }  // namespace bitbots_localization

src/bitbots_navigation/bitbots_localization/src/MotionModel.cpp

@@ -19,26 +19,27 @@ 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);
+  // Convert cartesian coordinates to polar coordinates with an orientation
+  const PolarCoordinates polar_movement = cartesianToPolar({linear_movement.x, linear_movement.y});
   // get the minimal absolute
   double orientation = signedAngle(rotational_movement.z);
   // Apply sample drift for odom data
   // no need for abs polar distance, because its positive every time
-  double polar_rot_with_drift = polar_rot - sample(drift_cov_(0, 0) * polar_dist + drift_cov_(0, 1) * abs(orientation));
+  double polar_rot_with_drift =
+      polar_movement.angle - sample(drift_cov_(0, 0) * polar_movement.radius + drift_cov_(0, 1) * abs(orientation));
   double polar_dist_with_drift =
-      polar_dist - sample(drift_cov_(1, 0) * polar_dist + drift_cov_(1, 1) * abs(orientation));
+      polar_movement.radius - sample(drift_cov_(1, 0) * polar_movement.radius + drift_cov_(1, 1) * abs(orientation));
   double orientation_with_drift =
-      orientation - sample(drift_cov_(2, 0) * polar_dist + drift_cov_(2, 1) * abs(orientation));
+      orientation - sample(drift_cov_(2, 0) * polar_movement.radius + drift_cov_(2, 1) * abs(orientation));
 
   // 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);
+  const CartesianCoordinates cartesian_with_offset =
+      polarToCartesian({state.getTheta() + polar_rot_with_drift, polar_dist_with_drift});
 
   // Apply new values onto state
-  state.setXPos(state.getXPos() + cartesian_with_offset_x);
-  state.setYPos(state.getYPos() + cartesian_with_offset_y);
+  state.setXPos(state.getXPos() + cartesian_with_offset.x);
+  state.setYPos(state.getYPos() + cartesian_with_offset.y);
   double theta = state.getTheta() + orientation_with_drift;
 
   state.setTheta(theta);

src/bitbots_navigation/bitbots_localization/src/ObservationModel.cpp

@@ -68,16 +68,16 @@ 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);
+    const PolarCoordinates measurement_polar = cartesianToPolar(iter_xyz[0], iter_xyz[1]);
+    last_measurement_lines_.emplace_back(measurement_polar.angle, measurement_polar.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);
+    const PolarCoordinates measurement_polar = cartesianToPolar(post.bb.center.position.x, post.bb.center.position.y);
+    last_measurement_goal_.emplace_back(measurement_polar.angle, measurement_polar.radius);
   }
 }
 

src/bitbots_navigation/bitbots_localization/src/StateDistribution.cpp

@@ -7,48 +7,45 @@
 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) {
-  field_x = field_size.first;
-  field_y = field_size.second;
-}
+    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 RobotStateDistributionOwnSideline::draw() const {
   if (random_number_generator_.getUniform(0, 1) > 0.5) {
-    return (RobotState(random_number_generator_.getUniform(-field_x / 2, 0.0),
-                       random_number_generator_.getGaussian(0.1) - field_y / 2,
+    return (RobotState(random_number_generator_.getUniform(-field_size_x_ / 2, 0.0),
+                       random_number_generator_.getGaussian(0.1) - field_size_y_ / 2,
                        random_number_generator_.getGaussian(0.2) + 1.57));
   } else {
-    return (RobotState(random_number_generator_.getUniform(-field_x / 2, 0.0),
-                       random_number_generator_.getGaussian(0.1) + field_y / 2,
+    return (RobotState(random_number_generator_.getUniform(-field_size_x_ / 2, 0.0),
+                       random_number_generator_.getGaussian(0.1) + field_size_y_ / 2,
                        random_number_generator_.getGaussian(0.2) - 1.57));
   }
 }
 
 RobotStateDistributionOpponentHalf::RobotStateDistributionOpponentHalf(
-    particle_filter::CRandomNumberGenerator& random_number_generator, std::pair<double, double> field_size)
+    particle_filter::CRandomNumberGenerator& random_number_generator, double field_size_x, double field_size_y)
     : random_number_generator_(random_number_generator) {
   // only own half
-  min_x_ = (field_size.first / 2.0) + 0.5;
-  min_y_ = (-field_size.second / 2.0) - 0.5;
+  min_x_ = (field_size_x / 2.0) + 0.5;
+  min_y_ = (-field_size_y / 2.0) - 0.5;
   max_x_ = 0 - 0.5;
-  max_y_ = (field_size.second / 2.0) + 0.5;
+  max_y_ = (field_size_y / 2.0) + 0.5;
 }
 
 const RobotState RobotStateDistributionOpponentHalf::draw() const {
@@ -58,13 +55,13 @@ 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, double field_size_x, double field_size_y)
     : random_number_generator_(random_number_generator) {
   // only own half
-  min_x_ = -field_size.first / 2.0;
-  min_y_ = -field_size.second / 2.0;
+  min_x_ = -field_size_x / 2.0;
+  min_y_ = -field_size_y / 2.0;
   max_x_ = 0;
-  max_y_ = field_size.second / 2.0;
+  max_y_ = field_size_y / 2.0;
 }
 
 const RobotState RobotStateDistributionOwnHalf::draw() const {

src/bitbots_navigation/bitbots_localization/src/localization.cpp

@@ -120,12 +120,12 @@ void Localization::updateParams(bool force_reload) {
                                                  config_.particle_filter.diffusion.starting_multiplier, drift_cov));
 
   // 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)));
-  robot_state_distribution_opponent_half.reset(new RobotStateDistributionOpponentHalf(
-      random_number_generator_, std::make_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)));
+  robot_state_distribution_own_sidelines.reset(
+      new RobotStateDistributionOwnSideline(random_number_generator_, field_dimensions_.x, field_dimensions_.y));
+  robot_state_distribution_opponent_half.reset(
+      new RobotStateDistributionOpponentHalf(random_number_generator_, field_dimensions_.x, field_dimensions_.y));
+  robot_state_distribution_own_half_.reset(
+      new RobotStateDistributionOwnHalf(random_number_generator_, 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;
+    const PolarCoordinates measurement_polar{measurement.first, measurement.second};
 
-    observationRelative = map->getObservationCoordinatesInMapFrame(measurement, best_estimate.getXPos(),
-                                                                   best_estimate.getYPos(), best_estimate.getTheta());
-    double occupancy = map->get_occupancy(observationRelative.first, observationRelative.second);
+    const CartesianCoordinates observation = map->getObservationCoordinatesInMapFrame(
+        measurement_polar, best_estimate.getXPos(), best_estimate.getYPos(), best_estimate.getTheta());
+    double occupancy = map->get_occupancy(observation.x, observation.y);
 
     geometry_msgs::msg::Point point;
-    point.x = observationRelative.first;
-    point.y = observationRelative.second;
+    point.x = observation.x;
+    point.y = observation.y;
 
     std_msgs::msg::ColorRGBA color;
     color.b = 1;

src/bitbots_navigation/bitbots_localization/src/map.cpp

@@ -53,32 +53,31 @@ 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;
+    const PolarCoordinates observation_polar{observation.first, observation.second};
 
     // get rating per line
-    lineRelative = getObservationCoordinatesInMapFrame(observation, state.getXPos(), state.getYPos(), state.getTheta());
-    double occupancy = get_occupancy(lineRelative.first, lineRelative.second);
+    const CartesianCoordinates line_relative =
+        getObservationCoordinatesInMapFrame(observation_polar, state.getXPos(), state.getYPos(), state.getTheta());
+    double occupancy = get_occupancy(line_relative.x, line_relative.y);
 
     rating.push_back(occupancy);
   }
   return rating;
 }
 
-std::pair<double, double> Map::getObservationCoordinatesInMapFrame(std::pair<double, double> observation, double stateX,
-                                                                   double stateY, double stateT) {
+CartesianCoordinates Map::getObservationCoordinatesInMapFrame(PolarCoordinates 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
   // 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);
+  const CartesianCoordinates observation_with_theta =
+      polarToCartesian({observation.angle + stateT, observation.radius});
 
   // add to particle
-  std::pair<double, double> observationRelative =
-      std::make_pair(stateX + observationWithTheta.first, stateY + observationWithTheta.second);
-
-  return observationRelative;  // in cartesian
+  return {stateX + observation_with_theta.x, stateY + observation_with_theta.y};
 }
 
 nav_msgs::msg::OccupancyGrid Map::get_map_msg(std::string frame_id, int threshold) {

src/bitbots_navigation/bitbots_localization/src/tools.cpp

@@ -2,21 +2,18 @@
 
 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);
+PolarCoordinates cartesianToPolar(CartesianCoordinates coordinates) {
+  return {atan2(coordinates.y, coordinates.x), hypot(coordinates.x, coordinates.y)};
 }
 
-std::pair<double, double> polarToCartesian(double t, double r) {
-  double x = r * std::cos(t);
-  double y = r * std::sin(t);
+PolarCoordinates cartesianToPolar(double x, double y) { return cartesianToPolar({x, y}); }
 
-  return std::make_pair(x, y);
+CartesianCoordinates polarToCartesian(PolarCoordinates coordinates) {
+  return {coordinates.radius * std::cos(coordinates.angle), coordinates.radius * std::sin(coordinates.angle)};
 }
 
+CartesianCoordinates polarToCartesian(double angle, double radius) { return polarToCartesian({angle, radius}); }
+
 double signedAngle(double angle) {
   if (angle > M_PI) {
     angle -= M_PI * 2;

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