Look along path while approaching beds

mosfet/bot.py

@@ -67,6 +67,16 @@ def look_direction_from_eye(position, target):
     return eye - target
 
 
+def navigation_look_target(path_points):
+    if len(path_points) > YAW_LOOK_AHEAD:
+        point = path_points[YAW_LOOK_AHEAD]
+    elif path_points:
+        point = path_points[-1]
+    else:
+        return None
+    return vector.Point3D((point[0], point[1] + 1, point[2]))
+
+
 def update_pitch(g, look_at_d):
     if look_at_d is None:
         target_pitch = 0.0
@@ -247,10 +257,8 @@ def tick(global_state):
 
     if g.look_at:
         look_at = vector.Point3D(g.look_at)
-    elif g.path and len(g.path) > YAW_LOOK_AHEAD:
-        look_at = vector.Point3D(g.path[YAW_LOOK_AHEAD])
     elif g.path and len(g.path):
-        look_at = vector.Point3D(g.path[-1])
+        look_at = navigation_look_target(g.path)
     else:
         look_at = None
 

mosfet/jobs/sleep_with_bed.py

@@ -126,7 +126,7 @@ class SleepWithBedStates:
             self.g.path = navpath[:-1]
             self.opening_deadline = time.monotonic() + max(
                 8.0, len(navpath) * 0.75)
-            self.g.look_at = self.area
+            self.g.look_at = None
             self.state = self.going_to_bed
             return
 
@@ -149,7 +149,10 @@ class SleepWithBedStates:
             print('Could not reach bed opening', self.opening,
                   '; trying another bed')
             self.bad_beds.append(self.area)
+            self.g.look_at = None
             self.state = self.choose_bed
+        else:
+            self.g.look_at = None
 
     def turn_to_bed(self):
         self.g.look_at = self.area
@@ -209,7 +212,7 @@ class SleepWithBedStates:
         self.g.path = navpath
         self.opening_deadline = time.monotonic() + max(
             8.0, len(navpath) * 0.75)
-        self.g.look_at = self.area
+        self.g.look_at = None
         self.state = self.going_to_area
 
     def going_to_area(self):
@@ -222,7 +225,10 @@ class SleepWithBedStates:
         elif time.monotonic() >= self.opening_deadline:
             print('Could not reach bed placement opening', self.opening)
             self.bad_areas.append(self.area)
+            self.g.look_at = None
             self.state = self.cleanup
+        else:
+            self.g.look_at = None
 
     def turn_to_bed_area(self):
         self.g.look_at = self.area

tests/test_farming_pitch.py

@@ -2,7 +2,10 @@ import unittest
 
 from munch import Munch
 
-from mosfet.bot import look_direction_from_eye, update_pitch, update_yaw
+from mosfet.bot import (
+    look_direction_from_eye, navigation_look_target, update_pitch,
+    update_yaw,
+)
 from mosfet.vector import Point3D, Vector3D
 
 
@@ -58,6 +61,16 @@ class TestFarmingPitch(unittest.TestCase):
         self.assertGreaterEqual(state.yaw, -180.0)
         self.assertLess(state.yaw, 180.0)
 
+    def test_path_navigation_looks_at_the_block_above_its_waypoint(self):
+        path = [(index, 64, 0) for index in range(6)]
+
+        target = navigation_look_target(path)
+
+        self.assertEqual(target.tuple(), (4, 65, 0))
+        self.assertEqual(navigation_look_target([(1, 64, 2)]).tuple(),
+                         (1, 65, 2))
+        self.assertIsNone(navigation_look_target([]))
+
     def test_pitch_tracks_look_target_instead_of_a_fixed_farming_angle(self):
         state = Munch(pitch=0.0)
         direction_to_crop = LookDirection(45.0)

tests/test_sleep_with_bed.py

@@ -243,6 +243,37 @@ class TestSleepWithBed(unittest.TestCase):
         self.assertEqual(state.beds, [])
         self.assertEqual(state.state, state.choose_bed)
 
+    def test_bed_approach_follows_path_before_aiming_at_bed(self):
+        bed = (4, 64, 2)
+        opening = (3, 64, 0)
+        state = SleepWithBedStates(Munch(
+            pos=(0, 64, 0), path=[opening], look_at=bed,
+            game=FakeGame(), world=FakeWorld()))
+        state.area = bed
+        state.opening = opening
+        state.opening_deadline = 9999999999
+
+        state.going_to_bed()
+        self.assertIsNone(state.g.look_at)
+
+        state.g.pos = opening
+        state.going_to_bed()
+        self.assertEqual(state.g.look_at, bed)
+        self.assertEqual(state.state, state.turn_to_bed)
+
+    def test_bed_placement_approach_follows_path(self):
+        opening = (3, 64, 0)
+        state = SleepWithBedStates(Munch(
+            pos=(0, 64, 0), path=[opening], look_at=(4, 64, 2),
+            game=FakeGame(), world=FakeWorld()))
+        state.area = (4, 64, 2)
+        state.opening = opening
+        state.opening_deadline = 9999999999
+
+        state.going_to_area()
+
+        self.assertIsNone(state.g.look_at)
+
     def test_arrival_accepts_completed_path_with_small_position_error(self):
         state = SleepWithBedStates(Munch(
             pos=(0, 64, 0), path=[(1, 64, 0)], look_at=None,