feat(command-executor): movement safety

internal/services/command/executor/move_to_executor.go

@@ -33,30 +33,23 @@ func newMoveToExecutor(
 func (e moveToExecutor) Execute(ctx context.Context, inputs command.MoveToInputs) (command.MoveToOutputs, error) {
 	wg := sync.WaitGroup{}
 
+	runCtx, cancelRunCtx := context.WithCancel(ctx)
+	defer cancelRunCtx()
+
 	wg.Add(1)
 	go func() {
-		defer wg.Done()
+		defer func() {
+			wg.Done()
+			cancelRunCtx() // if reached location, we don't need to run anymore
+		}()
 		e.trackingLocationUntilReached(ctx, inputs.Location)
 	}()
 
-	switch inputs.Direction {
-	case command.MoveDirectionForward:
-		if err := e.driveMotorService.MoveForward(ctx, drivemotor.MoveForwardParams{
-			Speed: inputs.MotorSpeed,
-		}); err != nil {
-			return command.MoveToOutputs{}, fmt.Errorf("failed to move forward: %w", err)
-		}
-
-	case command.MoveDirectionBackward:
-		if err := e.driveMotorService.MoveBackward(ctx, drivemotor.MoveBackwardParams{
-			Speed: inputs.MotorSpeed,
-		}); err != nil {
-			return command.MoveToOutputs{}, fmt.Errorf("failed to move backward: %w", err)
-		}
-
-	default:
-		return command.MoveToOutputs{}, fmt.Errorf("invalid move direction: %s", inputs.Direction)
-	}
+	wg.Add(1)
+	go func() {
+		defer wg.Done()
+		e.run(runCtx, inputs)
+	}()
 
 	wg.Wait()
 
@@ -101,3 +94,205 @@ func (e moveToExecutor) trackingLocationUntilReached(ctx context.Context, locati
 	case <-ctx.Done():
 	}
 }
+
+func (e moveToExecutor) run(ctx context.Context, inputs command.MoveToInputs) {
+	e.log.Info("start running with safety monitor", slog.Any("inputs", inputs))
+
+	evChan := make(chan events.UpdateDistanceSensorEvent, 1)
+	e.subscriber.Subscribe(ctx, events.DistanceSensorUpdatedTopic, func(msg *eventbus.Message) {
+		ev, ok := msg.Payload.(events.UpdateDistanceSensorEvent)
+		if !ok {
+			e.log.Error("invalid distance sensor event", slog.Any("event", msg.Payload))
+			return
+		}
+
+		select {
+		case evChan <- ev:
+		default:
+			e.log.Warn("distance sensor event channel is full, dropping event", slog.Any("event", ev))
+		}
+	})
+
+	runDriveMotorFunc := func() error {
+		// TODO: we can optimize this func
+		return e.runDriveMotor(ctx, inputs.Direction, inputs.MotorSpeed)
+	}
+
+	stopDriveMotorFunc := func() error {
+		return e.driveMotorService.Stop(ctx)
+	}
+
+	h := moveToSafetyHandler{
+		log:                  e.log,
+		stopDriveMotorFunc:   stopDriveMotorFunc,
+		resumeDriveMotorFunc: runDriveMotorFunc,
+		Direction:            inputs.Direction,
+		ObstacleTracking:     inputs.ObstacleTracking,
+		CargoLostDistance:    inputs.CargoLostDistance,
+	}
+
+	for {
+		select {
+		case <-ctx.Done():
+			return
+		case ev := <-evChan:
+			h.Handle(ev)
+		}
+	}
+}
+
+func (e moveToExecutor) runDriveMotor(ctx context.Context, direction command.MoveDirection, speed uint8) error {
+	switch direction {
+	case command.MoveDirectionForward:
+		if err := e.driveMotorService.MoveForward(ctx, drivemotor.MoveForwardParams{
+			Speed: speed,
+		}); err != nil {
+			return fmt.Errorf("failed to move forward: %w", err)
+		}
+
+	case command.MoveDirectionBackward:
+		if err := e.driveMotorService.MoveBackward(ctx, drivemotor.MoveBackwardParams{
+			Speed: speed,
+		}); err != nil {
+			return fmt.Errorf("failed to move backward: %w", err)
+		}
+
+	default:
+		return fmt.Errorf("invalid move direction: %s", direction)
+	}
+
+	return nil
+}
+
+type moveToSafetyHandler struct {
+	log                  *slog.Logger
+	stopDriveMotorFunc   func() error
+	resumeDriveMotorFunc func() error
+
+	Direction         command.MoveDirection
+	ObstacleTracking  command.ObstacleTracking
+	CargoLostDistance uint16
+
+	isDriveMotorRunning bool
+	cargoLost           bool
+	obstacleDetected    bool
+}
+
+func (m *moveToSafetyHandler) Handle(ev events.UpdateDistanceSensorEvent) {
+	currentCargoLost := m.isCargoLost(ev.DownDistance)
+
+	// cargo lost
+	if currentCargoLost && !m.cargoLost {
+		m.cargoLost = true
+		m.log.Warn("cargo lost detected", slog.Uint64("down_distance", uint64(ev.DownDistance)))
+		m.stopMovement()
+		// TODO: tracking lift cargo here, should block until cargo is lifted
+		return
+	}
+
+	// cargo back in position
+	if !currentCargoLost && m.cargoLost {
+		m.cargoLost = false
+		m.log.Info("cargo back in position", slog.Uint64("down_distance", uint64(ev.DownDistance)))
+	}
+
+	// If cargo is lost, we don't need to handle obstacle detection
+	// cargo lost has higher priority than obstacle detection
+	if currentCargoLost {
+		return
+	}
+
+	currentObstacleDetected := m.isObstacleDetected(ev)
+
+	// obstacle detected
+	if currentObstacleDetected && !m.obstacleDetected {
+		m.obstacleDetected = true
+		m.log.Warn("obstacle detected",
+			slog.String("direction", string(m.Direction)),
+			slog.Uint64("front_distance", uint64(ev.FrontDistance)),
+			slog.Uint64("back_distance", uint64(ev.BackDistance)))
+		m.stopMovement()
+		return
+	}
+
+	// obstacle cleared
+	if !currentObstacleDetected && m.obstacleDetected {
+		m.obstacleDetected = false
+		m.log.Info("obstacle cleared",
+			slog.String("direction", string(m.Direction)),
+			slog.Uint64("front_distance", uint64(ev.FrontDistance)),
+			slog.Uint64("back_distance", uint64(ev.BackDistance)))
+		m.resumeMovement()
+		return
+	}
+}
+
+//   - There are two thresholds: EnterDistance (start detecting obstacle),
+//     ExitDistance (stop detecting obstacle).
+//
+//   - If `obstacleDetected` is already true (we are currently tracking an obstacle):
+//
+//   - Only stop tracking when actual distance > ExitDistance.
+//
+//   - Otherwise, still considered in obstacle state.
+//
+//   - If `obstacleDetected` is false:
+//
+//   - Start tracking when actual distance < EnterDistance.
+//
+// Diagram:
+// `
+//
+//	   ↓ decreasing distance                  ↑ increasing distance
+//	┌────────────────────────────┐      ┌───────────────────────────┐
+//	│ actualDistance < EnterDist ├─────▶│ obstacleDetected = true   │
+//	└────────────────────────────┘      └───────────────────────────┘
+//	                                             │
+//	       ┌─────────────────────────────────────┘
+//	       │
+//	       ▼
+//	┌────────────────────────────┐
+//	│ actualDistance > ExitDist  ├─────▶ obstacleDetected = false
+//	└────────────────────────────┘
+//
+// `
+func (m *moveToSafetyHandler) isObstacleDetected(ev events.UpdateDistanceSensorEvent) bool {
+	actualDistance := ev.FrontDistance
+	if m.Direction == command.MoveDirectionBackward {
+		actualDistance = ev.BackDistance
+	}
+
+	if m.obstacleDetected {
+		return actualDistance < m.ObstacleTracking.ExitDistance
+	}
+
+	return actualDistance < m.ObstacleTracking.EnterDistance
+}
+
+func (m *moveToSafetyHandler) isCargoLost(downDistance uint16) bool {
+	return downDistance > m.CargoLostDistance
+}
+
+func (m *moveToSafetyHandler) stopMovement() {
+	if !m.isDriveMotorRunning {
+		return
+	}
+
+	if err := m.stopDriveMotorFunc(); err != nil {
+		m.log.Error("failed to stop motor", slog.Any("error", err))
+	}
+
+	m.isDriveMotorRunning = false
+}
+
+func (m *moveToSafetyHandler) resumeMovement() {
+	if m.isDriveMotorRunning || m.cargoLost {
+		return
+	}
+
+	if err := m.resumeDriveMotorFunc(); err != nil {
+		m.log.Error("failed to resume motor", slog.Any("error", err))
+	}
+
+	m.isDriveMotorRunning = true
+}

internal/services/command/inputs.go

@@ -67,10 +67,17 @@ const (
 	MoveDirectionBackward MoveDirection = "BACKWARD"
 )
 
+type ObstacleTracking struct {
+	EnterDistance uint16 `json:"enter_distance"`
+	ExitDistance  uint16 `json:"exit_distance"`
+}
+
 type MoveToInputs struct {
-	Location   string        `json:"location" validate:"required"`
-	Direction  MoveDirection `json:"direction" validate:"required,enum"`
-	MotorSpeed uint8         `json:"motor_speed" validate:"required,max=100"`
+	Location          string           `json:"location" validate:"required"`
+	Direction         MoveDirection    `json:"direction" validate:"required,enum"`
+	MotorSpeed        uint8            `json:"motor_speed" validate:"required,max=100"`
+	ObstacleTracking  ObstacleTracking `json:"obstacle_tracking" validate:"required"`
+	CargoLostDistance uint16           `json:"cargo_lost_distance" validate:"required,max=250"`
 }
 
 func (MoveToInputs) CommandType() CommandType {