Merge branch 'develop' into backend

.github/workflows/build_test.yaml

@@ -15,13 +15,13 @@ jobs:
           || fromJSON('[ "ubuntu-latest" ]')
       }}
     timeout-minutes: 30
-    container: ghcr.io/ibis-ssl/crane:base
+    container: ros:${{ matrix.rosdistro }}
     env:
       DEBIAN_FRONTEND: noninteractive
     strategy:
       fail-fast: false
       matrix:
-        rosdistro: [humble]
+        rosdistro: [jazzy]
     steps:
       - name: suppress warnings
         run: |
@@ -52,6 +52,11 @@ jobs:
         with:
           token: ${{ secrets.GITHUB_TOKEN }}
 
+      - name: Install wget
+        run: |
+          sudo apt-get update
+          sudo apt-get install -y wget libcpprest-dev
+
       - name: Clone dependency packages
         run: |
           mkdir -p dependency_ws
@@ -76,7 +81,6 @@ jobs:
         run: |
           . /opt/ros/${{ matrix.rosdistro }}/setup.sh
           colcon build --event-handlers console_cohesion+ \
-            --packages-above-and-dependencies ${{ steps.get-modified-packages.outputs.modified-packages }} \
             --cmake-args -DCMAKE_BUILD_TYPE=Release \
             --mixin coverage-gcc coverage-pytest compile-commands
         shell: bash

.github/workflows/custom_dict.json

@@ -6,7 +6,6 @@
     "robocupssl",
     "teleop",
     "inplay",
-    "liborocos",
     "matplotlibcpp",
     "DPPS",
     "consai",

.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:

.pre-commit-config.yaml

@@ -49,7 +49,7 @@ repos:
       - id: shellcheck
 
   - repo: https://github.com/scop/pre-commit-shfmt
-    rev: v3.10.0-1
+    rev: v3.10.0-2
     hooks:
       - id: shfmt
         args: [-w, -s, -i=4]

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

@@ -67,7 +67,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();
       });
     };
@@ -97,25 +97,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/CMakeLists.txt

@@ -18,7 +18,10 @@ find_package(rclcpp_components REQUIRED)
 find_package(backward_ros REQUIRED)
 ament_auto_find_build_dependencies()
 
-ament_auto_add_library(${PROJECT_NAME}_component SHARED DIRECTORY src)
+ament_auto_add_library(${PROJECT_NAME}_component SHARED
+  src/rvo2_planner.cpp
+  src/crane_local_planner.cpp
+)
 
 target_precompile_headers(${PROJECT_NAME}_component
   PUBLIC
@@ -37,7 +40,7 @@ ament_auto_add_executable(${PROJECT_NAME}_node
   src/${PROJECT_NAME}_node.cpp
 )
 
-include(/opt/ros/humble/share/backward_ros/cmake/BackwardConfig.cmake)
+include(/opt/ros/$ENV{ROS_DISTRO}/share/backward_ros/cmake/BackwardConfig.cmake)
 add_backward(${PROJECT_NAME}_node)
 
 if(BUILD_TESTING)

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/include/crane_local_planner/local_planner.hpp

@@ -11,12 +11,12 @@
 #include <crane_msgs/msg/robot_commands.hpp>
 #include <crane_msgs/msg/world_model.hpp>
 #include <functional>
-#include <grid_map_ros/grid_map_ros.hpp>
+// #include <grid_map_ros/grid_map_ros.hpp>
 #include <memory>
 #include <rclcpp/rclcpp.hpp>
 #include <std_msgs/msg/float32.hpp>
 
-#include "gridmap_planner.hpp"
+// #include "gridmap_planner.hpp"
 #include "rvo2_planner.hpp"
 #include "simple_planner.hpp"
 #include "visibility_control.h"
@@ -37,15 +37,16 @@ class LocalPlannerComponent : public rclcpp::Node
   explicit LocalPlannerComponent(const rclcpp::NodeOptions & options)
   : rclcpp::Node("local_planner", options)
   {
-    declare_parameter("planner", "gridmap");
+    declare_parameter("planner", "rvo2");
     auto planner_str = get_parameter("planner").as_string();
 
     crane::ConsaiVisualizerBuffer::activate(*this);
 
     process_time_pub = create_publisher<std_msgs::msg::Float32>("process_time", 10);
-    if (planner_str == "gridmap") {
-      planner = std::make_shared<GridMapPlanner>(*this);
-    } else if (planner_str == "simple") {
+    // if (planner_str == "gridmap") {
+    //   planner = std::make_shared<GridMapPlanner>(*this);
+    // }
+    if (planner_str == "simple") {
       planner = std::make_shared<SimplePlanner>(*this);
     } else if (planner_str == "rvo2") {
       planner = std::make_shared<RVO2Planner>(*this);

crane_local_planner/package.xml

@@ -14,12 +14,12 @@
   <depend>consai_visualizer_msgs</depend>
   <depend>crane_msg_wrappers</depend>
   <depend>crane_msgs</depend>
-  <depend>grid_map_core</depend>
-  <depend>grid_map_ros</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
   <depend>rvo2_vendor</depend>
   <depend>std_msgs</depend>
+  <!-- <depend>grid_map_core</depend> -->
+  <!-- <depend>grid_map_ros</depend> -->
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>crane_lint_common</test_depend>

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