Merge branch 'develop' into metis

.github/workflows/build_test.yaml

@@ -21,7 +21,7 @@ jobs:
     strategy:
       fail-fast: false
       matrix:
-        rosdistro: [humble, jazzy]
+        rosdistro: [jazzy]
     steps:
       - name: suppress warnings
         run: |

.github/workflows/full_build.yaml

@@ -17,7 +17,7 @@ jobs:
     strategy:
       fail-fast: false
       matrix:
-        rosdistro: [humble]
+        rosdistro: [jazzy]
     steps:
       - name: suppress warnings
         run: |

.github/workflows/scenario_test.yaml

@@ -17,7 +17,7 @@ jobs:
     strategy:
       fail-fast: false
       matrix:
-        rosdistro: [humble]
+        rosdistro: [jazzy]
     steps:
       - uses: actions/checkout@v4
         with:

consai_ros2/robocup_ssl_comm/include/robocup_ssl_comm/multicast.hpp

@@ -88,7 +88,7 @@ class MulticastReceiver
     return received;
   }
 
-  size_t available() { return socket.available(); }
+  size_t available() const { return socket.available(); }
 
 private:
   asio::io_context io_context;

crane_game_analyzer/include/crane_game_analyzer/game_analyzer.hpp

@@ -69,7 +69,7 @@ class GameAnalyzerComponent : public rclcpp::Node
     auto & theirs = world_model->theirs.robots;
     Point & ball_pos = world_model->ball.pos;
     auto get_nearest_ball_robot = [&](std::vector<RobotInfo::SharedPtr> & robots) {
-      return *std::min_element(robots.begin(), robots.end(), [ball_pos](auto & a, auto & b) {
+      return *std::ranges::min_element(robots, [ball_pos](auto & a, auto & b) {
         return (a->pose.pos - ball_pos).squaredNorm() < (b->pose.pos - ball_pos).squaredNorm();
       });
     };
@@ -99,25 +99,17 @@ class GameAnalyzerComponent : public rclcpp::Node
     auto latest_time = ball_records.front().stamp;
     auto latest_position = ball_records.front().position;
     // 一定時間以上前の履歴を削除
-    ball_records.erase(
-      std::remove_if(
-        ball_records.begin(), ball_records.end(),
-        [&](auto & record) {
-          return (latest_time - record.stamp) > config.ball_idle.threshold_duration * 2;
-        }),
-      ball_records.end());
+    std::erase_if(ball_records, [&](auto & ball_record) {
+      return (latest_time - ball_record.stamp) > config.ball_idle.threshold_duration * 2;
+    });
 
     // ボール履歴（新しいほど，indexが若い）のチェックして，ボールがストップしているかを確認
-    for (auto record : ball_records) {
-      if (
-        (latest_position - record.position).norm() <
-        config.ball_idle.move_distance_threshold_meter) {
-        if ((latest_time - record.stamp) < config.ball_idle.threshold_duration) {
-          return false;
-        }
-      }
-    }
-    return true;
+    return not std::ranges::any_of(ball_records, [&](const auto & ball_record) {
+      bool distance_cond = (latest_position - ball_record.position).norm() <
+                           config.ball_idle.move_distance_threshold_meter;
+      bool time_cond = (latest_time - ball_record.stamp) < config.ball_idle.threshold_duration;
+      return distance_cond && time_cond;
+    });
   }
 
   std::optional<RobotCollisionInfo> getRobotCollisionInfo()

crane_local_planner/include/crane_local_planner/gridmap_planner.hpp

@@ -52,7 +52,11 @@ struct EigenArrayHash
 struct AStarNode
 {
   grid_map::Index index;
-  double g, h;
+
+  double g;
+
+  double h;
+
   std::optional<grid_map::Index> parent_index = std::nullopt;
 
   [[nodiscard]] double calcHeuristic(const grid_map::Index & goal_index) const

crane_local_planner/src/gridmap_planner.cpp

@@ -120,7 +120,7 @@ std::vector<grid_map::Index> GridMapPlanner::findPathAStar(
         path.push_back(current.parent_index.value());
         current = closedSet[current.parent_index.value()];
       }
-      std::reverse(path.begin(), path.end());
+      std::ranges::reverse(path);
       return path;
     }
 
@@ -148,7 +148,7 @@ std::vector<grid_map::Index> GridMapPlanner::findPathAStar(
         // closedSetとopenSetに含まれていない場合のみ追加
         if (
           closedSet.count(next.index) == 0 &&
-          std::find_if(openSet.begin(), openSet.end(), [index = next.index](const auto & elem) {
+          std::ranges::find_if(openSet, [index = next.index](const auto & elem) {
             return elem.second.x() == index.x() && elem.second.y() == index.y();
           }) == openSet.end()) {
           openSet.emplace(next, next.index);

crane_robot_receiver/src/robot_receiver_node.cpp

@@ -9,6 +9,7 @@
 #include <crane_msg_wrappers/consai_visualizer_wrapper.hpp>
 #include <crane_msgs/msg/robot_feedback.hpp>
 #include <crane_msgs/msg/robot_feedback_array.hpp>
+#include <format>
 #include <iostream>
 #include <rclcpp/rclcpp.hpp>
 
@@ -17,13 +18,14 @@ using boost::asio::ip::udp;
 struct RobotInterfaceConfig
 {
   std::string ip;
+
   int port;
 };
 
 auto makeConfig(uint8_t id) -> RobotInterfaceConfig
 {
   RobotInterfaceConfig config;
-  config.ip = "224.5.20." + std::to_string(id + 100);
+  config.ip = std::format("224.5.20.{}", id + 100);
   config.port = 50100 + id;
   return config;
 }
@@ -35,16 +37,32 @@ struct RobotFeedback
   uint8_t kick_state;
 
   uint8_t temperature[7];
+
   uint16_t error_id;
+
   uint16_t error_info;
+
   float error_value;
+
   float motor_current[4];
+
   uint8_t ball_detection[4];
 
   bool ball_sensor;
 
-  float_t yaw_angle, diff_angle;
-  float_t odom[2], odom_speed[2], mouse_odom[2], mouse_vel[2], voltage[2];
+  float_t yaw_angle;
+
+  float_t diff_angle;
+
+  std::array<float_t, 2> odom;
+
+  std::array<float_t, 2> odom_speed;
+
+  std::array<float_t, 2> mouse_odom;
+
+  std::array<float_t, 2> mouse_vel;
+
+  std::array<float_t, 2> voltage;
 
   uint8_t check_ver;
 
@@ -53,12 +71,14 @@ struct RobotFeedback
 
 union FloatUnion {
   float f;
-  char b[4];
+
+  std::array<char, 4> b;
 };
 
 union Uint16Union {
   uint16_t u16;
-  char b[2];
+
+  std::array<char, 2> b;
 };
 
 class MulticastReceiver
@@ -244,7 +264,7 @@ class MulticastReceiver
     robot_feedback = feedback;
   }
 
-  RobotFeedback getFeedback() { return robot_feedback; }
+  RobotFeedback getFeedback() const { return robot_feedback; }
 
   const int robot_id;
 
@@ -263,9 +283,7 @@ class MulticastReceiver
 class RobotReceiverNode : public rclcpp::Node
 {
 public:
-  explicit RobotReceiverNode(uint8_t robot_num = 10)
-  : rclcpp::Node("robot_receiver_node"),
-    visualizer(std::make_unique<crane::ConsaiVisualizerBuffer::MessageBuilder>("robot_receiver"))
+  explicit RobotReceiverNode(uint8_t robot_num = 10) : rclcpp::Node("robot_receiver_node")
   {
     crane::ConsaiVisualizerBuffer::activate(*this);
     publisher = create_publisher<crane_msgs::msg::RobotFeedbackArray>("/robot_feedback", 10);
@@ -344,7 +362,8 @@ class RobotReceiverNode : public rclcpp::Node
 
   rclcpp::Publisher<crane_msgs::msg::RobotFeedbackArray>::SharedPtr publisher;
 
-  crane::ConsaiVisualizerBuffer::MessageBuilder::UniquePtr visualizer;
+  crane::ConsaiVisualizerBuffer::MessageBuilder::UniquePtr visualizer =
+    std::make_unique<crane::ConsaiVisualizerBuffer::MessageBuilder>("robot_receiver");
 };
 
 int main(int argc, char * argv[])

crane_robot_skills/include/crane_robot_skills/skill_base.hpp

@@ -11,6 +11,7 @@
 #include <crane_msg_wrappers/consai_visualizer_wrapper.hpp>
 #include <crane_msg_wrappers/robot_command_wrapper.hpp>
 #include <crane_msg_wrappers/world_model_wrapper.hpp>
+#include <format>
 #include <functional>
 #include <memory>
 #include <string>
@@ -53,8 +54,8 @@ class StateMachine
 
   void update()
   {
-    if (auto it = std::find_if(
-          transitions.begin(), transitions.end(),
+    if (auto it = std::ranges::find_if(
+          transitions,
           [this](const Transition & transition) {
             return transition.from == current_state && transition.condition();
           });
@@ -106,11 +107,11 @@ inline std::string getValueString(const ContextType & type)
       [&value_string](const int e) { value_string = std::to_string(e); },
       [&value_string](const std::string & e) { value_string = e; },
       [&value_string](const Point & e) {
-        value_string = "(" + std::to_string(e.x()) + ", " + std::to_string(e.y()) + ")";
+        value_string = std::format("({}, {})", std::to_string(e.x()), std::to_string(e.y()));
       },
       [&value_string](const std::optional<Point> & e) {
         if (e) {
-          value_string = "(" + std::to_string(e->x()) + ", " + std::to_string(e->y()) + ")";
+          value_string = std::format("({}, {})", std::to_string(e->x()), std::to_string(e->y()));
         } else {
           value_string = "nullopt";
         }
@@ -203,16 +204,16 @@ class SkillInterface
 
   void getParameterSchemaString(std::ostream & os) const
   {
-    for (const auto & element : parameters) {
-      os << element.first << ": ";
+    for (const auto & [name, parameter] : parameters) {
+      os << name << ": ";
       std::visit(
         overloaded{
           [&os](double e) { os << "double, " << e << std::endl; },
           [&os](int e) { os << "int, " << e << std::endl; },
           [&os](const std::string & e) { os << "string, " << e << std::endl; },
           [&os](bool e) { os << "bool, " << e << std::endl; },
           [&os](Point e) { os << "Point, " << e.x() << ", " << e.y() << std::endl; }},
-        element.second);
+        parameter);
     }
   }
 

crane_robot_skills/include/crane_robot_skills/sub_attacker.hpp

@@ -27,18 +27,19 @@ class SubAttacker : public SkillBase<RobotCommandWrapperPosition>
   void print(std::ostream & os) const override { os << "[SubAttacker]"; }
 
   static std::vector<std::pair<double, Point>> getPositionsWithScore(
-    Segment ball_line, Point next_target, const WorldModelWrapper::SharedPtr & world_model);
+    const Segment & ball_line, const Point & next_target,
+    const WorldModelWrapper::SharedPtr & world_model);
 
-  static std::vector<Point> getPoints(Segment ball_line, double interval);
+  static std::vector<Point> getPoints(const Segment & ball_line, double interval);
 
-  static std::vector<Point> getPoints(Point center, float unit, int unit_num);
+  static std::vector<Point> getPoints(const Point & center, float unit, int unit_num);
 
   static std::vector<Point> getDPPSPoints(
-    Point center, double r_resolution, int theta_div_num,
+    const Point & center, double r_resolution, int theta_div_num,
     const WorldModelWrapper::SharedPtr & world_model);
 
   static double getPointScore(
-    Point p, Point next_target, const WorldModelWrapper::SharedPtr & world_model);
+    const Point & p, const Point & next_target, const WorldModelWrapper::SharedPtr & world_model);
 };
 }  // namespace crane::skills
 #endif  // CRANE_ROBOT_SKILLS__SUB_ATTACKER_HPP_

crane_robot_skills/src/ball_nearby_positioner.cpp

@@ -56,11 +56,7 @@ Status BallNearByPositioner::update()
         if (theirs.size() > 2) {
           auto nearest_robot =
             world_model()->getNearestRobotWithDistanceFromPoint(world_model()->ball.pos, theirs);
-          theirs.erase(
-            std::remove_if(
-              theirs.begin(), theirs.end(),
-              [&](const auto & r) { return r->id == nearest_robot.first->id; }),
-            theirs.end());
+          std::erase_if(theirs, [&](const auto & r) { return r->id == nearest_robot.first->id; });
           auto second_nearest_robot =
             world_model()->getNearestRobotWithDistanceFromPoint(world_model()->ball.pos, theirs);
           return (second_nearest_robot.first->pose.pos - world_model()->ball.pos).normalized();

crane_robot_skills/src/goal_kick.cpp

@@ -46,9 +46,8 @@ double GoalKick::getBestAngleToShootFromPoint(
   // 隙間のなかで更に良い角度を計算する。
   // キック角度の最低要求精度をオフセットとしてできるだけ端っこを狙う
   if (goal_angle_width < minimum_angle_accuracy * 2.0) {
-    double best_angle1, best_angle2;
-    best_angle1 = best_angle - goal_angle_width / 2.0 + minimum_angle_accuracy;
-    best_angle2 = best_angle + goal_angle_width / 2.0 - minimum_angle_accuracy;
+    double best_angle1 = best_angle - goal_angle_width / 2.0 + minimum_angle_accuracy;
+    double best_angle2 = best_angle + goal_angle_width / 2.0 - minimum_angle_accuracy;
     Point their_goalie_pos = [&]() -> Point {
       const auto & enemy_robots = world_model->theirs.getAvailableRobots();
       if (not enemy_robots.empty()) {

crane_robot_skills/src/goalie.cpp

@@ -49,23 +49,19 @@ void Goalie::emitBallFromPenaltyArea()
   Point ball = world_model()->ball.pos;
   // パスできるロボットのリストアップ
   auto passable_robot_list = world_model()->ours.getAvailableRobots(command.getMsg().robot_id);
-  passable_robot_list.erase(
-    std::remove_if(
-      passable_robot_list.begin(), passable_robot_list.end(),
-      [&](const RobotInfo::SharedPtr & r) {
-        if (
-          std::abs(r->pose.pos.x() - world_model()->getOurGoalCenter().x()) <
-          world_model()->getDefenseHeight()) {
-          // ゴールラインに近いロボットは除外
-          return true;
-        } else if (world_model()->getDistanceFromRobotToBall(r->getID()) < 0.5) {
-          // ボールに近いロボットは除外
-          return true;
-        } else {
-          return false;
-        }
-      }),
-    passable_robot_list.end());
+  std::erase_if(passable_robot_list, [&](const RobotInfo::SharedPtr & r) {
+    if (
+      std::abs(r->pose.pos.x() - world_model()->getOurGoalCenter().x()) <
+      world_model()->getDefenseHeight()) {
+      // ゴールラインに近いロボットは除外
+      return true;
+    } else if (world_model()->getDistanceFromRobotToBall(r->getID()) < 0.5) {
+      // ボールに近いロボットは除外
+      return true;
+    } else {
+      return false;
+    }
+  });
 
   Point pass_target = [&]() {
     if (not passable_robot_list.empty()) {
@@ -162,8 +158,8 @@ void Goalie::inplay(bool enable_emit)
         } else {
           Point threat_point = world_model()->ball.pos;
           bool penalty_area_pass_to_side = [&]() {
-            Point penalty_base_1, penalty_base_2;
-            penalty_base_1 = penalty_base_2 = world_model()->getOurGoalCenter();
+            Point penalty_base_1 = world_model()->getOurGoalCenter();
+            Point penalty_base_2 = world_model()->getOurGoalCenter();
             penalty_base_1.y() = world_model()->penalty_area_size.y() * 0.5;
             penalty_base_2.y() = -world_model()->penalty_area_size.y() * 0.5;
             auto offset =
@@ -196,7 +192,8 @@ void Goalie::inplay(bool enable_emit)
           }();
 
           bool penalty_area_pass_to_front = [&]() {
-            Point penalty_front_1, penalty_front_2;
+            Point penalty_front_1;
+            Point penalty_front_2;
             penalty_front_1.x() = penalty_front_2.x() =
               world_model()->getOurGoalCenter().x() - world_model()->penalty_area_size.x();
             penalty_front_1.y() = world_model()->penalty_area_size.y() * 0.5;