feat(game_controller_hsl): finalize implementation of base states

Author: texhnolyze

src/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/game_status_capsule.py

@@ -1,7 +1,7 @@
 from typing import Optional
 
 from bitbots_utils.utils import get_parameters_from_other_node
-from game_controller_hl_interfaces.msg import GameState
+from game_controller_hsl_interfaces.msg import GameState
 from std_msgs.msg import Bool
 
 from bitbots_blackboard.capsules import AbstractBlackboardCapsule
@@ -20,32 +20,37 @@ def __init__(self, node, blackboard=None):
         self.last_goal_from_us_time = -86400.0
         self.last_goal_time = -86400.0
         self.free_kick_kickoff_team: Optional[bool] = None
-        self.game_controller_stop: Bool = False
+        self.game_controller_stop: bool = False
         # publish stopped msg for hcm
-        self.stop_pub = self.node.create_publisher(Bool, "game_controller/stop_msg", 1)
+        self.stop_pub = node.create_publisher(Bool, "game_controller/stop_msg", 1)
 
-    def get_gamestate(self) -> int:
-        # Init, ready, set, playing, finished, standby
+    def get_game_state(self) -> int:
+        # Init, ready, set, playing, finished
         return self.gamestate.main_state
 
-    def get_gamePhase(self) -> int:
-        # Timeout, Normal, Penaltyshoot
-        return self.gamestate.gamePhase
+    def get_game_phase(self) -> int:
+        # Timeout, Normal, Extratime, Penaltyshoot
+        return self.gamestate.game_phase
 
-    def get_setPlay(self) -> int:
-        # None, Kick In, Goalkick, Cornerkick, Pushing Freekick, Penaltykick
-        return self.gamestate.setPlay
+    def get_set_play(self) -> int:
+        # None, Direct Freekick, Indirect Freekick, Penalty, Throw in, Goalkick, Cornerkick,
+        return self.gamestate.set_play
 
     def get_secondary_team(self) -> int:
-        # Team ID, wer in set Play den Baall hat
-        return self.gamestate.kickingTeam
+        # Team ID, wer in set Play den Ball hat
+        return self.gamestate.kicking_team
 
     def has_kickoff(self) -> bool:
         # vegelcih mit eigener Teamnummer
-        return self.gamestate.kickingTeam == self.team_id
+        return self.gamestate.kicking_team == self.team_id
+
+    def is_stopped(self) -> bool:
+        return self.gamestate.stopped
 
     def has_penalty_kick(self) -> bool:
-        return self.gamestate.set_play == GameState.SET_PLAY_PENALTY_KICK and self.gamestate.kickingTeam == self.team_id
+        return (
+            self.gamestate.set_play == GameState.SET_PLAY_PENALTY_KICK and self.gamestate.kicking_team == self.team_id
+        )
 
     def get_our_goals(self) -> int:
         return self.gamestate.own_score
@@ -74,7 +79,7 @@ def get_secondary_seconds_remaining(self) -> float:
         )
 
     def get_seconds_since_unpenalized(self) -> float:
-        return self._node.get_clock().now().nanoseconds / 1e9 - self.seconds_till_unpenalized
+        return self._node.get_clock().now().nanoseconds / 1e9 - self.unpenalized_time
 
     def get_is_penalized(self) -> bool:
         return self.gamestate.penalized
@@ -98,7 +103,7 @@ def gamestate_callback(self, gamestate_msg: GameState) -> None:
 
         self.game_controller_stop = gamestate_msg.stopped
 
-        self.stop_pub.publish(self.game_controller_stop)
+        self.stop_pub.publish(Bool(data=self.game_controller_stop))
 
         """Anstoß im Falle von Overtime jetzt erstmal nicht genauer geregelt
         if (
@@ -109,13 +114,14 @@ def gamestate_callback(self, gamestate_msg: GameState) -> None:
             # secondary action is now executed but we will not see this in the new messages.
             # it will look like a normal kick off, but we need to remember that this is some sort of free kick
             # we set the kickoff value accordingly, then we will not be allowed to move if it is a kick for the others
-            self.free_kick_kickoff_team = gamestate_msg.kickingTeam
+            self.free_kick_kickoff_team = gamestate_msg.kicking_team
 
+        if gamestate_msg.set_play != 2 and gamestate_msg.secondary_time == 0:
+            self.free_kick_kickoff_team = gamestate_msg.kicking_team
 
-        if gamestate_msg.setPlay != 2 and gamestate_msg.secondaryTime == 0:
+        if gamestate_msg.set_play != 2 and gamestate_msg.secondary_time == 0:
             self.free_kick_kickoff_team = None
 
-
         if self.free_kick_kickoff_team is not None:
             gamestate_msg.has_kick_off = self.free_kick_kickoff_team == self.team_id
         """

src/bitbots_behavior/bitbots_blackboard/package.xml

@@ -23,7 +23,7 @@
     <build_depend>bitbots_docs</build_depend>
     <depend>bitbots_tf_buffer</depend>
     <depend>bitbots_utils</depend>
-    <depend>game_controller_hl_interfaces</depend>
+    <depend>game_controller_hsl_interfaces</depend>
     <depend>python3-numpy</depend>
     <depend>rclpy</depend>
     <depend>ros2_numpy</depend>

src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/decisions/game_state_decider.py

@@ -1,6 +1,6 @@
 from bitbots_blackboard.body_blackboard import BodyBlackboard
 from dynamic_stack_decider.abstract_decision_element import AbstractDecisionElement
-from game_controller_hl_interfaces.msg import GameState
+from game_controller_hsl_interfaces.msg import GameState
 
 
 class GameStateDecider(AbstractDecisionElement):
@@ -30,6 +30,11 @@ def perform(self, reevaluate=False):
             return "FINISHED"
         elif game_state_number == GameState.STATE_STANDBY:
             return "STANDBY"
+        else:
+            # This should never happen, but all cases required string response
+            # as we do not get any stack trace otherwise
+            self.blackboard.node.get_logger().error(f"Received unknown game state number: {game_state_number}")
+            return "UNKNOWN"
 
     def get_reevaluate(self):
         """

src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/decisions/secondary_state_decider.py

@@ -1,6 +1,6 @@
 from bitbots_blackboard.body_blackboard import BodyBlackboard
 from dynamic_stack_decider.abstract_decision_element import AbstractDecisionElement
-from game_controller_hl_interfaces.msg import GameState
+from game_controller_hsl_interfaces.msg import GameState
 
 
 class SecondaryStateDecider(AbstractDecisionElement):
@@ -16,6 +16,9 @@ def __init__(self, blackboard, dsd, parameters):
 
     def perform(self, reevaluate=False):
         state_number = self.blackboard.gamestate.get_secondary_state()
+        set_play_number = self.blackboard.gamestate.get_set_play()
+        game_phase_number = self.blackboard.gamestate.get_game_phase()
+
         # todo this is a temporary hack to make GUI work
         if state_number == GameState.STATE_NORMAL:
             return "NORMAL"
@@ -37,6 +40,11 @@ def perform(self, reevaluate=False):
             return "GOAL_KICK"
         elif state_number == GameState.STATE_THROW_IN:
             return "THROW_IN"
+        else:
+            self.blackboard.node.get_logger().error(
+                f"Unknown secondary state with game phase {game_phase_number} and set play {set_play_number}"
+            )
+            return "UNKNOWN"
 
     def get_reevaluate(self):
         """

src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/body_behavior.py

@@ -4,7 +4,7 @@
 from bitbots_blackboard.body_blackboard import BodyBlackboard
 from bitbots_tf_buffer import Buffer
 from dynamic_stack_decider.dsd import DSD
-from game_controller_hl_interfaces.msg import GameState
+from game_controller_hsl_interfaces.msg import GameState
 from geometry_msgs.msg import PoseWithCovarianceStamped, Twist
 from rclpy.callback_groups import MutuallyExclusiveCallbackGroup
 from rclpy.duration import Duration

src/bitbots_behavior/bitbots_body_behavior/package.xml

@@ -22,7 +22,7 @@
   <depend>bitbots_tts</depend>
   <depend>bitbots_utils</depend>
   <depend>dynamic_stack_decider</depend>
-  <depend>game_controller_hl_interfaces</depend>
+  <depend>game_controller_hsl_interfaces</depend>
   <depend>geometry_msgs</depend>
   <depend>python3-numpy</depend>
   <depend>rclpy</depend>

src/bitbots_lowlevel/bitbots_buttons/CMakeLists.txt

@@ -15,7 +15,7 @@ find_package(ament_cmake REQUIRED)
 find_package(backward_ros REQUIRED)
 find_package(bitbots_docs REQUIRED)
 find_package(bitbots_msgs REQUIRED)
-find_package(game_controller_hl_interfaces REQUIRED)
+find_package(game_controller_hsl_interfaces REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(std_srvs REQUIRED)
@@ -27,7 +27,7 @@ ament_target_dependencies(
   ament_cmake
   backward_ros
   bitbots_msgs
-  game_controller_hl_interfaces
+  game_controller_hsl_interfaces
   rclcpp
   std_msgs
   std_srvs

src/bitbots_lowlevel/bitbots_buttons/package.xml

@@ -20,14 +20,14 @@
   <depend>backward_ros</depend>
   <depend>bitbots_docs</depend>
   <depend>bitbots_msgs</depend>
-  <depend>game_controller_hl_interfaces</depend>
+  <depend>game_controller_hsl_interfaces</depend>
   <depend>rclcpp</depend>
   <depend>std_msgs</depend>
   <depend>std_srvs</depend>
   <depend>test_msgs</depend>
 
   <export>
-      <bitbots_documentation>
+    <bitbots_documentation>
       <language>python3</language>
       <status>tested_robot</status>
     </bitbots_documentation>

src/bitbots_lowlevel/bitbots_buttons/src/button_node.cpp

@@ -4,7 +4,7 @@
 #include <bitbots_msgs/msg/buttons.hpp>
 #include <bitbots_msgs/srv/manual_penalize.hpp>
 #include <bitbots_msgs/srv/set_teaching_mode.hpp>
-#include <game_controller_hl_interfaces/msg/game_state.hpp>
+#include <game_controller_hsl_interfaces/msg/game_state.hpp>
 #include <rclcpp/experimental/executors/events_executor/events_executor.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <std_msgs/msg/bool.hpp>
@@ -53,12 +53,12 @@ class ButtonNode : public rclcpp::Node {
     teaching_mode_client_ = this->create_client<bitbots_msgs::srv::SetTeachingMode>("teaching_mode");
     buttons_sub_ = this->create_subscription<bitbots_msgs::msg::Buttons>(
         "/buttons", 1, std::bind(&bitbots_buttons::ButtonNode::buttonCb, this, _1));
-    gamestate_sub_ = this->create_subscription<game_controller_hl_interfaces::msg::GameState>(
+    gamestate_sub_ = this->create_subscription<game_controller_hsl_interfaces::msg::GameState>(
         "gamestate", 1, std::bind(&bitbots_buttons::ButtonNode::gamestateCb, this, _1));
   }
 
   // Sets the in_game_ variable to true, if a Gamestate message from the Gamecontroller arrives.
-  void gamestateCb(const game_controller_hl_interfaces::msg::GameState::SharedPtr msg) { in_game_ = true; }
+  void gamestateCb(const game_controller_hsl_interfaces::msg::GameState::SharedPtr msg) { in_game_ = true; }
 
   void buttonCb(const bitbots_msgs::msg::Buttons::SharedPtr msg) {
     // button1 - red
@@ -197,7 +197,7 @@ class ButtonNode : public rclcpp::Node {
   rclcpp::Client<std_srvs::srv::Empty>::SharedPtr foot_zero_client_;
   rclcpp::Client<std_srvs::srv::SetBool>::SharedPtr power_client_;
   rclcpp::Subscription<bitbots_msgs::msg::Buttons>::SharedPtr buttons_sub_;
-  rclcpp::Subscription<game_controller_hl_interfaces::msg::GameState>::SharedPtr gamestate_sub_;
+  rclcpp::Subscription<game_controller_hsl_interfaces::msg::GameState>::SharedPtr gamestate_sub_;
 };
 }  // namespace bitbots_buttons
 

src/bitbots_misc/bitbots_bringup/launch/highlevel.launch

@@ -16,7 +16,7 @@
 
     <!-- launch game controller -->
     <group if="$(var game_controller)">
-        <include file="$(find-pkg-share game_controller_hl)/launch/game_controller.launch">
+        <include file="$(find-pkg-share game_controller_hsl)/launch/game_controller.launch">
             <arg name="sim" value="$(var sim)" />
             <arg name="use_parameter_blackboard" value="true" />
             <arg name="parameter_blackboard_name" value="parameter_blackboard" />