Cleanup behavior repo (#334)

README.md

@@ -1,6 +1,4 @@
 # bitbots_behavior
 
 This is the behavior code of the RoboCup Humanoid League team Hamburg Bit-Bots.
-It is divided into a behavior for the head and the body.
-Both are programmed using the dynamic_stack_decider package (https://github.com/bit-bots/dynamic_stack_decider).
-
+It is build using the dynamic_stack_decider framework (https://github.com/bit-bots/dynamic_stack_decider).

bitbots_blackboard/CMakeLists.txt

@@ -9,7 +9,6 @@ endif()
 find_package(bio_ik_msgs REQUIRED)
 find_package(ament_cmake REQUIRED)
 find_package(sensor_msgs REQUIRED)
-find_package(humanoid_league_msgs REQUIRED)
 find_package(rclpy REQUIRED)
 find_package(tf2 REQUIRED)
 find_package(bitbots_msgs REQUIRED)
@@ -20,15 +19,15 @@ find_package(geometry_msgs REQUIRED)
 find_package(bitbots_docs REQUIRED)
 
 set(INCLUDE_DIRS ${bio_ik_msgs_INCLUDE_DIRS} ${ament_cmake_INCLUDE_DIRS}
-  ${sensor_msgs_INCLUDE_DIRS} ${humanoid_league_msgs_INCLUDE_DIRS}
+  ${sensor_msgs_INCLUDE_DIRS}
   ${rclpy_INCLUDE_DIRS} ${tf2_INCLUDE_DIRS} ${bitbots_msgs_INCLUDE_DIRS}
   ${std_msgs_INCLUDE_DIRS} ${tf2_geometry_msgs_INCLUDE_DIRS}
   ${std_srvs_INCLUDE_DIRS} ${geometry_msgs_INCLUDE_DIRS}
   ${bitbots_docs_INCLUDE_DIRS})
 include_directories(${INCLUDE_DIRS})
 
 set(LIBRARY_DIRS ${bio_ik_msgs_LIBRARY_DIRS} ${ament_cmake_LIBRARY_DIRS}
-  ${sensor_msgs_LIBRARY_DIRS} ${humanoid_league_msgs_LIBRARY_DIRS}
+  ${sensor_msgs_LIBRARY_DIRS}
   ${rclpy_LIBRARY_DIRS} ${tf2_LIBRARY_DIRS} ${bitbots_msgs_LIBRARY_DIRS}
   ${std_msgs_LIBRARY_DIRS} ${tf2_geometry_msgs_LIBRARY_DIRS}
   ${std_srvs_LIBRARY_DIRS} ${geometry_msgs_LIBRARY_DIRS}
@@ -37,7 +36,7 @@ set(LIBRARY_DIRS ${bio_ik_msgs_LIBRARY_DIRS} ${ament_cmake_LIBRARY_DIRS}
 link_directories(${LIBRARY_DIRS})
 
 set(LIBS ${bio_ik_msgs_LIBRARIES} ${ament_cmake_LIBRARIES}
-  ${sensor_msgs_LIBRARIES} ${humanoid_league_msgs_LIBRARIES} ${rclpy_LIBRARIES}
+  ${sensor_msgs_LIBRARIES} ${rclpy_LIBRARIES}
   ${tf2_LIBRARIES} ${bitbots_msgs_LIBRARIES} ${std_msgs_LIBRARIES}
   ${tf2_geometry_msgs_LIBRARIES} ${std_srvs_LIBRARIES} ${geometry_msgs_LIBRARIES}
   ${bitbots_docs_LIBRARIES})
@@ -48,7 +47,6 @@ enable_bitbots_docs()
 ament_export_dependencies(bio_ik_msgs)
 ament_export_dependencies(ament_cmake)
 ament_export_dependencies(sensor_msgs)
-ament_export_dependencies(humanoid_league_msgs)
 ament_export_dependencies(rclpy)
 ament_export_dependencies(tf2)
 ament_export_dependencies(bitbots_msgs)

bitbots_blackboard/bitbots_blackboard/blackboard.py

@@ -1,47 +1,35 @@
-from typing import Optional
-
 from bitbots_blackboard.capsules.animation_capsule import AnimationCapsule
-from bitbots_blackboard.capsules.blackboard_capsule import BlackboardCapsule
+from bitbots_blackboard.capsules.misc_capsule import MiscCapsule
 from bitbots_blackboard.capsules.game_status_capsule import GameStatusCapsule
 from bitbots_blackboard.capsules.kick_capsule import KickCapsule
 from bitbots_blackboard.capsules.pathfinding_capsule import PathfindingCapsule
 from bitbots_blackboard.capsules.team_data_capsule import TeamDataCapsule
 from bitbots_blackboard.capsules.world_model_capsule import WorldModelCapsule
 from bitbots_blackboard.capsules.costmap_capsule import CostmapCapsule
 from bitbots_utils.utils import get_parameter_dict
-from rclpy.action import ActionClient
 from rclpy.node import Node
 import tf2_ros as tf2
-from rclpy.publisher import Publisher
 
 
 class BodyBlackboard:
     def __init__(self, node: Node, tf_buffer: tf2.Buffer):
+        # References
         self.node = node
         self.tf_buffer = tf_buffer
 
+        # Config
         self.config = get_parameter_dict(node, "body")
         self.base_footprint_frame: str = self.node.get_parameter("base_footprint_frame").value
         self.map_frame: str = self.node.get_parameter("map_frame").value
         self.odom_frame: str = self.node.get_parameter("odom_frame").value
         self.in_sim: bool = self.node.get_parameter("use_sim_time").value
-        self.blackboard = BlackboardCapsule(node)
+
+        # Capsules
+        self.misc = MiscCapsule(node)
         self.gamestate = GameStatusCapsule(node)
         self.animation = AnimationCapsule(node)
         self.kick = KickCapsule(self)
         self.world_model = WorldModelCapsule(self)
         self.costmap = CostmapCapsule(self)
         self.pathfinding = PathfindingCapsule(self, node)
         self.team_data = TeamDataCapsule(node)
-        self.goalie_arms_animation: str = self.node.get_parameter("Animations.Goalie.goalieArms").value
-        self.goalie_falling_right_animation: str = self.node.get_parameter("Animations.Goalie.fallRight").value
-        self.goalie_falling_left_animation: str = self.node.get_parameter("Animations.Goalie.fallLeft").value
-        self.goalie_falling_center_animation: str = self.node.get_parameter("Animations.Goalie.fallCenter").value
-        self.cheering_animation: str = self.node.get_parameter("Animations.Misc.cheering").value
-        self.init_animation: str = self.node.get_parameter("Animations.Misc.init").value
-
-        self.dynup_action_client: Optional[ActionClient] = None
-        self.dynup_cancel_pub: Optional[Publisher] = None
-        self.hcm_deactivate_pub: Optional[Publisher] = None
-
-        self.lookat_action_client: Optional[ActionClient] = None

bitbots_blackboard/bitbots_blackboard/capsules/animation_capsule.py

@@ -1,25 +1,44 @@
 """
 AnimationCapsule
 ^^^^^^^^^^^^^^^^
+
+Communicates with the animation action server and plays predefined animations.
 """
 from rclpy.action import ActionClient
+from rclpy.callback_groups import ReentrantCallbackGroup
 from rclpy.duration import Duration
 from rclpy.node import Node
-from rclpy.callback_groups import ReentrantCallbackGroup
 
-from humanoid_league_msgs.action import PlayAnimation
+from bitbots_msgs.action import Dynup, LookAt, PlayAnimation
 
 
 class AnimationCapsule:
     def __init__(self, node: Node):
         self.node = node
         self.active = False
+
+        # Config
+        self.goalie_arms_animation: str = self.node.get_parameter("Animations.Goalie.goalieArms").value
+        self.goalie_falling_right_animation: str = self.node.get_parameter("Animations.Goalie.fallRight").value
+        self.goalie_falling_left_animation: str = self.node.get_parameter("Animations.Goalie.fallLeft").value
+        self.goalie_falling_center_animation: str = self.node.get_parameter("Animations.Goalie.fallCenter").value
+        self.cheering_animation: str = self.node.get_parameter("Animations.Misc.cheering").value
+        self.init_animation: str = self.node.get_parameter("Animations.Misc.init").value
+
         self.animation_client = ActionClient(
             node,
             PlayAnimation,
             'animation',
             callback_group=ReentrantCallbackGroup())
 
+        self.dynup_action_client = ActionClient(
+            node,
+            Dynup,
+            "dynup",
+            callback_group=ReentrantCallbackGroup())
+
+        self.lookat_action_client = ActionClient(node, LookAt, "look_at_goal")
+
     def play_animation(self, animation: str, from_hcm: bool) -> bool:
         """
         plays the animation "ani" and sets the flag "BusyAnimation"

bitbots_blackboard/bitbots_blackboard/capsules/costmap_capsule.py

@@ -12,7 +12,6 @@
 
 import matplotlib.pyplot as plt
 import numpy as np
-import ros2_numpy
 import tf2_ros as tf2
 from bitbots_utils.utils import (get_parameter_dict,
                                  get_parameters_from_other_node)
@@ -22,11 +21,12 @@
 from rclpy.clock import ClockType
 from rclpy.duration import Duration
 from rclpy.time import Time
+from ros2_numpy import numpify, msgify
 from scipy.interpolate import griddata
 from scipy.ndimage import gaussian_filter
 from soccer_vision_3d_msgs.msg import Robot, RobotArray
-from tf2_geometry_msgs import PointStamped
 from tf_transformations import euler_from_quaternion, quaternion_from_euler
+from tf2_geometry_msgs import PointStamped
 
 
 class CostmapCapsule:
@@ -64,11 +64,11 @@ def __init__(self, blackboard: "BodyBlackboard"):
 
     def robot_callback(self, msg: RobotArray):
         """
-        Callback with new obstacles
+        Callback with new robot detections
         """
         # Init a new obstacle costmap
         obstacle_map = np.zeros_like(self.costmap)
-        # Iterate over all obstacles
+        # Iterate over all robots
         robot: Robot
         for robot in msg.robots:
             # Convert position to array index
@@ -95,7 +95,7 @@ def publish_costmap(self):
         normalized_costmap = (255 - ((self.costmap - np.min(self.costmap)) / (
                     np.max(self.costmap) - np.min(self.costmap))) * 255 / 2.1).astype(np.int8).T
         # Build the OccupancyGrid message
-        msg: OccupancyGrid = ros2_numpy.msgify(
+        msg: OccupancyGrid = msgify(
             OccupancyGrid,
             normalized_costmap,
             info=MapMetaData(
@@ -124,9 +124,9 @@ def get_pass_regions(self) -> np.ndarray:
         for pose in self._blackboard.team_data.get_active_teammate_poses(count_goalies=False):
             # Get positions
             goal_position = np.array([self.field_length / 2, 0, 0])  # position of the opponent goal
-            teammate_position = ros2_numpy.numpify(pose.position)
+            teammate_position = numpify(pose.position)
             # Get vector
-            vector_teammate_to_goal = goal_position - ros2_numpy.numpify(pose.position)
+            vector_teammate_to_goal = goal_position - numpify(pose.position)
             # Position between robot and goal but 1m away from the robot
             pass_pos = vector_teammate_to_goal / np.linalg.norm(vector_teammate_to_goal) * pass_dist + teammate_position
             # Convert position to array index
@@ -169,7 +169,7 @@ def cost_at_relative_xy(self, x: float, y: float) -> float:
             return 0.0
 
         point = PointStamped()
-        point.header.stamp = Time(seconds=0, nanoseconds=0, clock_type=ClockType.ROS_TIME).to_msg()
+        point.header.stamp = Time(clock_type=ClockType.ROS_TIME).to_msg()
         point.header.frame_id = self.base_footprint_frame
         point.point.x = x
         point.point.y = y
@@ -310,7 +310,7 @@ def get_cost_of_kick_relative(self, x: float, y: float, direction: float, kick_l
             return 0.0
 
         pose = PoseStamped()
-        pose.header.stamp = Time(seconds=0, nanoseconds=0, clock_type=ClockType.ROS_TIME).to_msg()
+        pose.header.stamp = Time(clock_type=ClockType.ROS_TIME).to_msg()
         pose.header.frame_id = self.base_footprint_frame
         pose.pose.position.x = float(x)
         pose.pose.position.y = float(y)
@@ -324,8 +324,7 @@ def get_cost_of_kick_relative(self, x: float, y: float, direction: float, kick_l
         except (tf2.ConnectivityException, tf2.LookupException, tf2.ExtrapolationException) as e:
             self._blackboard.node.get_logger().warn(e)
             return 0.0
-        d = euler_from_quaternion(
-            [pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w])[2]
+        d = euler_from_quaternion(numpify(pose.pose.orientation))[2]
         return self.get_cost_of_kick(pose.pose.position.x, pose.pose.position.y, d, kick_length, angular_range)
 
     def get_cost_of_kick(self, x: float, y: float, direction: float, kick_length: float, angular_range: float) -> float:

bitbots_blackboard/bitbots_blackboard/capsules/game_status_capsule.py

@@ -3,12 +3,11 @@
 ^^^^^^^^^^^^^^^^^
 
 Provides information about the current game state.
-
 """
 from rclpy.node import Node
 
+from bitbots_msgs.msg import GameState
 from bitbots_utils.utils import get_parameters_from_other_node
-from humanoid_league_msgs.msg import GameState
 
 
 class GameStatusCapsule:
@@ -23,7 +22,7 @@ def __init__(self, node: Node):
         self.free_kick_kickoff_team = None
 
     def is_game_state_equals(self, value):
-        assert value in [GameState.GAMESTATE_PLAYING, GameState.GAMESTATE_FINISHED, GameState.GAMESTATE_INITAL,
+        assert value in [GameState.GAMESTATE_PLAYING, GameState.GAMESTATE_FINISHED, GameState.GAMESTATE_INITIAL,
                          GameState.GAMESTATE_READY, GameState.GAMESTATE_SET]
         return value == self.get_gamestate()
 
@@ -54,30 +53,30 @@ def get_opp_goals(self):
         return self.gamestate.rival_score
 
     def get_seconds_since_own_goal(self):
-        return float(self.node.get_clock().now().seconds_nanoseconds()[0] + self.node.get_clock().now().seconds_nanoseconds()[1]/1e9) - self.last_goal_from_us_time
+        return self.node.get_clock().now().nanoseconds / 1e9 - self.last_goal_from_us_time
 
     def get_seconds_since_any_goal(self):
-        return float(self.node.get_clock().now().seconds_nanoseconds()[0] + self.node.get_clock().now().seconds_nanoseconds()[1]/1e9) - self.last_goal_time
+        return self.node.get_clock().now().nanoseconds / 1e9 - self.last_goal_time
 
     def get_seconds_remaining(self):
         # Time from the message minus time passed since receiving it
-        return max(self.gamestate.seconds_remaining - (float(self.node.get_clock().now().seconds_nanoseconds()[0] + self.node.get_clock().now().seconds_nanoseconds()[1]/1e9) - self.last_update), 0)
+        return max(self.gamestate.seconds_remaining - (self.node.get_clock().now().nanoseconds / 1e9 - self.last_update), 0)
 
     def get_secondary_seconds_remaining(self):
         """Seconds remaining for things like kickoff"""
         # Time from the message minus time passed since receiving it
-        return max(self.gamestate.secondary_seconds_remaining - (float(self.node.get_clock().now().seconds_nanoseconds()[0] + self.node.get_clock().now().seconds_nanoseconds()[1]/1e9) - self.last_update), 0)
+        return max(self.gamestate.secondary_seconds_remaining - (self.node.get_clock().now().nanoseconds / 1e9 - self.last_update), 0)
 
     def get_seconds_since_last_drop_ball(self):
         """Returns the seconds since the last drop in"""
         if self.gamestate.drop_in_time == -1:
             return None
         else:
             # Time from the message plus seconds passed since receiving it
-            return self.gamestate.drop_in_time + (float(self.node.get_clock().now().seconds_nanoseconds()[0] + self.node.get_clock().now().seconds_nanoseconds()[1]/1e9) - self.last_update)
+            return self.gamestate.drop_in_time + (self.node.get_clock().now().nanoseconds / 1e9 - self.last_update)
 
     def get_seconds_since_unpenalized(self):
-        return float(self.node.get_clock().now().seconds_nanoseconds()[0] + self.node.get_clock().now().seconds_nanoseconds()[1]/1e9) - self.unpenalized_time
+        return self.node.get_clock().now().nanoseconds / 1e9 - self.unpenalized_time
 
     def get_is_penalized(self):
         return self.gamestate.penalized
@@ -93,14 +92,14 @@ def get_red_cards(self):
 
     def gamestate_callback(self, gs: GameState):
         if self.gamestate.penalized and not gs.penalized:
-            self.unpenalized_time = float(self.node.get_clock().now().seconds_nanoseconds()[0] + self.node.get_clock().now().seconds_nanoseconds()[1]/1e9)
+            self.unpenalized_time = self.node.get_clock().now().nanoseconds / 1e9
 
         if gs.own_score > self.gamestate.own_score:
-            self.last_goal_from_us_time = float(self.node.get_clock().now().seconds_nanoseconds()[0] + self.node.get_clock().now().seconds_nanoseconds()[1]/1e9)
-            self.last_goal_time = float(self.node.get_clock().now().seconds_nanoseconds()[0] + self.node.get_clock().now().seconds_nanoseconds()[1]/1e9)
+            self.last_goal_from_us_time = self.node.get_clock().now().nanoseconds / 1e9
+            self.last_goal_time = self.node.get_clock().now().nanoseconds / 1e9
 
         if gs.rival_score > self.gamestate.rival_score:
-            self.last_goal_time = float(self.node.get_clock().now().seconds_nanoseconds()[0] + self.node.get_clock().now().seconds_nanoseconds()[1]/1e9)
+            self.last_goal_time = self.node.get_clock().now().nanoseconds / 1e9
 
         if gs.secondary_state_mode == 2 and self.gamestate.secondary_state_mode != 2 \
                 and gs.game_state == GameState.GAMESTATE_PLAYING:
@@ -115,5 +114,5 @@ def gamestate_callback(self, gs: GameState):
         if self.free_kick_kickoff_team is not None:
             gs.has_kick_off = self.free_kick_kickoff_team == self.team_id
 
-        self.last_update = float(self.node.get_clock().now().seconds_nanoseconds()[0] + self.node.get_clock().now().seconds_nanoseconds()[1]/1e9)
+        self.last_update = self.node.get_clock().now().nanoseconds / 1e9
         self.gamestate = gs

bitbots_blackboard/bitbots_blackboard/capsules/kick_capsule.py

@@ -1,3 +1,9 @@
+"""
+KickCapsule
+^^^^^^^^^^^^^^^^
+
+Communicates with the dynamic_kick.
+"""
 from typing import TYPE_CHECKING, Optional
 
 if TYPE_CHECKING: