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