Skip to content

Commit 9dad19f

Browse files
cleWu03texhnolyze
cleWu03
authored and
texhnolyze
committed
stopped from game_controller
1 parent 0ce3052 commit 9dad19f

5 files changed

Lines changed: 72 additions & 45 deletions

File tree

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

Lines changed: 20 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,7 @@
22

33
from bitbots_utils.utils import get_parameters_from_other_node
44
from game_controller_hl_interfaces.msg import GameState
5+
from std_msgs.msg import Bool
56

67
from bitbots_blackboard.capsules import AbstractBlackboardCapsule
78

@@ -19,31 +20,32 @@ def __init__(self, node, blackboard=None):
1920
self.last_goal_from_us_time = -86400.0
2021
self.last_goal_time = -86400.0
2122
self.free_kick_kickoff_team: Optional[bool] = None
23+
self.game_controller_stop: Bool = False
24+
# publish stopped msg for hcm
25+
self.stop_pub = self.node.create_publisher(Bool, "game_controller/stop_msg", 1)
2226

2327
def get_gamestate(self) -> int:
24-
#Init, ready, set, playing, finished, standby
28+
# Init, ready, set, playing, finished, standby
2529
return self.gamestate.main_state
2630

2731
def get_gamePhase(self) -> int:
28-
#Timeout, Normal, Penaltyshoot
32+
# Timeout, Normal, Penaltyshoot
2933
return self.gamestate.gamePhase
3034

3135
def get_setPlay(self) -> int:
32-
#None, Kick In, Goalkick, Cornerkick, Pushing Freekick, Penaltykick
36+
# None, Kick In, Goalkick, Cornerkick, Pushing Freekick, Penaltykick
3337
return self.gamestate.setPlay
3438

3539
def get_secondary_team(self) -> int:
36-
#Team ID, wer in set Play den Baall hat
40+
# Team ID, wer in set Play den Baall hat
3741
return self.gamestate.kickingTeam
3842

3943
def has_kickoff(self) -> bool:
40-
#vegelcih mit eigener Teamnummer
44+
# vegelcih mit eigener Teamnummer
4145
return self.gamestate.kickingTeam == self.team_id
4246

4347
def has_penalty_kick(self) -> bool:
44-
return(
45-
self.gamestate.set_play == GameState.SET_PLAY_PENALTY_KICK
46-
and self.gamestate.kickingTeam == self.team_id)
48+
return self.gamestate.set_play == GameState.SET_PLAY_PENALTY_KICK and self.gamestate.kickingTeam == self.team_id
4749

4850
def get_our_goals(self) -> int:
4951
return self.gamestate.own_score
@@ -67,8 +69,7 @@ def get_secondary_seconds_remaining(self) -> float:
6769
"""Seconds remaining for things like kickoff"""
6870
# Time from the message minus time passed since receiving it
6971
return max(
70-
self.gamestate.secondary_time
71-
- (self._node.get_clock().now().nanoseconds / 1e9 - self.last_update),
72+
self.gamestate.secondary_time - (self._node.get_clock().now().nanoseconds / 1e9 - self.last_update),
7273
0.0,
7374
)
7475

@@ -95,7 +96,11 @@ def gamestate_callback(self, gamestate_msg: GameState) -> None:
9596
if gamestate_msg.rival_score > self.gamestate.rival_score:
9697
self.last_goal_time = self._node.get_clock().now().nanoseconds / 1e9
9798

98-
'''Anstoß im Falle von Overtime jetzt erstmal nicht genauer geregelt
99+
self.game_controller_stop = gamestate_msg.stopped
100+
101+
self.stop_pub.publish(self.game_controller_stop)
102+
103+
"""Anstoß im Falle von Overtime jetzt erstmal nicht genauer geregelt
99104
if (
100105
gamestate_msg.main_state == GameState.STATE_SET
101106
and self.gamestate.setPlay != 2
@@ -105,14 +110,14 @@ def gamestate_callback(self, gamestate_msg: GameState) -> None:
105110
# it will look like a normal kick off, but we need to remember that this is some sort of free kick
106111
# we set the kickoff value accordingly, then we will not be allowed to move if it is a kick for the others
107112
self.free_kick_kickoff_team = gamestate_msg.kickingTeam
108-
109-
113+
114+
110115
if gamestate_msg.setPlay != 2 and gamestate_msg.secondaryTime == 0:
111116
self.free_kick_kickoff_team = None
112-
117+
113118
114119
if self.free_kick_kickoff_team is not None:
115120
gamestate_msg.has_kick_off = self.free_kick_kickoff_team == self.team_id
116-
'''
121+
"""
117122
self.last_update = self._node.get_clock().now().nanoseconds / 1e9
118123
self.gamestate = gamestate_msg

‎src/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/decisions/stop.py‎

Lines changed: 15 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -15,3 +15,18 @@ def perform(self, reevaluate=False):
1515

1616
def get_reevaluate(self):
1717
return True
18+
19+
class GameControllerStop(AbstractHCMDecisionElement):
20+
"""
21+
Handles manual stops
22+
"""
23+
24+
def perform(self, reevaluate=False):
25+
if self.blackboard.game_controller_stop:
26+
# we do an action sequence to go into stop and to stay there
27+
return "STOPPED"
28+
else:
29+
return "FREE"
30+
31+
def get_reevaluate(self):
32+
return True

‎src/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/hcm.dsd‎

Lines changed: 32 additions & 30 deletions
Original file line numberDiff line numberDiff line change
@@ -21,33 +21,35 @@ $StartHCM
2121
TEACH --> @RobotStateRecord, @SetTorque + stiff:false, @Wait
2222
HOLD --> @SetTorque + stiff:true, @Wait
2323
FINISHED --> @SetTorque + stiff:true + r:false, @RobotStateControllable, @PlayAnimationDynup + direction:walkready + r:false
24-
OFF --> $Stop
25-
STOPPED --> @RobotStatePenalty, @CancelGoals, @StopWalking, @PlayAnimationDynup + direction:walkready + r:false, @Wait
26-
FREE -->$CheckIMU
27-
IMU_NOT_STARTED --> @RobotStateStartup, @WaitForIMUStartup
28-
PROBLEM --> @RobotStateHardwareProblem, @WaitForIMU
29-
OKAY --> $CheckPressureSensor
30-
PRESSURE_NOT_STARTED --> @RobotStateStartup, @WaitForPressureStartup
31-
PROBLEM --> @RobotStateHardwareProblem, @WaitForPressure
32-
OKAY --> $PickedUp
33-
PICKED_UP --> @RobotStatePickedUp, @PlayAnimationDynup + direction:walkready + r:false, @Wait
34-
ON_GROUND --> $Fallen
35-
FALLEN_FRONT --> @RobotStateFallen, @CancelGoals, @StopWalking, @RobotStateGettingUp, @PlayAnimationStandupFront, @SetSquat + squat:true
36-
FALLEN_BACK --> @RobotStateFallen, @CancelGoals, @StopWalking, @RobotStateGettingUp, @SetFootZero, @PlayAnimationFallingBack, @PlayAnimationStandupBack, @SetSquat + squat:true
37-
FALLEN_RIGHT --> @RobotStateFallen, @CancelGoals, @StopWalking, @PlayAnimationTurningFrontRight
38-
FALLEN_LEFT --> @RobotStateFallen, @CancelGoals, @StopWalking, @PlayAnimationTurningFrontLeft
39-
NOT_FALLEN --> $Falling
40-
FALLING_LEFT --> @RobotStateFalling, @CancelGoals, @StopWalking, @PlayAnimationFallingLeft, @Wait
41-
FALLING_RIGHT --> @RobotStateFalling, @CancelGoals, @StopWalking, @PlayAnimationFallingRight, @Wait
42-
FALLING_FRONT --> @RobotStateFalling, @CancelGoals, @StopWalking, @PlayAnimationFallingFront, @Wait
43-
FALLING_BACK --> @RobotStateFalling, @CancelGoals, @StopWalking, @PlayAnimationFallingBack, @Wait
44-
NOT_FALLING --> $InSquat
45-
YES --> @RobotStateGettingUp, @Complain, @PlayAnimationDynup + direction:rise, @SetSquat + squat:false
46-
NO --> $PlayingExternalAnimation
47-
ANIMATION_RUNNING --> @StopWalking, @RobotStateAnimationRunning, @Wait
48-
ANIMATION_SERVER_TIMEOUT --> @CancelAnimation
49-
FREE --> $RecentWalkingGoals
50-
STAY_WALKING --> @RobotStateWalking, @Wait
51-
NOT_WALKING --> $RecentKickGoals
52-
KICKING --> @RobotStateKicking, @Wait
53-
NOT_KICKING --> @RobotStateControllable, @Wait
24+
OFF --> $GameControllerStop
25+
STOPPED --> @RobotStatePenalty, @CancelGoals, @StopWalking, @Wait
26+
FREE --> $Stop
27+
STOPPED --> @RobotStatePenalty, @CancelGoals, @StopWalking, @PlayAnimationDynup + direction:walkready + r:false, @Wait
28+
FREE -->$CheckIMU
29+
IMU_NOT_STARTED --> @RobotStateStartup, @WaitForIMUStartup
30+
PROBLEM --> @RobotStateHardwareProblem, @WaitForIMU
31+
OKAY --> $CheckPressureSensor
32+
PRESSURE_NOT_STARTED --> @RobotStateStartup, @WaitForPressureStartup
33+
PROBLEM --> @RobotStateHardwareProblem, @WaitForPressure
34+
OKAY --> $PickedUp
35+
PICKED_UP --> @RobotStatePickedUp, @PlayAnimationDynup + direction:walkready + r:false, @Wait
36+
ON_GROUND --> $Fallen
37+
FALLEN_FRONT --> @RobotStateFallen, @CancelGoals, @StopWalking, @RobotStateGettingUp, @PlayAnimationStandupFront, @SetSquat + squat:true
38+
FALLEN_BACK --> @RobotStateFallen, @CancelGoals, @StopWalking, @RobotStateGettingUp, @SetFootZero, @PlayAnimationFallingBack, @PlayAnimationStandupBack, @SetSquat + squat:true
39+
FALLEN_RIGHT --> @RobotStateFallen, @CancelGoals, @StopWalking, @PlayAnimationTurningFrontRight
40+
FALLEN_LEFT --> @RobotStateFallen, @CancelGoals, @StopWalking, @PlayAnimationTurningFrontLeft
41+
NOT_FALLEN --> $Falling
42+
FALLING_LEFT --> @RobotStateFalling, @CancelGoals, @StopWalking, @PlayAnimationFallingLeft, @Wait
43+
FALLING_RIGHT --> @RobotStateFalling, @CancelGoals, @StopWalking, @PlayAnimationFallingRight, @Wait
44+
FALLING_FRONT --> @RobotStateFalling, @CancelGoals, @StopWalking, @PlayAnimationFallingFront, @Wait
45+
FALLING_BACK --> @RobotStateFalling, @CancelGoals, @StopWalking, @PlayAnimationFallingBack, @Wait
46+
NOT_FALLING --> $InSquat
47+
YES --> @RobotStateGettingUp, @Complain, @PlayAnimationDynup + direction:rise, @SetSquat + squat:false
48+
NO --> $PlayingExternalAnimation
49+
ANIMATION_RUNNING --> @StopWalking, @RobotStateAnimationRunning, @Wait
50+
ANIMATION_SERVER_TIMEOUT --> @CancelAnimation
51+
FREE --> $RecentWalkingGoals
52+
STAY_WALKING --> @RobotStateWalking, @Wait
53+
NOT_WALKING --> $RecentKickGoals
54+
KICKING --> @RobotStateKicking, @Wait
55+
NOT_KICKING --> @RobotStateControllable, @Wait

‎src/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/hcm_blackboard.py‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -25,6 +25,7 @@ def __init__(self, node: Node):
2525
# Basic state
2626
self.current_state: T_RobotControlState = RobotControlState.STARTUP
2727
self.stopped: bool = False
28+
self.game_controller_stop: bool = False
2829

2930
# Save start time
3031
self.start_time: Time = self.node.get_clock().now()

‎src/bitbots_motion/bitbots_hcm/bitbots_hcm/humanoid_control_module.py‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -80,6 +80,7 @@ def __init__(self, use_sim_time, simulation_active, visualization_active):
8080
self.node.create_subscription(Bool, "core/power_switch_status", self.power_cb, 1)
8181
self.node.create_subscription(Bool, "hcm_deactivate", self.deactivate_cb, 1)
8282
self.node.create_subscription(DiagnosticArray, "diagnostics_agg", self.diag_cb, 1)
83+
self.node.create_subscription(Bool, "game_controller/stop_msg", self.stop_cb, 1)
8384

8485
# Create services
8586
self.node.create_service(SetBool, "record_mode", self.set_record_mode_callback)
@@ -122,6 +123,9 @@ def deactivate_cb(self, msg: Bool):
122123
"""Deactivates the HCM."""
123124
self.hcm_deactivated = msg.data
124125

126+
def stop_cb(self, msg: Bool):
127+
self.blackboard.game_controller_stop = msg.data
128+
125129
def set_manual_penalize_mode_callback(self, req: ManualPenalize.Request, resp: ManualPenalize.Response):
126130
"""Callback for the manual penalize service."""
127131
if req.penalize == ManualPenalize.Request.OFF:

0 commit comments

Comments
 (0)