Skip to content

Commit bb7d064

Browse files
cleWu03cleWu03
committed
stopped from game_controller
1 parent e696382 commit bb7d064

5 files changed

Lines changed: 60 additions & 34 deletions

File tree

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

Lines changed: 8 additions & 4 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

@@ -20,6 +21,9 @@ def __init__(self, node, blackboard=None):
2021
self.last_goal_from_us_time = -86400.0
2122
self.last_goal_time = -86400.0
2223
self.free_kick_kickoff_team: Optional[bool] = None
24+
self.game_controller_stop: Bool = False
25+
#publish stopped msg for hcm
26+
self.stop_pub = self.node.create_publisher(Bool, "game_controller/stop_msg", 1)
2327

2428
def get_game_state(self) -> int:
2529
#Init, ready, set, playing, finished
@@ -70,14 +74,12 @@ def get_seconds_remaining(self) -> float:
7074
# Time from the message minus time passed since receiving it
7175
return max(
7276
self.gamestate.secs_remaining - (self._node.get_clock().now().nanoseconds / 1e9 - self.last_update), 0.0
73-
self.gamestate.secs_remaining - (self._node.get_clock().now().nanoseconds / 1e9 - self.last_update), 0.0
7477
)
7578

7679
def get_secondary_seconds_remaining(self) -> float:
7780
"""Seconds remaining for things like kickoff"""
7881
# Time from the message minus time passed since receiving it
7982
return max(
80-
self.gamestate.secondary_time
8183
self.gamestate.secondary_time
8284
- (self._node.get_clock().now().nanoseconds / 1e9 - self.last_update),
8385
0.0,
@@ -107,7 +109,10 @@ def gamestate_callback(self, gamestate_msg: GameState) -> None:
107109
if gamestate_msg.rival_score > self.gamestate.rival_score:
108110
self.last_goal_time = self._node.get_clock().now().nanoseconds / 1e9
109111

110-
'''Anstoß im Falle von Overtime jetzt erstmal nicht genauer geregelt
112+
self.game_controller_stop = gamestate_msg.stopped
113+
114+
self.stop_pub.publish(self.game_controller_stop)
115+
111116
'''Anstoß im Falle von Overtime jetzt erstmal nicht genauer geregelt
112117
if (
113118
gamestate_msg.main_state == GameState.STATE_SET
@@ -135,6 +140,5 @@ def gamestate_callback(self, gamestate_msg: GameState) -> None:
135140
if self.free_kick_kickoff_team is not None:
136141
gamestate_msg.has_kick_off = self.free_kick_kickoff_team == self.team_id
137142
'''
138-
'''
139143
self.last_update = self._node.get_clock().now().nanoseconds / 1e9
140144
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)