From 277f4d095a01b3faf3001fe59d06235bdc169e08 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Wed, 21 Feb 2024 22:34:04 +0900 Subject: [PATCH] =?UTF-8?q?=E3=81=8A=E6=8E=83=E9=99=A4=EF=BC=88=E3=81=99?= =?UTF-8?q?=E3=81=A3=E3=81=8D=E3=82=8A=E3=80=9C=EF=BC=89?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_simple_ai/include/crane_commander.hpp | 1 - .../crane_planner_plugins/marker_planner.hpp | 13 ++++--------- .../crane_planner_plugins/our_kickoff_planner.hpp | 6 ------ .../include/crane_planner_plugins/skill_planner.hpp | 13 ++----------- 4 files changed, 6 insertions(+), 27 deletions(-) diff --git a/crane_simple_ai/include/crane_commander.hpp b/crane_simple_ai/include/crane_commander.hpp index 5b26ae474..59529597c 100644 --- a/crane_simple_ai/include/crane_commander.hpp +++ b/crane_simple_ai/include/crane_commander.hpp @@ -98,7 +98,6 @@ class ROSNode : public rclcpp::Node ROSNode() : Node("crane_commander") { world_model = std::make_shared(*this); - commander = std::make_shared(0, world_model); visualizer = std::make_shared(*this, "simple_ai"); publisher_robot_commands = create_publisher("/control_targets", 10); diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/marker_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/marker_planner.hpp index b32a31f4d..b55b47d6f 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/marker_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/marker_planner.hpp @@ -86,12 +86,9 @@ class MarkerPlanner : public PlannerBase selected_robots.push_back(selectable_robots[min_index]); skill_map.emplace( selectable_robots[min_index], - std::make_pair( - std::make_shared(selectable_robots[min_index], world_model), - std::make_shared(selectable_robots[min_index], world_model))); - skill_map[selectable_robots[min_index]].second->setParameter( - "marking_robot_id", enemy_robot->id); - skill_map[selectable_robots[min_index]].second->setParameter("mark_distance", 0.5); + std::make_shared(selectable_robots[min_index], world_model)); + skill_map[selectable_robots[min_index]]->setParameter("marking_robot_id", enemy_robot->id); + skill_map[selectable_robots[min_index]]->setParameter("mark_distance", 0.5); } return selected_robots; @@ -101,9 +98,7 @@ class MarkerPlanner : public PlannerBase // key: ID of our robot in charge, value: ID of the enemy marked robot std::unordered_map marking_target_map; - std::unordered_map< - uint8_t, std::pair, std::shared_ptr>> - skill_map; + std::unordered_map> skill_map; }; } // namespace crane diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/our_kickoff_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/our_kickoff_planner.hpp index e93cc4328..66234722c 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/our_kickoff_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/our_kickoff_planner.hpp @@ -28,12 +28,8 @@ class OurKickOffPlanner : public PlannerBase private: std::shared_ptr kickoff_attack; - std::shared_ptr attacker_command; - std::shared_ptr kickoff_support; - std::shared_ptr supporter_command; - public: COMPOSITION_PUBLIC explicit OurKickOffPlanner( WorldModelWrapper::SharedPtr & world_model, ConsaiVisualizerWrapper::SharedPtr visualizer) @@ -83,9 +79,7 @@ class OurKickOffPlanner : public PlannerBase }); kickoff_attack = std::make_shared(*best_attacker, world_model); - attacker_command = std::make_shared(*best_attacker, world_model); kickoff_support = std::make_shared(*best_supporter, world_model); - supporter_command = std::make_shared(*best_supporter, world_model); return {*best_attacker, *best_supporter}; } diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/skill_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/skill_planner.hpp index cbfa4b96c..58772e9d0 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/skill_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/skill_planner.hpp @@ -65,8 +65,6 @@ class GoalieSkillPlanner : public PlannerBase public: std ::shared_ptr skill = nullptr; - std ::shared_ptr robot_command_wrapper = nullptr; - COMPOSITION_PUBLIC explicit GoalieSkillPlanner( WorldModelWrapper ::SharedPtr & world_model, ConsaiVisualizerWrapper ::SharedPtr visualizer) : PlannerBase("Goalie", world_model, visualizer) @@ -76,7 +74,7 @@ class GoalieSkillPlanner : public PlannerBase std ::pair> calculateRobotCommand( const std ::vector & robots) override { - if (not skill or not robot_command_wrapper) { + if (not skill) { return {PlannerBase ::Status ::RUNNING, {}}; } else { std ::vector robot_commands; @@ -89,8 +87,6 @@ class GoalieSkillPlanner : public PlannerBase -> std ::vector override { skill = std ::make_shared(world_model->getOurGoalieId(), world_model); - robot_command_wrapper = - std ::make_shared(world_model->getOurGoalieId(), world_model); return {world_model->getOurGoalieId()}; } }; @@ -100,8 +96,6 @@ class BallPlacementSkillPlanner : public PlannerBase public: std ::shared_ptr skill = nullptr; - std ::shared_ptr robot_command_wrapper = nullptr; - COMPOSITION_PUBLIC explicit BallPlacementSkillPlanner( WorldModelWrapper ::SharedPtr & world_model, ConsaiVisualizerWrapper ::SharedPtr visualizer) : PlannerBase("BallPlacement", world_model, visualizer) @@ -111,7 +105,7 @@ class BallPlacementSkillPlanner : public PlannerBase std ::pair> calculateRobotCommand( const std ::vector & robots) override { - if (not skill or not robot_command_wrapper) { + if (not skill) { return {PlannerBase ::Status ::RUNNING, {}}; } else { if (auto target = world_model->getBallPlacementTarget(); target.has_value()) { @@ -140,9 +134,6 @@ class BallPlacementSkillPlanner : public PlannerBase skill->setParameter("placement_x", target->x()); skill->setParameter("placement_y", target->y()); } - - robot_command_wrapper = - std ::make_shared(selected_robots.front(), world_model); return {selected_robots.front()}; } };