Marking coordinator

install/setup.bash

@@ -28,4 +28,4 @@ COLCON_CURRENT_PREFIX="$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null
 _colcon_prefix_chain_bash_source_script "$COLCON_CURRENT_PREFIX/local_setup.bash"
 
 unset COLCON_CURRENT_PREFIX
-unset _colcon_prefix_chain_bash_source_script
+unset _colcon_prefix_chain_bash_source_script

install/setup.zsh

@@ -28,4 +28,4 @@ COLCON_CURRENT_PREFIX="$(builtin cd -q "`dirname "${(%):-%N}"`" > /dev/null && p
 _colcon_prefix_chain_zsh_source_script "$COLCON_CURRENT_PREFIX/local_setup.zsh"
 
 unset COLCON_CURRENT_PREFIX
-unset _colcon_prefix_chain_zsh_source_script
+unset _colcon_prefix_chain_zsh_source_script

launch/soccer.launch.py

@@ -237,6 +237,13 @@ def generate_launch_description():
                 output="screen",
                 parameters=[param_config_filepath],
                 on_exit=Shutdown(),
+            ),
+            Node(
+                package="rj_strategy",
+                executable="marking_node",
+                output="screen",
+                parameters=[param_config_filepath],
+                on_exit=Shutdown(),
             )
         ]
     )

rj_gameplay/rj_gameplay.egg-info/PKG-INFO

@@ -0,0 +1,12 @@
+Metadata-Version: 2.1
+Name: rj-gameplay
+Version: 0.0.0
+Summary: Rewrite of the gameplay library.
+Home-page: UNKNOWN
+Maintainer: oswinso
+Maintainer-email: [email protected]
+License: UNKNOWN
+Platform: UNKNOWN
+
+UNKNOWN
+

rj_gameplay/rj_gameplay.egg-info/SOURCES.txt

@@ -0,0 +1,90 @@
+README.md
+setup.cfg
+setup.py
+rj_gameplay/__init__.py
+rj_gameplay/gameplay_node.py
+rj_gameplay/play_selector.py
+rj_gameplay.egg-info/PKG-INFO
+rj_gameplay.egg-info/SOURCES.txt
+rj_gameplay.egg-info/dependency_links.txt
+rj_gameplay.egg-info/entry_points.txt
+rj_gameplay.egg-info/requires.txt
+rj_gameplay.egg-info/top_level.txt
+rj_gameplay.egg-info/zip-safe
+rj_gameplay/eval/__init__.py
+rj_gameplay/play/__init__.py
+rj_gameplay/play/defend_restart.py
+rj_gameplay/play/defense.py
+rj_gameplay/play/defensive_clear.py
+rj_gameplay/play/keepaway.py
+rj_gameplay/play/kickoff_play.py
+rj_gameplay/play/offense.py
+rj_gameplay/play/penalty_defense.py
+rj_gameplay/play/penalty_offense.py
+rj_gameplay/play/prep_penalty_offense.py
+rj_gameplay/play/restart.py
+rj_gameplay/play/test_motion_planning.py
+rj_gameplay/role/__init__.py
+rj_gameplay/role/capture_role.py
+rj_gameplay/role/dumb_move.py
+rj_gameplay/role/goalie_role.py
+rj_gameplay/role/marker.py
+rj_gameplay/role/passer.py
+rj_gameplay/role/receiver.py
+rj_gameplay/role/seeker.py
+rj_gameplay/role/striker.py
+rj_gameplay/situation/__init__.py
+rj_gameplay/situation/decision_tree/__init__.py
+rj_gameplay/situation/decision_tree/plays.py
+rj_gameplay/skill/__init__.py
+rj_gameplay/skill/capture.py
+rj_gameplay/skill/dribble.py
+rj_gameplay/skill/intercept.py
+rj_gameplay/skill/kick.py
+rj_gameplay/skill/line_kick.py
+rj_gameplay/skill/mark.py
+rj_gameplay/skill/move.py
+rj_gameplay/skill/pivot.py
+rj_gameplay/skill/pivot_kick.py
+rj_gameplay/skill/receive.py
+rj_gameplay/skill/settle.py
+rj_gameplay/tactic/__init__.py
+rj_gameplay/tactic/clear_tactic.py
+rj_gameplay/tactic/dumb_tactic.py
+rj_gameplay/tactic/goalie_tactic.py
+rj_gameplay/tactic/line_tactic.py
+rj_gameplay/tactic/move_tactic.py
+rj_gameplay/tactic/nmark_tactic.py
+rj_gameplay/tactic/pass_tactic.py
+rj_gameplay/tactic/prep_move.py
+rj_gameplay/tactic/seek.py
+rj_gameplay/tactic/striker_tactic.py
+rj_gameplay/tactic/wall_tactic.py
+stp/__init__.py
+stp/global_parameters.py
+stp/local_parameters.py
+stp/pylint_stp.py
+stp/rc.py
+stp/testing.py
+stp/action/__init__.py
+stp/formations/__init__.py
+stp/formations/diamond_formation.py
+stp/formations/x_formation.py
+stp/play/__init__.py
+stp/play/pure_play.py
+stp/role/__init__.py
+stp/role/constraint.py
+stp/role/cost.py
+stp/role/assignment/__init__.py
+stp/role/assignment/naive.py
+stp/situation/__init__.py
+stp/skill/__init__.py
+stp/skill/action_behavior.py
+stp/tactic/__init__.py
+stp/utils/__init__.py
+stp/utils/constants.py
+stp/utils/enum.py
+stp/utils/fsm.py
+stp/utils/pass_seeker_optimizer.py
+stp/utils/typed_key_dict.py
+stp/utils/world_state_converter.py

rj_gameplay/rj_gameplay.egg-info/dependency_links.txt

@@ -0,0 +1 @@
+

rj_gameplay/rj_gameplay.egg-info/entry_points.txt

@@ -0,0 +1,3 @@
+[console_scripts]
+gameplay_node = rj_gameplay.gameplay_node:main
+

rj_gameplay/rj_gameplay.egg-info/requires.txt

@@ -0,0 +1 @@
+setuptools

rj_gameplay/rj_gameplay.egg-info/top_level.txt

@@ -0,0 +1,2 @@
+rj_gameplay
+stp

rj_gameplay/rj_gameplay.egg-info/zip-safe

@@ -0,0 +1 @@
+

src/rj_constants/include/rj_constants/constants.hpp

@@ -7,6 +7,7 @@
 /// all weights in kilograms
 
 // Number of identifiable robots on one team
+// IF U CHANGE THIS NUMBER CHECK Marking.msg
 inline constexpr size_t kNumShells = 16;
 
 // Number of playing robots on one team

src/rj_msgs/CMakeLists.txt

@@ -59,6 +59,7 @@ rosidl_generate_interfaces(
 
   # Coordinators
   msg/KickerPicker.msg
+  msg/Marking.msg
 
   # Agent Request Messages
   request/JoinWallRequest.msg
@@ -84,6 +85,7 @@ rosidl_generate_interfaces(
   # Services
   srv/AgentCommunication.srv
   srv/KickerPicker.srv
+  srv/Marking.srv
   srv/ListJoysticks.srv
   srv/PlanHypotheticalPath.srv
   srv/QuickCommands.srv

src/rj_msgs/msg/Marking.msg

@@ -0,0 +1,2 @@
+# This is a message to pick a robot to mark
+uint8[16] mark_robot_ids  # ID of the robot to mark on the opposing team

src/rj_msgs/srv/Marking.srv

@@ -0,0 +1,5 @@
+# Request a robot to be added to or removed from the marking group
+uint8 robot_id     # ID of the robot (0 to kNumShells-1)
+bool join
+---
+bool success      # Always true

src/rj_strategy/CMakeLists.txt

@@ -47,7 +47,6 @@ set(RJ_STRATEGY_SRC
   src/agent/position/goalie.cpp
   src/agent/position/idle.cpp
   src/agent/position/line.cpp
-  src/agent/position/marker.cpp
   src/agent/position/offense.cpp
   src/agent/position/penalty_non_kicker.cpp
   src/agent/position/penalty_player.cpp
@@ -64,6 +63,7 @@ set(RJ_STRATEGY_SRC
 
   # Coordinators
   src/coordinator/kicker_picker_client.cpp
+  src/coordinator/marking_client.cpp
 )
 
 # rj_strategy library
@@ -115,6 +115,23 @@ ament_target_dependencies(kicker_picker_node
   ${RJ_STRATEGY_DEPS}
 )
 
+# Marking Node
+add_executable(marking_node
+  ${RJ_STRATEGY_SRC}
+  src/coordinator/marking_client.cpp
+  src/coordinator/marking.cpp
+)
+
+target_include_directories(marking_node
+  PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)
+
+ament_target_dependencies(marking_node
+  ${RJ_STRATEGY_DEPS}
+)
+
 # Agent Action Clients
 add_executable(agent_action_clients
   ${RJ_STRATEGY_SRC}
@@ -146,7 +163,7 @@ install(
 )
 
 install(
-  TARGETS straight_line_test_node kicker_picker_node agent_action_clients
+  TARGETS straight_line_test_node kicker_picker_node marking_node agent_action_clients
   DESTINATION lib/${PROJECT_NAME}
 )
 

src/rj_strategy/include/rj_strategy/agent/position.hpp

@@ -40,11 +40,20 @@
 #include <rj_msgs/msg/position_request.hpp>
 #include <rj_msgs/msg/test_response.hpp>
 
+// Coordinators
+#include "rj_strategy/coordinator/kicker_picker_client.hpp"
+#include "rj_strategy/coordinator/marking_client.hpp"
+
 // tell compiler this class exists, but no need to import the whole header
 class AgentActionClient;
 
 namespace strategy {
 
+struct ClientHandles {
+    std::unique_ptr<KickerPickerClient> kickerPickerClient;
+    std::unique_ptr<MarkingClient> markingClient;
+};
+
 /*
  * Position is an abstract superclass. Its subclasses handle strategy logic.
  * The goal is to isolate the strategy logic from the ROS interfacing. This

src/rj_strategy/include/rj_strategy/agent/position/defense.hpp

@@ -16,9 +16,7 @@
 #include <rj_msgs/action/robot_move.hpp>
 
 #include "rj_strategy/agent/position.hpp"
-#include "rj_strategy/agent/position/marker.hpp"
 #include "rj_strategy/agent/position/waller.hpp"
-
 namespace strategy {
 
 /*
@@ -28,6 +26,8 @@ namespace strategy {
 class Defense : public Position {
 public:
     Defense(int r_id);
+    Defense(int r_id, std::shared_ptr<ClientHandles> clientHandles);
+    Defense(const Position& other, std::shared_ptr<ClientHandles> clientHandles);
     ~Defense() override = default;
     Defense(const Position& other);
 
@@ -45,8 +45,8 @@ class Defense : public Position {
 
 private:
     // static constexpr int kMaxWallers{6};
-    static constexpr int kMaxWallers{
-        static_cast<int>(kNumShells)};  // This effectively turns off marking
+    static constexpr int kMaxWallers{2};
+    // static_cast<int>(kNumShells)};  // This effectively turns off marking
 
     /**
      * @brief The derived_get_task method returns the task for the defensive robot
@@ -120,8 +120,13 @@ class Defense : public Position {
     int get_waller_id();
     State current_state_ = JOINING_WALL;
 
-    int get_marker_target_id();
-    Marker marker_;
+    bool sent_join_marking_group_request_ = false;
+    RJ::Time request_time_;
+    RJ::Seconds kMarkingGroupJoinTimeout{2.0};  // 2 seconds
+
+    bool pending_marking_state_ = false;
+
+    std::shared_ptr<ClientHandles> clientHandles_;
 };
 
 }  // namespace strategy

src/rj_strategy/include/rj_strategy/agent/position/free_kicker.hpp

@@ -18,6 +18,8 @@ namespace strategy {
 class FreeKicker : public Position {
 public:
     FreeKicker(int r_id);
+    FreeKicker(int r_id, std::shared_ptr<ClientHandles> clientHandles);
+    FreeKicker(const Position& other, std::shared_ptr<ClientHandles> clientHandles);
     FreeKicker(const Position&);
     ~FreeKicker() = default;
 
@@ -38,6 +40,7 @@ class FreeKicker : public Position {
 
 private:
     std::optional<RobotIntent> derived_get_task(RobotIntent intent) override;
+    std::shared_ptr<ClientHandles> clientHandles_;
 };
 
 }  // namespace strategy

src/rj_strategy/include/rj_strategy/agent/position/goal_kicker.hpp

@@ -21,6 +21,7 @@ class GoalKicker : public Position {
 public:
     GoalKicker(int r_id);
     ~GoalKicker() = default;
+    GoalKicker(int r_id, std::shared_ptr<ClientHandles> clientHandles);
 
     /**
      * @brief Does nothing; this position is a special case
@@ -39,6 +40,8 @@ class GoalKicker : public Position {
 
 private:
     std::optional<RobotIntent> derived_get_task(RobotIntent intent) override;
+
+    std::shared_ptr<ClientHandles> clientHandles_;
 };
 
-}  // namespace strategy
+}  // namespace strategy

src/rj_strategy/include/rj_strategy/agent/position/goalie.hpp

@@ -23,6 +23,8 @@ namespace strategy {
 class Goalie : public Position {
 public:
     Goalie(int r_id);
+    Goalie(int r_id, std::shared_ptr<ClientHandles> clientHandles);
+    Goalie(const Position& other, std::shared_ptr<ClientHandles> clientHandles);
     ~Goalie() override = default;
     Goalie(const Position& other);
 
@@ -87,6 +89,8 @@ class Goalie : public Position {
     State latest_state_ = IDLING;
 
     rj_geometry::Point penalty_location();
+
+    std::shared_ptr<ClientHandles> clientHandles_;
 };
 
 }  // namespace strategy

src/rj_strategy/include/rj_strategy/agent/position/idle.hpp

@@ -10,6 +10,8 @@ class Idle : public Position {
     Idle(int r_id);
     ~Idle() = default;
     Idle(const Position& other);
+    Idle(int r_id, std::shared_ptr<ClientHandles> clientHandles);
+    Idle(const Position& other, std::shared_ptr<ClientHandles> clientHandles);
 
     /**
      * @brief Does nothing; this position is a special case
@@ -28,5 +30,7 @@ class Idle : public Position {
 
 private:
     std::optional<RobotIntent> derived_get_task(RobotIntent intent) override;
+
+    std::shared_ptr<ClientHandles> clientHandles_;
 };
 }  // namespace strategy