WIP

fabid · fabid · commit 2d10c1672e26 · 2025-07-30T15:22:50.000+09:00
diff --git a/artefacts.yaml b/artefacts.yaml
@@ -21,6 +21,29 @@ jobs:
         - name: bringup
           ros_testfile: "src/sam_bot_nav2_gz/test/test_bringup.launch.py"
 
+  pytest:
+    type: test
+    package:
+      docker:
+        build:
+          dockerfile: ./Dockerfile
+    runtime:
+      simulator: gazebo:fortress
+      framework: null
+    timeout: 5 #minutes
+    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"
   nav2:
     type: test
     package:
@@ -39,7 +62,7 @@ jobs:
           - /distance_from_start_gt
           - /distance_from_start_est
         params:
-          launch/world: ["bookstore.sdf"]
+          launch/world: ["empty.sdf"]
           controller_server/controller_frequency: [10.0]
       settings:
         - name: reach_goal
diff --git a/src/sam_bot_nav2_gz/test/bridge.yaml b/src/sam_bot_nav2_gz/test/bridge.yaml
@@ -3,3 +3,8 @@
   ros_type_name: "geometry_msgs/msg/PoseArray"
   gz_type_name: "ignition.msgs.Pose_V"
   direction: GZ_TO_ROS 
+- ros_topic_name: "/world/worldWrapper/pose"
+  gz_topic_name: "/world/worldWrapper/pose"
+  ros_type_name: "geometry_msgs/msg/TransformStamped"
+  gz_type_name: "ignition.msgs.Pose"
+  direction: GZ_TO_ROS
diff --git a/src/sam_bot_nav2_gz/test/simulation_state_util.py b/src/sam_bot_nav2_gz/test/simulation_state_util.py
@@ -0,0 +1,90 @@
+import rclpy
+from rclpy.node import Node
+from rclpy.callback_groups import ReentrantCallbackGroup
+from geometry_msgs.msg import TransformStamped
+import math
+import threading
+import time
+from typing import Tuple
+
+
+class RobotState:
+    """Represents the current state of the robot in simulation"""
+    
+    def __init__(self):
+        self.position = (0.0, 0.0)
+        self._lock = threading.Lock()
+    
+    def update_transform(self, transform_msg: TransformStamped):
+        """Update robot state from transform message"""
+        with self._lock:
+            self.position = (
+                transform_msg.transform.translation.x,
+                transform_msg.transform.translation.y
+            )
+    
+    def distance_to(self, coordinates: Tuple[float, float]) -> float:
+        """Calculate distance from current position to target coordinates"""
+        with self._lock:
+            dx = self.position[0] - coordinates[0]
+            dy = self.position[1] - coordinates[1]
+            return math.sqrt(dx**2 + dy**2)
+
+
+class SimulationStateUtil(Node):
+    """Utility class for accessing simulation state in tests"""
+    
+    def __init__(self):
+        super().__init__("simulation_state_util")
+        self.get_logger().info("Simulation State Util Started")
+        
+        # Create callback group for parallel execution
+        self.callback_group = ReentrantCallbackGroup()
+        
+        # Robot state
+        self.robot = RobotState()
+        
+        # Data received flag
+        self._transform_received = False
+        
+        # Subscriber
+        self._setup_subscriptions()
+    
+    def _setup_subscriptions(self):
+        """Set up ROS2 subscription for ground truth transform"""
+        self.transform_sub = self.create_subscription(
+            TransformStamped, 
+            "/world/worldWrapper/pose", 
+            self._transform_callback, 
+            10, 
+            callback_group=self.callback_group
+        )
+    
+    def _transform_callback(self, msg: TransformStamped):
+        """Handle transform messages"""
+        self.robot.update_transform(msg)
+        self._transform_received = True
+    
+    def wait_for_initial_state(self, timeout: float = 10.0) -> bool:
+        """Wait for initial data from transform subscription"""
+        start_time = time.time()
+        while time.time() - start_time < timeout:
+            if self._transform_received:
+                self.get_logger().info("Initial simulation state received")
+                return True
+            time.sleep(0.1)
+        
+        self.get_logger().warn("Timeout waiting for initial simulation state")
+        return False
+    
+    def get(self, entity: str):
+        """Get simulation entity by name"""
+        if entity == "robot":
+            return self.robot
+        else:
+            raise ValueError(f"Unknown entity: {entity}")
+    
+    def shutdown_util(self):
+        """Clean shutdown of the utility"""
+        self.get_logger().info("Shutting down simulation state util")
+        self.destroy_node()
diff --git a/src/sam_bot_nav2_gz/test/test_reach_goal_pytest.py b/src/sam_bot_nav2_gz/test/test_reach_goal_pytest.py
@@ -0,0 +1,185 @@
+import os
+import pytest
+import rclpy
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription, ExecuteProcess, DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import Node
+import launch_pytest
+from launch_pytest.tools import process as process_tools
+from artefacts_toolkit.rosbag import rosbag, image_topics
+from artefacts_toolkit.chart import make_chart
+from artefacts_toolkit.config import get_artefacts_param
+import sys
+import os
+sys.path.append(os.path.dirname(__file__))
+from simulation_state_util import SimulationStateUtil
+
+
+ARTEFACTS_PARAMS_FILE = os.environ.get(
+    "ARTEFACTS_SCENARIO_PARAMS_FILE", "scenario_params.yaml"
+)
+
+def deep_merge_dicts(source, override):
+    """Recursively merge two dictionaries, with values from `override` taking precedence over `source`"""
+    for key, value in override.items():
+        if isinstance(value, dict) and key in source:
+            source[key] = deep_merge_dicts(source[key], value)
+        else:
+            source[key] = value
+    return source
+
+def merge_ros_params_files(source, override, destination):
+    """Merge two ROS2 yaml parameter files into one, overriding the values in the first one with the values in `override`"""
+    import yaml
+
+    with open(source, "r") as f:
+        source_params = yaml.safe_load(f)
+
+    with open(override, "r") as f:
+        override_params = yaml.safe_load(f)
+
+    merged_params = deep_merge_dicts(source_params, override_params)
+    with open(destination, "w") as f:
+        yaml.dump(merged_params, f)
+
+@pytest.fixture()
+def reach_goal_proc():
+    reach_goal = Node(
+        package="sam_bot_nav2_gz",
+        executable="reach_goal.py",
+        output="screen",
+        cached_output=True,
+    )
+    return reach_goal
+
+
+@launch_pytest.fixture
+def launch_description(reach_goal_proc):
+    """Launch description fixture for pytest-based testing"""
+    try:
+        world = get_artefacts_param("launch", "world", default="empty.sdf")
+    except FileNotFoundError:
+        world = "empty.sdf"
+
+    run_headless = LaunchConfiguration("run_headless")
+    source_params_file = "src/sam_bot_nav2_gz/config/nav2_params.yaml"
+    new_params_file = "all_params.yaml"
+    try:
+        merge_ros_params_files(source_params_file, ARTEFACTS_PARAMS_FILE, new_params_file)
+    except FileNotFoundError:
+        pass
+    
+    launch_navigation_stack = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            [
+                os.path.join(
+                    get_package_share_directory("sam_bot_nav2_gz"),
+                    "launch",
+                    "complete_navigation.launch.py"
+                ),
+            ]
+        ),
+        launch_arguments=[
+            ("run_headless", run_headless),
+            ("world_file", world),
+            ("params_file", new_params_file),
+            ],
+    )
+
+ 
+    topics = ["/odom"]
+    metrics = ["/distance_from_start_gt", "/distance_from_start_est", "/odometry_error"]
+    camera_topics = ["/sky_cam"]
+    sim_topics = ["/world/dynamic_pose/info"]
+    bag_recorder, rosbag_filepath = rosbag.get_bag_recorder(
+            topics + sim_topics + metrics + camera_topics, use_sim_time=True
+        )
+
+    # Gazebo ros bridge
+    gz_bridge = Node(
+        package="ros_gz_bridge",
+        executable="parameter_bridge",
+        parameters=[{
+            "config_file": os.path.join(
+                "src",
+                "sam_bot_nav2_gz",
+                "test",
+                 "bridge.yaml"
+                )}],
+        output="screen",
+        )
+
+    test_odometry_node = ExecuteProcess(
+        cmd=[
+            "python3",
+            os.path.join(
+                "src",
+                "sam_bot_nav2_gz",
+                "test",
+                "test_odometry_node.py"
+            ),
+        ]
+    )
+
+    return LaunchDescription(
+        [
+            DeclareLaunchArgument(
+                name="run_headless",
+                default_value="False",
+                description="Start GZ in headless mode and don't start RViz (overrides use_rviz)",
+            ),
+            launch_navigation_stack,
+            reach_goal_proc,
+            test_odometry_node,
+            gz_bridge,
+            #bag_recorder,
+        ]
+    )
+
+
+@pytest.fixture(scope="function")
+def simulation_state_util():
+    """Fixture that provides access to simulation state during tests"""
+    if not rclpy.ok():
+        rclpy.init()
+    util = SimulationStateUtil()
+    
+    # Wait for initial state
+    util.wait_for_initial_state(timeout=15.0)
+    
+    yield util
+    
+    # Cleanup
+    util.shutdown_util()
+
+
+@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_reached_goal(reach_goal_proc,launch_context, simulation_state_util):
+    """Check that the navigation goal is reached"""
+    def validate_goal_output(output):
+        return output and 'Goal succeeded!' in output
+
+    # Get the reach_goal process from the launch context
+    process_tools.wait_for_output_sync(
+        launch_context, reach_goal_proc, validate_goal_output, timeout=40)
+    
+    # Additional assertion using simulation state
+    goal_coordinates = (2.0, 3.0)  # Example goal coordinates - adjust as needed
+    distance = simulation_state_util.get("robot").distance_to(goal_coordinates)
+    assert distance < 1.0, f"Robot is not close enough to goal at {goal_coordinates}. Distance: {distance}"
+
+
diff --git a/src/sam_bot_nav2_gz/world/empty.sdf b/src/sam_bot_nav2_gz/world/empty.sdf
@@ -21,7 +21,14 @@
       <ambient>1.0 1.0 1.0</ambient>
       <background>0.8 0.8 0.8</background>
     </scene>
-
+ <model name="worldWrapper">
+<plugin
+        filename="gz-sim-pose-publisher-system"
+        name="gz::sim::systems::PosePublisher">
+        <use_pose_vector_msg>false</use_pose_vector_msg>
+        <publish_nested_model_pose>true</publish_nested_model_pose>
+        <static_publisher>false</static_publisher>
+      </plugin>
     <light type="directional" name="sun">
       <cast_shadows>true</cast_shadows>
       <pose>0 0 10 0 0 0</pose>
@@ -93,5 +100,6 @@
         <pose>0.0 0.0 0.0 0 0 0</pose>
     </include>
 
+  </model>
   </world>
-</sdf>
+</sdf>
