New image builds and new tests

Signed-off-by: Aaron Chong <aaronchongth@gmail.com>

.github/docker/minimal-nav1-bringup/Dockerfile

@@ -9,8 +9,13 @@ RUN mkdir -p /tb3 && cd /tb3 \
 
 ENV TURTLEBOT3_MODEL=burger
 
+# Modify existing launch file to add initial pose
+RUN cd /tb3 \
+  && curl --output turtlebot3_navigation.launch "https://raw.githubusercontent.com/ROBOTIS-GIT/turtlebot3/refs/heads/noetic-devel/turtlebot3_navigation/launch/turtlebot3_navigation.launch" \
+  && sed -z 's|amcl.launch"/>|amcl.launch"><arg name="initial_pose_x" value="-2.0"/><arg name="initial_pose_y" value="-0.5"/></include>|' turtlebot3_navigation.launch > turtlebot3_navigation_edited.launch
+
 RUN rm -rf \
   /var/lib/apt/lists \
   /dist
 
-ENTRYPOINT ["bash", "-c", ". /opt/ros/$ROS_DISTRO/setup.bash && roslaunch --wait turtlebot3_navigation turtlebot3_navigation.launch map_file:=/tb3/navigation2/nav2_bringup/maps/tb3_sandbox.yaml open_rviz:=false initial_pose_x:=-2.0 initial_pose_y:=-0.5 initial_pose_a:=0.0"]
+ENTRYPOINT ["bash", "-c", ". /opt/ros/$ROS_DISTRO/setup.bash && roslaunch --wait /tb3/turtlebot3_navigation_edited.launch map_file:=/tb3/navigation2/nav2_bringup/maps/tb3_sandbox.yaml open_rviz:=false"]

.github/workflows/nightly.yaml

@@ -1,11 +1,11 @@
 name: nightly
 
-on:
-  schedule:
-    # 2am SGT
-    - cron: '0 18 * * *'
+# on:
+#   schedule:
+#     # 2am SGT
+#     - cron: '0 18 * * *'
 
-# on: [push]
+on: [push]
 
 jobs:
   build-minimal-nav2-docker-images:

free_fleet_adapter/tests/integration/test_nav1_robot_adapter.py

@@ -105,6 +105,16 @@ def test_robot_battery_soc(self):
             tf_buffer=tf_buffer
         )
 
+        robot_exists = False
+        for i in range(10):
+            transform = robot_adapter.get_pose()
+            if transform is not None:
+                robot_exists = True
+                break
+            time.sleep(1)
+
+        assert robot_exists
+
         battery_soc = robot_adapter.get_battery_soc()
         assert math.isclose(battery_soc, 1.0)
 
@@ -123,6 +133,16 @@ def test_robot_unable_to_update(self):
             tf_buffer=tf_buffer
         )
 
+        robot_exists = False
+        for i in range(10):
+            transform = robot_adapter.get_pose()
+            if transform is not None:
+                robot_exists = True
+                break
+            time.sleep(1)
+
+        assert robot_exists
+
         able_to_update = False
         try:
             robot_adapter.update(
@@ -151,6 +171,16 @@ def test_idle_robot_navigate_is_done(self):
             fleet_handle=None,
             tf_buffer=tf_buffer
         )
+
+        robot_exists = False
+        for i in range(10):
+            transform = robot_adapter.get_pose()
+            if transform is not None:
+                robot_exists = True
+                break
+            time.sleep(1)
+
+        assert robot_exists
         assert robot_adapter._is_navigation_done()
 
     def test_robot_stop_without_command(self):
@@ -167,6 +197,16 @@ def test_robot_stop_without_command(self):
             fleet_handle=None,
             tf_buffer=tf_buffer
         )
+
+        robot_exists = False
+        for i in range(10):
+            transform = robot_adapter.get_pose()
+            if transform is not None:
+                robot_exists = True
+                break
+            time.sleep(1)
+
+        assert robot_exists
         assert robot_adapter.execution is None
         robot_adapter.stop(None)
         assert robot_adapter.execution is None
@@ -187,6 +227,16 @@ def test_robot_handle_navigate_to_invalid_map(self):
             tf_buffer=tf_buffer
         )
 
+        robot_exists = False
+        for i in range(10):
+            transform = robot_adapter.get_pose()
+            if transform is not None:
+                robot_exists = True
+                break
+            time.sleep(1)
+
+        assert robot_exists
+
         able_to_handle_navigate = False
         try:
             robot_adapter._handle_navigate_to_pose(
@@ -201,35 +251,82 @@ def test_robot_handle_navigate_to_invalid_map(self):
             able_to_handle_navigate = False
         assert not able_to_handle_navigate
 
-    # def test_robot_stop_navigate(self):
-    #     tf_buffer = Buffer()
-
-    #     robot_adapter = Nav1RobotAdapter(
-    #         name='tb3_0',
-    #         configuration=None,
-    #         robot_config_yaml={
-    #             'initial_map': 'L1',
-    #         },
-    #         node=self.node,
-    #         zenoh_session=self.zenoh_session,
-    #         fleet_handle=None,
-    #         tf_buffer=tf_buffer
-    #     )
-
-    #     robot_adapter._handle_navigate_to_pose(
-    #         'L1',
-    #         -1.6,
-    #         -0.5,
-    #         0.0,
-    #         0.0,
-    #         5.0
-    #     )
-    #     assert robot_adapter.nav_goal_id is not None
-    #     assert not robot_adapter._is_navigation_done()
-    #     time.sleep(1)
-    #     robot_adapter._handle_stop_navigation()
-    #     time.sleep(1)
-    #     assert robot_adapter._is_navigation_done()
+    def test_robot_handle_navigate_to_pose(self):
+        tf_buffer = Buffer()
+
+        robot_adapter = Nav1RobotAdapter(
+            name='tb3_0',
+            configuration=None,
+            robot_config_yaml={
+                'initial_map': 'L1',
+            },
+            node=self.node,
+            zenoh_session=self.zenoh_session,
+            fleet_handle=None,
+            tf_buffer=tf_buffer
+        )
+
+        robot_exists = False
+        for i in range(10):
+            transform = robot_adapter.get_pose()
+            if transform is not None:
+                robot_exists = True
+                break
+            time.sleep(1)
+
+        assert robot_exists
+
+        robot_adapter._handle_navigate_to_pose(
+            'L1',
+            -1.8,
+            -0.5,
+            0.0,
+            0.0,
+            5.0
+        )
+        assert not robot_adapter._is_navigation_done()
+        time.sleep(5)
+        assert robot_adapter._is_navigation_done()
+
+    def test_robot_stop_navigate(self):
+        tf_buffer = Buffer()
+
+        robot_adapter = Nav1RobotAdapter(
+            name='tb3_0',
+            configuration=None,
+            robot_config_yaml={
+                'initial_map': 'L1',
+            },
+            node=self.node,
+            zenoh_session=self.zenoh_session,
+            fleet_handle=None,
+            tf_buffer=tf_buffer
+        )
+
+        robot_exists = False
+        for i in range(10):
+            transform = robot_adapter.get_pose()
+            if transform is not None:
+                robot_exists = True
+                break
+            time.sleep(1)
+
+        assert robot_exists
+
+        robot_adapter._handle_navigate_to_pose(
+            'L1',
+            1.808,
+            0.503,
+            0.0,
+            0.0,
+            5.0
+        )
+        assert robot_adapter.nav_goal_id is not None
+        assert not robot_adapter._is_navigation_done()
+        time.sleep(1)
+        robot_adapter._handle_stop_navigation()
+        time.sleep(1)
+        assert robot_adapter._is_navigation_done()
 
     def test_robot_execute_unknown_action(self):
         tf_buffer = Buffer()

free_fleet_examples/free_fleet_examples/nav1_move_base_cancel.py

@@ -17,12 +17,9 @@
 
 import argparse
 import sys
-import time
-
 
 from free_fleet_adapter.nav1_robot_adapter import Nav1MoveBaseHandler
 import rclpy
-from tf2_ros import Buffer
 
 import zenoh
 

free_fleet_examples/free_fleet_examples/nav1_move_base_simple_goal.py

@@ -22,7 +22,6 @@
 
 from free_fleet_adapter.nav1_robot_adapter import Nav1MoveBaseHandler
 import rclpy
-from tf2_ros import Buffer
 
 import zenoh