Relative motion fixes over wireless, and posture commands are blocking

README.md

@@ -54,19 +54,21 @@ robot.arm_to([0.1, 0.5, 0, 0, 0, 0])
 robot.move_to_nav_posture()
 
 # Move the robot back to origin
-# navigate_to() is only allowed in navigation mode
-robot.navigate_to([0, 0, 0])
+# move_base_to() is only allowed in navigation mode
+robot.move_base_to([0, 0, 0])
 
 # Move the robot 0.5m forward
-robot.navigate_to([0.5, 0, 0], relative=True)
+robot.move_base_to([0.5, 0, 0], relative=True)
 
 # Rotate the robot 90 degrees to the left
-robot.navigate_to([0, 0, 3.14159/2], relative=True)
+robot.move_base_to([0, 0, 3.14159/2], relative=True)
 
 # And to the right
-robot.navigate_to([0, 0, -3.14159/2], relative=True)
+robot.move_base_to([0, 0, -3.14159/2], relative=True)
 ```
 
+You can find this code in [examples/simple_navigation.py](examples/simple_navigation.py).
+
 ## Apps
 
 After [installation](#installation), on the robot, run the server:

examples/simple_motions.py

@@ -0,0 +1,64 @@
+# Copyright (c) Hello Robot, Inc.
+# All rights reserved.
+#
+# This source code is licensed under the license found in the LICENSE file in the root directory
+# of this source tree.
+#
+# Some code may be adapted from other open-source works with their respective licenses. Original
+# license information maybe found below, if so.
+
+from stretch.agent import RobotClient
+
+print("Initialize the robot client")
+print("robot = RobotClient()")
+robot = RobotClient()
+print("Done")
+# On future connection attempts, the IP address can be left blank
+
+# Turn head towards robot's hand
+print("Turn head towards robot's hand")
+print("robot.move_to_manip_posture()")
+robot.move_to_manip_posture()
+print("Done")
+
+# Move forward 0.1 along robot x axis in maniplation mode, and move arm to 0.5 meter height
+print("Move forward 0.1 along robot x axis in maniplation mode, and move arm to 0.5 meter height")
+print("robot.arm_to([0.1, 0.5, 0, 0, 0, 0], blocking=True)")
+robot.arm_to([0.1, 0.5, 0, 0, 0, 0])
+print("Done")
+
+# Turn head towards robot's base and switch base to navigation mode
+# In navigation mode, we can stream velocity commands to the base for smooth motions, and base
+# rotations are enabled
+print("Turn head towards robot's base and switch base to navigation mode")
+print("robot.move_to_nav_posture()")
+robot.move_to_nav_posture()
+print("Done")
+
+# Move the robot back to origin
+# move_base_to() is only allowed in navigation mode
+print("Move the robot back to origin")
+print("robot.move_base_to([0, 0, 0])")
+robot.move_base_to([0, 0, 0])
+print("Done")
+
+# Move the robot 0.5m forward
+print("Move the robot 0.25m forward")
+print("robot.move_base_to([0.25, 0, 0], relative=True)")
+robot.move_base_to([0.5, 0, 0], relative=True)
+print("Done")
+
+# Rotate the robot 90 degrees to the left
+print("Rotate the robot 90 degrees to the left")
+print("robot.move_base_to([0, 0, 3.14159/2], relative=True)")
+robot.move_base_to([0, 0, 3.14159 / 2], relative=True, verbose=True)
+print("Done")
+
+# And to the right
+print("Rotate the robot 90 degrees to the right")
+print("robot.move_base_to([0, 0, -3.14159/2], relative=True, blocking=True)")
+robot.move_base_to([0, 0, -3.14159 / 2], relative=True, verbose=True)
+
+print("Stop the robot")
+print("robot.stop()")
+robot.stop()

scripts/set_robot_ip.sh

@@ -0,0 +1,29 @@
+#!/bin/bash
+
+## Check if an IP address is provided
+if [ $# -eq 0 ]; then
+    echo "Error: Please provide an IP address as an argument."
+    echo "Usage: $0 <ip_address>"
+    exit 1
+fi
+
+## Validate IP address format
+if ! [[ $1 =~ ^[0-9]+\.[0-9]+\.[0-9]+\.[0-9]+$ ]]; then
+    echo "Error: Invalid IP address format. Please use xxx.xxx.xxx.xxx"
+    exit 1
+fi
+
+## Create directory if it doesn't exist
+mkdir -p ~/.stretch
+
+## Write IP address to file
+echo "$1" > ~/.stretch/robot_ip.txt
+
+## Verify file creation and content
+if [ -f ~/.stretch/robot_ip.txt ]; then
+    echo "Success: IP address $1 has been written to ~/.stretch/robot_ip.txt"
+else
+    echo "Error: Failed to create or write to ~/.stretch/robot_ip.txt"
+    exit 1
+fi
+

src/stretch/agent/operations/navigate.py

@@ -79,7 +79,7 @@ def run(self):
             theta = math.atan2(
                 xyz[1] - self.robot.get_base_pose()[1], xyz[0] - self.robot.get_base_pose()[0]
             )
-            self.robot.navigate_to(
+            self.robot.move_base_to(
                 np.array([start_xyz[0], start_xyz[1], theta + np.pi / 2]),
                 blocking=True,
                 timeout=30.0,
@@ -94,10 +94,10 @@ def run(self):
 
         # Orient the robot towards the object and use the end effector camera to pick it up
         xyt = self.plan.trajectory[-1].state
-        # self.robot.navigate_to(xyt + np.array([0, 0, np.pi / 2]), blocking=True, timeout=30.0)
+        # self.robot.move_base_to(xyt + np.array([0, 0, np.pi / 2]), blocking=True, timeout=30.0)
         if self.be_precise:
             self.warn("Moving again to make sure we're close enough to the goal.")
-            self.robot.navigate_to(xyt, blocking=True, timeout=30.0)
+            self.robot.move_base_to(xyt, blocking=True, timeout=30.0)
 
     def was_successful(self):
         """This will be successful if we got within a reasonable distance of the target object."""

src/stretch/agent/robot_agent.py

@@ -135,6 +135,9 @@ def __init__(
 
         self.reset_object_plans()
 
+        # Is this still running?
+        self._running = True
+
         # Store the current scene graph computed from detected objects
         self.scene_graph = None
 
@@ -164,8 +167,13 @@ def __init__(
             self.sub_socket = self.context.socket(zmq.SUB)
             self.sub_socket.connect(f"tcp://localhost:{obs_sub_port}")
             self.sub_socket.setsockopt_string(zmq.SUBSCRIBE, "")
+        else:
+            self._update_map_thread = None
+            self._get_observations_thread = None
 
     def __del__(self):
+        if self._update_map_thread is not None and self._update_map_thread.is_alive():
+            self._update_map_thread.join()
         self._update_map_thread.join()
 
     def _create_voxel_map(self, parameters: Parameters) -> SparseVoxelMap:
@@ -406,7 +414,7 @@ def rotate_in_place(
             steps += 1
         while i < steps:
             t0 = timeit.default_timer()
-            self.robot.navigate_to(
+            self.robot.move_base_to(
                 [x, y, theta + (i * step_size)],
                 relative=False,
                 blocking=True,
@@ -459,7 +467,8 @@ def show_map(self):
         )
 
     def get_observations_loop(self):
-        while True:
+        """Threaded function that gets observations in real-time."""
+        while self.robot.running and self._running:
             obs = None
             t0 = timeit.default_timer()
 
@@ -615,8 +624,8 @@ def update_map_with_pose_graph(self):
         self._obs_history_lock.release()
 
     def update_map_loop(self):
-        """Threaded function that updates our voxel map in real-time"""
-        while True:
+        """Threaded function that updates our voxel map in real-time."""
+        while self.robot.running and self._running:
             with self._robot_lock:
                 self.update_map_with_pose_graph()
             time.sleep(0.5)
@@ -989,11 +998,11 @@ def recover_from_invalid_start(self) -> bool:
         if self.space.is_valid(xyt_goal_backward, verbose=True):
             logger.warning("Trying to move backwards...")
             # Compute the position forward or backward from the robot
-            self.robot.navigate_to(xyt_goal_backward, relative=False)
+            self.robot.move_base_to(xyt_goal_backward, relative=False)
         elif self.space.is_valid(xyt_goal_forward, verbose=True):
             logger.warning("Trying to move forward...")
             # Compute the position forward or backward from the robot
-            self.robot.navigate_to(xyt_goal_forward, relative=False)
+            self.robot.move_base_to(xyt_goal_forward, relative=False)
         else:
             logger.warning("Could not recover from invalid start state!")
             return False
@@ -1074,15 +1083,15 @@ def move_to_any_instance(
                 print("ROBOT IS STUCK! Move back!")
                 r = np.random.randint(3)
                 if r == 0:
-                    self.robot.navigate_to([-0.1, 0, 0], relative=True, blocking=True)
+                    self.robot.move_base_to([-0.1, 0, 0], relative=True, blocking=True)
                 elif r == 1:
-                    self.robot.navigate_to([0, 0, np.pi / 4], relative=True, blocking=True)
+                    self.robot.move_base_to([0, 0, np.pi / 4], relative=True, blocking=True)
                 elif r == 2:
-                    self.robot.navigate_to([0, 0, -np.pi / 4], relative=True, blocking=True)
+                    self.robot.move_base_to([0, 0, -np.pi / 4], relative=True, blocking=True)
                 return False
 
             time.sleep(1.0)
-            self.robot.navigate_to([0, 0, np.pi / 2], relative=True)
+            self.robot.move_base_to([0, 0, np.pi / 2], relative=True)
             self.robot.move_to_manip_posture()
             return True
 
@@ -1445,7 +1454,7 @@ def run_exploration(
             if not start_is_valid:
                 print("Start not valid. back up a bit.")
                 print(f"robot base pose: {self.robot.get_base_pose()}")
-                self.robot.navigate_to([-0.1, 0, 0], relative=True)
+                self.robot.move_base_to([-0.1, 0, 0], relative=True)
                 continue
 
             # Now actually plan to the frontier
@@ -1503,11 +1512,11 @@ def run_exploration(
 
                 r = np.random.randint(3)
                 if r == 0:
-                    self.robot.navigate_to([-0.1, 0, 0], relative=True, blocking=True)
+                    self.robot.move_base_to([-0.1, 0, 0], relative=True, blocking=True)
                 elif r == 1:
-                    self.robot.navigate_to([0, 0, np.pi / 4], relative=True, blocking=True)
+                    self.robot.move_base_to([0, 0, np.pi / 4], relative=True, blocking=True)
                 elif r == 2:
-                    self.robot.navigate_to([0, 0, -np.pi / 4], relative=True, blocking=True)
+                    self.robot.move_base_to([0, 0, -np.pi / 4], relative=True, blocking=True)
 
             # Append latest observations
             if not self._realtime_updates:
@@ -1564,7 +1573,7 @@ def move_closed_loop(self, goal: np.ndarray, max_time: float = 10.0) -> bool:
         t0 = timeit.default_timer()
         self.robot.move_to_nav_posture()
         while True:
-            self.robot.navigate_to(goal, blocking=False, timeout=30.0)
+            self.robot.move_base_to(goal, blocking=False, timeout=30.0)
             if self.robot.last_motion_failed():
                 return False
             position = self.robot.get_base_pose()