Fix: Continue immediately after successful ball placement

Author: g3force

internal/app/engine/common.go

@@ -95,15 +95,15 @@ func createStageChange(stage *state.Referee_Stage) *statemachine.Change {
 	}
 }
 
-func (e *Engine) findRobotInsideRadius(robots []*Robot, pos *geom.Vector2, radius float64) *Robot {
+func (e *Engine) findRobotInsideRadius(robots []*Robot, pos *geom.Vector2, radius float64) (matchingRobots []*Robot) {
 	for _, robot := range robots {
 		distance := robot.Pos.DistanceTo(pos)
 		if distance < radius {
-			return robot
+			matchingRobots = append(matchingRobots, robot)
 		}
 	}
 
-	return nil
+	return
 }
 
 func goDur(duration *durationpb.Duration) time.Duration {

internal/app/engine/process_continue_next_command.go

@@ -4,7 +4,10 @@ import (
 	"fmt"
 	"github.com/RoboCup-SSL/ssl-game-controller/internal/app/geom"
 	"github.com/RoboCup-SSL/ssl-game-controller/internal/app/state"
+	"github.com/RoboCup-SSL/ssl-game-controller/pkg/timer"
 	"google.golang.org/protobuf/types/known/timestamppb"
+	"strings"
+	"time"
 )
 
 const distanceToBallDuringPenalty = 1.0
@@ -28,7 +31,11 @@ func (e *Engine) createNextCommandContinueAction(
 		readyAt = nil
 	} else {
 		if lastReadyAt == nil {
-			readyAt = timestamppb.New(e.timeProvider().Add(e.gameConfig.PreparationTimeBeforeResume))
+			if e.placementSucceededRecently() {
+				readyAt = timestamppb.New(e.timeProvider())
+			} else {
+				readyAt = timestamppb.New(e.timeProvider().Add(e.gameConfig.PreparationTimeBeforeResume))
+			}
 		} else {
 			readyAt = lastReadyAt
 		}
@@ -53,6 +60,20 @@ func (e *Engine) createNextCommandContinueAction(
 	}
 }
 
+func (e *Engine) placementSucceededRecently() bool {
+	events := e.currentState.FindGameEvents(state.GameEvent_PLACEMENT_SUCCEEDED)
+	for _, event := range events {
+		if event.CreatedTimestamp == nil {
+			continue
+		}
+		placementSucceededTime := timer.TimestampMicroToTime(event.GetCreatedTimestamp())
+		if e.timeProvider().Sub(placementSucceededTime) < time.Second {
+			return true
+		}
+	}
+	return false
+}
+
 func (e *Engine) LastContinueAction(actionType ContinueAction_Type) *ContinueAction {
 	for _, action := range e.gcState.ContinueActions {
 		if *action.Type == actionType {
@@ -175,9 +196,23 @@ func (e *Engine) readyToContinueFromStop() (issues []string) {
 		issues = append(issues, "Blue team has too many robots")
 	}
 	radius := e.gameConfig.DistanceToBallInStop + robotRadius - distanceThreshold
-	robotNearBall := e.findRobotInsideRadius(e.trackerStateGc.Robots, e.trackerStateGc.Ball.Pos.ToVector2(), radius)
-	if robotNearBall != nil {
-		issues = append(issues, fmt.Sprintf("Robot %s is too close to ball", robotNearBall.Id.PrettyString()))
+
+	var robots []*Robot
+	if e.currentState.NextCommand != nil && *e.currentState.NextCommand.Type == state.Command_DIRECT {
+		// Only check for opponent teams robots
+		// This is not strictly correct, but necessary to continue directly after a ball placement, without
+		// waiting for robots to move out of the stop radius.
+		robots = robotsOfTeam(e.trackerStateGc.Robots, e.currentState.NextCommand.ForTeam.Opposite())
+	} else {
+		robots = e.trackerStateGc.Robots
+	}
+	robotsNearBall := e.findRobotInsideRadius(robots, e.trackerStateGc.Ball.Pos.ToVector2(), radius)
+	if len(robotsNearBall) > 0 {
+		var robotsStr []string
+		for _, robot := range robotsNearBall {
+			robotsStr = append(robotsStr, robot.Id.PrettyString())
+		}
+		issues = append(issues, fmt.Sprintf("Robots too close to ball: %s", strings.Join(robotsStr, ", ")))
 	}
 	if e.currentState.PlacementPos != nil {
 		ballToPlacementPosDist := e.currentState.PlacementPos.DistanceTo(e.trackerStateGc.Ball.Pos.ToVector2())
@@ -192,6 +227,15 @@ func (e *Engine) readyToContinueFromStop() (issues []string) {
 	return
 }
 
+func robotsOfTeam(robots []*Robot, team state.Team) (teamRobots []*Robot) {
+	for _, robot := range robots {
+		if *robot.Id.Team == team {
+			teamRobots = append(teamRobots, robot)
+		}
+	}
+	return
+}
+
 func (e *Engine) penaltyKeeperId() *state.RobotId {
 	forTeam := e.currentState.Command.ForTeam.Opposite()
 	teamInfo := e.currentState.TeamState[forTeam.String()]

pkg/timer/provider.go

@@ -15,3 +15,9 @@ func TimestampToTime(timestamp float64) time.Time {
 	nSec := int64((timestamp - float64(sec)) * 1e9)
 	return time.Unix(sec, nSec)
 }
+
+func TimestampMicroToTime(timestamp uint64) time.Time {
+	sec := int64(timestamp / 1000)
+	nSec := int64((timestamp - uint64(sec)) * 1000)
+	return time.Unix(sec, nSec)
+}