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 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..670855e63 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; @@ -31,32 +32,34 @@ class RobotStateDistribution : public particle_filter::StateDistribution { public: - RobotStateDistributionStartLeft(particle_filter::CRandomNumberGenerator& random_number_generator, - std::pair 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 field_size; + double field_size_x_; + double field_size_y_; }; class RobotStateDistributionOwnSideline : public particle_filter::StateDistribution { public: RobotStateDistributionOwnSideline(particle_filter::CRandomNumberGenerator& random_number_generator, - std::pair 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 { public: RobotStateDistributionOpponentHalf(particle_filter::CRandomNumberGenerator& random_number_generator, - std::pair 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 { public: - RobotStateDistributionOwnHalf(particle_filter::CRandomNumberGenerator& random_number_generator, - std::pair field_size); + RobotStateDistributionOwnHalf(particle_filter::CRandomNumberGenerator& random_number_generator, double field_size_x, + double field_size_y); 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..c58d290b9 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); + CartesianCoordinates getObservationCoordinatesInMapFrame(PolarCoordinates 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/include/bitbots_localization/tools.hpp b/src/bitbots_navigation/bitbots_localization/include/bitbots_localization/tools.hpp index 3c55b1d56..2406d4173 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,20 @@ #include namespace bitbots_localization { -std::pair cartesianToPolar(double x, double y); -std::pair 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 diff --git a/src/bitbots_navigation/bitbots_localization/src/MotionModel.cpp b/src/bitbots_navigation/bitbots_localization/src/MotionModel.cpp index 85b385edb..8700a2733 100644 --- a/src/bitbots_navigation/bitbots_localization/src/MotionModel.cpp +++ b/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); diff --git a/src/bitbots_navigation/bitbots_localization/src/ObservationModel.cpp b/src/bitbots_navigation/bitbots_localization/src/ObservationModel.cpp index 3a737f0f4..e9fbda901 100644 --- a/src/bitbots_navigation/bitbots_localization/src/ObservationModel.cpp +++ b/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 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); + 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 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); } } diff --git a/src/bitbots_navigation/bitbots_localization/src/StateDistribution.cpp b/src/bitbots_navigation/bitbots_localization/src/StateDistribution.cpp index 0e8ba1d49..909b5a217 100644 --- a/src/bitbots_navigation/bitbots_localization/src/StateDistribution.cpp +++ b/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 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 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 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 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 { diff --git a/src/bitbots_navigation/bitbots_localization/src/localization.cpp b/src/bitbots_navigation/bitbots_localization/src/localization.cpp index 212dcb83c..b173f7a66 100644 --- a/src/bitbots_navigation/bitbots_localization/src/localization.cpp +++ b/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& measurement : measurements) { // lines are in polar form! - std::pair 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; diff --git a/src/bitbots_navigation/bitbots_localization/src/map.cpp b/src/bitbots_navigation/bitbots_localization/src/map.cpp index b990313a2..15c301eaf 100644 --- a/src/bitbots_navigation/bitbots_localization/src/map.cpp +++ b/src/bitbots_navigation/bitbots_localization/src/map.cpp @@ -53,32 +53,31 @@ std::vector Map::provideRating(const RobotState& state, std::vector rating; for (const std::pair& observation : observations) { // lines are in polar form! - std::pair 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 Map::getObservationCoordinatesInMapFrame(std::pair 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 observationWithTheta = polarToCartesian(observation.first + stateT, observation.second); + const CartesianCoordinates observation_with_theta = + polarToCartesian({observation.angle + stateT, observation.radius}); // add to particle - std::pair 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) { diff --git a/src/bitbots_navigation/bitbots_localization/src/tools.cpp b/src/bitbots_navigation/bitbots_localization/src/tools.cpp index abcc12116..52391ec3d 100644 --- a/src/bitbots_navigation/bitbots_localization/src/tools.cpp +++ b/src/bitbots_navigation/bitbots_localization/src/tools.cpp @@ -2,21 +2,18 @@ namespace bitbots_localization { -std::pair 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 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; 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_) {