attempt to check distance to waypoint

Author: fabid

artefacts.yaml

@@ -34,13 +34,8 @@ jobs:
     scenarios:
       defaults: # Global to all scenarios, and overriden in specific scenarios.
         output_dirs: ["output"]
-        metrics:
-          - /odometry_error
-          - /distance_from_start_gt
-          - /distance_from_start_est
         params:
           launch/world: ["empty.sdf"]
-          controller_server/controller_frequency: [10.0]
       settings:
         - name: reach_goal
           run: "python3 -m pytest  src/sam_bot_nav2_gz/test/test_reach_goal_pytest.py -s --junit-xml output/tests_junit.xml"

src/sam_bot_nav2_gz/test/test_reach_goal_pytest.py

@@ -30,6 +30,7 @@
 import os
 sys.path.append(os.path.dirname(__file__))
 from sim_state import IgnitionSimStateUtil, EntityNotFoundError
+from sim_state.metric_value import Pose
 
 
 
@@ -60,7 +61,7 @@ def merge_ros_params_files(source, override, destination):
     with open(destination, "w") as f:
         yaml.dump(merged_params, f)
 
-@pytest.fixture()
+@pytest.fixture(scope="module")
 def reach_goal_proc():
     reach_goal = Node(
         package="sam_bot_nav2_gz",
@@ -71,8 +72,8 @@ def reach_goal_proc():
     return reach_goal
 
 
-@launch_pytest.fixture
-def launch_description(reach_goal_proc):
+@launch_pytest.fixture(scope="module")
+def launch_description(reach_goal_proc, sim):
     """Launch description fixture for pytest-based testing"""
     try:
         world = get_artefacts_param("launch", "world", default="empty.sdf")
@@ -100,11 +101,10 @@ def launch_description(reach_goal_proc):
         launch_arguments=[
             ("run_headless", run_headless),
             ("world_file", world),
-            ("params_file", new_params_file),
+            #("params_file", new_params_file),
             ],
     )
 
- 
 
     # Gazebo ros bridge
     gz_bridge = Node(
@@ -120,115 +120,30 @@ def launch_description(reach_goal_proc):
         output="screen",
         )
 
-    gz_env = {'GZ_SIM_SYSTEM_PLUGIN_PATH':
-           ':'.join([os.environ.get('GZ_SIM_SYSTEM_PLUGIN_PATH', default=''),
-                     os.environ.get('LD_LIBRARY_PATH', default='')]),
-           'IGN_GAZEBO_SYSTEM_PLUGIN_PATH':  # TODO(CH3): To support pre-garden. Deprecated.
-                      ':'.join([os.environ.get('IGN_GAZEBO_SYSTEM_PLUGIN_PATH', default=''),
-                                os.environ.get('LD_LIBRARY_PATH', default='')])}
-    log_level = LaunchConfiguration("log_level")
-    use_sim_time = LaunchConfiguration("use_sim_time")
-    gz_verbosity = LaunchConfiguration("gz_verbosity")
     pkg_share = launch_ros.substitutions.FindPackageShare(
         package="sam_bot_nav2_gz"
     ).find("sam_bot_nav2_gz")
-    default_model_path = os.path.join(
-        pkg_share, "src/description/sam_bot_description.urdf"
-    )
     run_headless = LaunchConfiguration("run_headless")
-    world_file_name = LaunchConfiguration("world_file")
     gz_models_path = ":".join([pkg_share, os.path.join(pkg_share, "models")])
-    #gz_models_path = os.path.join(pkg_share, "models")
-    world_path = PathJoinSubstitution([pkg_share, "world", world_file_name])
-    gazebo = [
-        ExecuteProcess(
-            condition=launch.conditions.IfCondition(run_headless),
-            cmd=['ruby', FindExecutable(name="ign"), 'gazebo',  '-r', '-v', gz_verbosity, '-s', '--headless-rendering', world_path],
-            output='screen',
-            additional_env=gz_env, # type: ignore
-            shell=False,
-        ),
-        ExecuteProcess(
-            condition=launch.conditions.UnlessCondition(run_headless),
-            cmd=['ruby', FindExecutable(name="ign"), 'gazebo',  '-r', '-v', gz_verbosity, world_path],
-            output='screen',
-            additional_env=gz_env, # type: ignore
-            shell=False,
-        )
-    ]
 
-    spawn_entity = Node(
-        package="ros_gz_sim",
-        executable="create",
-        output="screen",
-        arguments=[
-            "-name",
-            "sam_bot",
-            "-topic",
-            "robot_description",
-            "-z",
-            "1.0",
-            "-x",
-            "-2.0",
-            "--ros-args",
-            "--log-level",
-            log_level,
-        ],
-        parameters=[{"use_sim_time": use_sim_time}],
-    )
 
 
     return LaunchDescription(
         [
-            SetEnvironmentVariable(
-                name="IGN_GAZEBO_RESOURCE_PATH",
-                value=gz_models_path,
-            ),
-            DeclareLaunchArgument(
-                "gz_verbosity",
-                default_value="3",
-                description="Verbosity level for Ignition Gazebo (0~4).",
-            ),
-            DeclareLaunchArgument(
-                "gz_args",
-                default_value="",
-                description="Extra args for Gazebo (ie. '-s' for running headless)",
-            ),
-            DeclareLaunchArgument(
-                name="world_file",
-                default_value="empty.sdf",
-            ),
             DeclareLaunchArgument(
                 name="run_headless",
                 default_value="False",
                 description="Start GZ in hedless mode and don't start RViz (overrides use_rviz)",
             ),
-            DeclareLaunchArgument(
-                name="use_sim_time",
-                default_value="True",
-                description="Flag to enable use_sim_time",
-            ),
-            DeclareLaunchArgument(
-                name="log_level",
-                default_value="warn",
-                description="The level of logging that is applied to all ROS 2 nodes launched by this script.",
-            ),
-            #DeclareLaunchArgument(
-            #    name="run_headless",
-            #    default_value="False",
-            #    description="Start GZ in headless mode and don't start RViz (overrides use_rviz)",
-            #),
-            #launch_navigation_stack",
-            *gazebo,
-            spawn_entity,
-            #reach_goal_proc,
+            launch_navigation_stack,
+            reach_goal_proc,
             gz_bridge,
             launch_pytest.actions.ReadyToTest()
         ]
     )
 
 
-@pytest.fixture(scope="function")
+@pytest.fixture(scope="module")
 def sim():
     """Fixture that provides access to simulation state during tests"""
     if not rclpy.ok():
@@ -241,32 +156,41 @@ def sim():
     util.stop_recording()
 
 
-#@pytest.mark.launch(fixture=launch_description)
-#def test_nav2_started(reach_goal_proc, launch_context):
-#    """Test that Nav2 starts successfully"""
-#    def validate_nav2_output(output):
-#        return output and 'Nav2 active!' in output
-#
-#    # Get the reach_goal process from the launch context
-#    process_tools.wait_for_output_sync(
-#            launch_context, reach_goal_proc, validate_nav2_output, timeout=30)
-#
+@pytest.mark.launch(fixture=launch_description)
+def test_nav2_started(reach_goal_proc, launch_context):
+    """Test that Nav2 starts successfully"""
+    def validate_nav2_output(output):
+        print(output)
+        assert 'Nav2 active' in output, "process never printed Nav2 is ready"
+
+    # Get the reach_goal process from the launch context
+    process_tools.assert_output_sync(
+            launch_context, reach_goal_proc, validate_nav2_output, timeout=40)
+
 
 @pytest.mark.launch(fixture=launch_description)  
-def test_reached_goal(reach_goal_proc,launch_context, sim):
+def test_reached_goal(reach_goal_proc, launch_context, sim):
     """Check that the navigation goal is reached"""
     def validate_goal_output(output):
-        return output and 'Goal succeeded!' in output
+        print(output)
+        assert 'Goal succeeded!' in output, "process never printed Goal succeeded!"
 
     # Get the reach_goal process from the launch context
-    process_tools.wait_for_output_sync(launch_context, reach_goal_proc, validate_goal_output, timeout=5)
+    #process_tools.wait_for_output_sync(launch_context, reach_goal_proc, validate_goal_output, timeout=15)
+    process_tools.assert_output_sync(launch_context, reach_goal_proc, validate_goal_output, timeout=45)
 
     # Additional assertion using simulation state
-    goal_coordinates = (2.0, 3.0)  # Example goal coordinates - adjust as needed
-    entity = sim.get_entity('ros_symbol')
-    cam = sim.get_entity('sky_cam/camera_link')
-    dist = entity.distance_to(cam).now()
-    assert dist > 1.0, f"Robot is not close enough to camera at {cam.pose().now().x}. Distance: {dist}"
-    assert cam.pose().now().z > 1.0
-    entity.distance_to(cam).to_csv("output/test.csv")
+    robot = sim.get_entity('sam_bot')
+
+    # Create odom frame using add_transform (uses robot's pose at time 0)
+    initial_robot_pose = robot.pose().at(0) #shall we use .first()
+    sim.add_transform("world", "odom", initial_robot_pose)
+
+    # Create goal waypoint in odom frame
+    goal = Pose(x=0.8, y=-0.5, z=0.0, frame="odom")
+
+    dist = robot.distance_to(goal).now()
+    assert dist < 1.0, f"Robot is not close enough to goal. Distance: {dist}"
+    robot.distance_to(goal).to_csv("output/test.csv")
+    robot.pose().to_csv("output/pose.csv")