セッションの切り替えが毎フレーム発生してしまうバグを修正

session/crane_planner_plugins/include/crane_planner_plugins/total_defense_planner.hpp

@@ -8,17 +8,14 @@
 #define CRANE_PLANNER_PLUGINS__TOTAL_DEFENSE_PLANNER_HPP_
 
 #include <crane_basics/boost_geometry.hpp>
-#include <crane_basics/position_assignments.hpp>
+#include <crane_basics/stream.hpp>
 #include <crane_msg_wrappers/robot_command_wrapper.hpp>
 #include <crane_msg_wrappers/world_model_wrapper.hpp>
-#include <crane_msgs/srv/robot_select.hpp>
 #include <crane_planner_base/planner_base.hpp>
 #include <crane_planner_plugins/defense_functions.hpp>
 #include <crane_robot_skills/goalie.hpp>
 #include <functional>
 #include <memory>
-#include <rclcpp/rclcpp.hpp>
-#include <tuple>
 #include <unordered_map>
 #include <utility>
 #include <vector>
@@ -62,7 +59,8 @@ class TotalDefensePlanner : public PlannerBase
     auto goalie_id = world_model->getOurGoalieId();
     if (ranges::count(selectable_robots, goalie_id) != 0) {
       selected.push_back(goalie_id);
-      ranges::remove(remaining_robots, goalie_id);
+      remaining_robots |=
+        ranges::actions::remove_if([goalie_id](auto elem) { return elem == goalie_id; });
       auto base = std::make_shared<RobotCommandWrapperBase>(
         "goalie", world_model->getOurGoalieId(), world_model);
       goalie = std::make_shared<skills::Goalie>(base);

session/crane_planner_plugins/src/total_defense_planner.cpp

@@ -4,6 +4,7 @@
 // license that can be found in the LICENSE file or at
 // https://opensource.org/licenses/MIT.
 
+#include <crane_basics/position_assignments.hpp>
 #include <crane_planner_plugins/total_defense_planner.hpp>
 
 namespace crane

session/crane_session_controller/src/crane_session_controller.cpp

@@ -8,6 +8,7 @@
 
 #include <ament_index_cpp/get_package_share_directory.hpp>
 #include <boost/stacktrace.hpp>
+#include <crane_basics/stream.hpp>
 #include <crane_basics/time.hpp>
 #include <crane_planner_plugins/planners.hpp>
 #include <filesystem>
@@ -167,10 +168,19 @@ SessionControllerComponent::SessionControllerComponent(const rclcpp::NodeOptions
       std::sort(observed_robot_ids.begin(), observed_robot_ids.end());
 
       if (assigned_robot_ids.size() != observed_robot_ids.size()) {
+        RCLCPP_INFO_STREAM(
+          get_logger(), "ロボットの数が変動しています｜割当数：" << assigned_robot_ids.size()
+                                                                 << ", 観測数："
+                                                                 << observed_robot_ids.size());
         return true;
       } else {
         for (size_t i = 0; i < assigned_robot_ids.size(); i++) {
           if (assigned_robot_ids[i] != observed_robot_ids[i]) {
+            std::stringstream what;
+            what << "ロボットの数は変わっていないですが、ラインナップが変動しています\n";
+            what << "\tbefore: " << assigned_robot_ids;
+            what << "\tafter : " << observed_robot_ids;
+            RCLCPP_INFO(get_logger(), what.str().c_str());
             return true;
           }
         }
@@ -179,7 +189,6 @@ SessionControllerComponent::SessionControllerComponent(const rclcpp::NodeOptions
     }();
 
     if (robot_changed) {
-      RCLCPP_INFO(get_logger(), "ロボットの数か変動していますので再割当を行います");
       assign(play_situation.getSituationCommandText());
     } else if (world_model->isOurBallOwnerChanged() or world_model->isBallOwnerTeamChanged()) {
       RCLCPP_INFO(get_logger(), "ボールオーナーが変更されたので再割当を行います");
@@ -233,9 +242,7 @@ void SessionControllerComponent::assign(const std::string & session_name)
   PlannerContext planner_context;
   if (session != event_map.end()) {
     RCLCPP_INFO(
-      get_logger(),
-      "初期イベント「%s」に対応するセッション「%"
-      "s」の設定に従ってロボットを割り当てます",
+      get_logger(), "イベント「%s」に対応するセッション「%s」の設定に従ってロボットを割り当てます",
       session->first.c_str(), session->second.c_str());
     try {
       request(session->second, world_model->ours.getAvailableRobotIds(), planner_context);
@@ -254,7 +261,7 @@ void SessionControllerComponent::assign(const std::string & session_name)
     }
   } else {
     RCLCPP_ERROR(
-      get_logger(), "初期イベント「%s」に対応するセッションの設定が見つかりませんでした",
+      get_logger(), "イベント「%s」に対応するセッションの設定が見つかりませんでした",
       session_name.c_str());
   }
 }
@@ -304,18 +311,17 @@ void SessionControllerComponent::request(
         available_planners.push_back(*matched_planner);
       } else {
         if (not selectable_robot_ids.empty()) {
-          std::stringstream id_list_string;
-          for (auto id : response.selected_robots) {
-            id_list_string << std::to_string(id) << " ";
-          }
-          std::stringstream ids_string;
-          for (auto id : selectable_robot_ids) {
-            ids_string << std::to_string(id) << " ";
+          RCLCPP_INFO_STREAM(get_logger(), "\t選択可能なロボットID :" << selectable_robot_ids);
+          if (response.selected_robots.empty()) {
+            RCLCPP_INFO(
+              get_logger(), "\tセッション「%s」はロボットを確保しませんでした。",
+              p.session_name.c_str());
+          } else {
+            RCLCPP_INFO_STREAM(
+              get_logger(), "\tセッション「" << p.session_name
+                                             << "」に以下のロボットを割り当てました :"
+                                             << response.selected_robots);
           }
-          RCLCPP_INFO(get_logger(), "\t選択可能なロボットID : %s", ids_string.str().c_str());
-          RCLCPP_INFO(
-            get_logger(), "\tセッション「%s」に以下のロボットを割り当てました : %s",
-            p.session_name.c_str(), id_list_string.str().c_str());
           available_planners.push_back(new_planner);
         }
       }

utility/crane_basics/include/crane_basics/stream.hpp

@@ -0,0 +1,36 @@
+// Copyright (c) 2024 ibis-ssl
+//
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file or at
+// https://opensource.org/licenses/MIT.
+
+#ifndef CRANE_BASICS__STREAM_HPP_
+#define CRANE_BASICS__STREAM_HPP_
+
+#include <cstdint>
+#include <ostream>
+#include <type_traits>
+#include <vector>
+
+template <typename T>
+std::ostream & operator<<(std::ostream & os, const std::vector<T> & vec)
+{
+  os << "[";
+  for (size_t i = 0; i < vec.size(); ++i) {
+    if constexpr (std::is_same_v<T, std::uint8_t>) {
+      // uint8_t の場合は int にキャストして数字表示
+      os << static_cast<int>(vec[i]);
+    } else {
+      // それ以外の場合はそのまま出力
+      os << vec[i];
+    }
+
+    if (i < vec.size() - 1) {
+      os << ",";
+    }
+  }
+  os << "]";
+  return os;
+}
+
+#endif  // CRANE_BASICS__STREAM_HPP_