Move controller test to bringup and reduce test time

README.md

@@ -88,25 +88,6 @@ ros2 launch rosbot_bringup combined.launch.py
 
 ### Build and run Gazebo simulation
 
-Prerequisites:
-
-> [!TIP]
-> The default version of Gazebo Ignition will be installed with the instructions below. If you want to install a different version of the simulator, it is necessary to:
->
-> - Check compatible versions of ROS 2 and Gazebo in [this table](https://gazebosim.org/docs/garden/ros_installation#summary-of-compatible-ros-and-gazebo-combinations)
-> - [Install the appropriate version](https://gazebosim.org/docs/fortress/install_ubuntu#binary-installation-on-ubuntu),
-> - Add the `GZ_VERSION` environment variable appropriate to your version
->
->   ```bash
->   export GZ_VERSION=fortress
->   ```
-
-If you have installed multiple versions of Gazebo use the global variable to select the correct one:
-
-```bash
-export GZ_VERSION=fortress
-```
-
 Building:
 
 ```bash

rosbot/rosbot_hardware.repos

@@ -10,7 +10,7 @@ repositories:
   husarion_controllers:
     type: git
     url: https://github.com/husarion/husarion_controllers
-    version: main
+    version: 217b09830f5f42930098b9992eda41710702b625
   micro_ros_msgs:
     type: git
     url: https://github.com/micro-ROS/micro_ros_msgs.git

rosbot_bringup/test/test_diff_drive_ekf.py

@@ -22,7 +22,7 @@
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import PathJoinSubstitution
 from launch_ros.substitutions import FindPackageShare
-from test_utils import BringupTestNode
+from test_utils import BringupTestNode, readings_data_test
 
 
 @launch_pytest.fixture
@@ -56,10 +56,7 @@ def test_bringup_startup_success():
         node.start_publishing_fake_hardware()
 
         node.start_node_thread()
-        msgs_received_flag = node.odom_msg_event.wait(timeout=10.0)
-        assert (
-            msgs_received_flag
-        ), "Expected odometry/filtered message but it was not received. Check robot_localization!"
+        readings_data_test(node)
 
     finally:
         rclpy.shutdown()

rosbot_bringup/test/test_mecanum_ekf.py

@@ -22,7 +22,7 @@
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import PathJoinSubstitution
 from launch_ros.substitutions import FindPackageShare
-from test_utils import BringupTestNode
+from test_utils import BringupTestNode, readings_data_test
 
 
 @launch_pytest.fixture
@@ -56,10 +56,7 @@ def test_bringup_startup_success():
         node.start_publishing_fake_hardware()
 
         node.start_node_thread()
-        msgs_received_flag = node.odom_msg_event.wait(timeout=10.0)
-        assert (
-            msgs_received_flag
-        ), "Expected odometry/filtered message but it was not received. Check robot_localization!"
+        readings_data_test(node)
 
     finally:
         rclpy.shutdown()

rosbot_bringup/test/test_multirobot_ekf.py

@@ -22,7 +22,7 @@
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import PathJoinSubstitution
 from launch_ros.substitutions import FindPackageShare
-from test_utils import BringupTestNode
+from test_utils import BringupTestNode, readings_data_test
 
 robot_names = ["robot1", "robot2"]
 
@@ -57,16 +57,15 @@ def generate_test_description():
 @pytest.mark.launch(fixture=generate_test_description)
 def test_multirobot_bringup_startup_success():
     rclpy.init()
-    for robot_name in robot_names:
-        node = BringupTestNode("test_bringup", namespace=robot_name)
-        node.create_test_subscribers_and_publishers()
-        node.start_publishing_fake_hardware()
+    try:
+        for robot_name in robot_names:
+            node = BringupTestNode("test_bringup", namespace=robot_name)
+            node.create_test_subscribers_and_publishers()
+            node.start_publishing_fake_hardware()
+
+            node.start_node_thread()
+            readings_data_test(node)
 
-        node.start_node_thread()
-        msgs_received_flag = node.odom_msg_event.wait(timeout=20.0)
-        assert msgs_received_flag, (
-            f"Expected {robot_name}/odometry/filtered message but it was not received. "
-            "Check robot_localization!"
-        )
 
-    rclpy.shutdown()
+    finally:
+        rclpy.shutdown()

rosbot_bringup/test/test_namespaced_diff_drive_ekf.py

@@ -22,7 +22,7 @@
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import PathJoinSubstitution
 from launch_ros.substitutions import FindPackageShare
-from test_utils import BringupTestNode
+from test_utils import BringupTestNode, readings_data_test
 
 
 @launch_pytest.fixture
@@ -57,10 +57,7 @@ def test_namespaced_bringup_startup_success():
         node.start_publishing_fake_hardware()
 
         node.start_node_thread()
-        msgs_received_flag = node.odom_msg_event.wait(timeout=10.0)
-        assert (
-            msgs_received_flag
-        ), "Expected odometry/filtered message but it was not received. Check robot_localization!"
+        readings_data_test(node)
 
     finally:
         rclpy.shutdown()

rosbot_bringup/test/test_namespaced_mecanum_ekf.py

@@ -22,7 +22,7 @@
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import PathJoinSubstitution
 from launch_ros.substitutions import FindPackageShare
-from test_utils import BringupTestNode
+from test_utils import BringupTestNode, readings_data_test
 
 
 @launch_pytest.fixture
@@ -57,10 +57,7 @@ def test_namespaced_bringup_startup_success():
         node.start_publishing_fake_hardware()
 
         node.start_node_thread()
-        msgs_received_flag = node.odom_msg_event.wait(timeout=10.0)
-        assert (
-            msgs_received_flag
-        ), "Expected odometry/filtered message but it was not received. Check robot_localization!"
+        readings_data_test(node)
 
     finally:
         rclpy.shutdown()

rosbot_bringup/test/test_utils.py

@@ -13,13 +13,13 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
-from threading import Event, Thread
-
 import rclpy
+import time
+
 from nav_msgs.msg import Odometry
 from rclpy.node import Node
 from sensor_msgs.msg import Imu, JointState
-
+from threading import Event, Thread
 
 class BringupTestNode(Node):
     ROSBOT_HARDWARE_PUBLISHERS_RATE = 10.0
@@ -28,33 +28,55 @@ class BringupTestNode(Node):
 
     def __init__(self, name="test_node", namespace=None):
         super().__init__(name, namespace=namespace)
-        self.odom_msg_event = Event()
+        self.joint_state_msg_event = Event()
+        self.controller_odom_msg_event = Event()
+        self.imu_msg_event = Event()
+        self.ekf_odom_msg_event = Event()
 
-    def create_test_subscribers_and_publishers(self):
-        self.imu_publisher = self.create_publisher(Imu, "/_imu/data_raw", 10)
+        self.ros_spin_thread = None
+        self.timer = None
 
-        self.joint_states_publisher = self.create_publisher(JointState, "/_motors_response", 10)
+    def create_test_subscribers_and_publishers(self):
+        self.imu_pub = self.create_publisher(Imu, "/_imu/data_raw", 10)
+        self.joint_pub = self.create_publisher(JointState, "/_motors_response", 10)
 
-        self.odom_sub = self.create_subscription(
-            Odometry, "odometry/filtered", self.odometry_callback, 10
+        self.joint_state_sub = self.create_subscription(
+            JointState, "joint_states", self.joint_states_callback, 10
+        )
+        self.controller_odom_sub = self.create_subscription(
+            Odometry, "rosbot_base_controller/odom", self.controller_odometry_callback, 10
+        )
+        self.imu_sub = self.create_subscription(Imu, "imu_broadcaster/imu", self.imu_callback, 10)
+        self.ekf_odom_sub = self.create_subscription(
+            Odometry, "odometry/filtered", self.ekf_odometry_callback, 10
         )
-        self.timer = None
 
     def start_node_thread(self):
-        self.ros_spin_thread = Thread(target=lambda node: rclpy.spin(node), args=(self,))
-        self.ros_spin_thread.start()
+        if not self.ros_spin_thread:
+            self.ros_spin_thread = Thread(target=rclpy.spin, args=(self,), daemon=True)
+            self.ros_spin_thread.start()
 
     def start_publishing_fake_hardware(self):
-        self.timer = self.create_timer(
-            1.0 / self.ROSBOT_HARDWARE_PUBLISHERS_RATE,
-            self.timer_callback,
-        )
+        if not self.timer:
+            self.timer = self.create_timer(
+                1.0 / self.ROSBOT_HARDWARE_PUBLISHERS_RATE,
+                self.timer_callback,
+            )
 
     def timer_callback(self):
         self.publish_fake_hardware_messages()
 
-    def odometry_callback(self, data):
-        self.odom_msg_event.set()
+    def joint_states_callback(self, data):
+        self.joint_state_msg_event.set()
+
+    def controller_odometry_callback(self, data):
+        self.controller_odom_msg_event.set()
+
+    def imu_callback(self, data):
+        self.imu_msg_event.set()
+
+    def ekf_odometry_callback(self, data):
+        self.ekf_odom_msg_event.set()
 
     def publish_fake_hardware_messages(self):
         imu_msg = Imu()
@@ -72,5 +94,50 @@ def publish_fake_hardware_messages(self):
         joint_state_msg.position = [0.0, 0.0, 0.0, 0.0]
         joint_state_msg.velocity = [0.0, 0.0, 0.0, 0.0]
 
-        self.imu_publisher.publish(imu_msg)
-        self.joint_states_publisher.publish(joint_state_msg)
+        self.imu_pub.publish(imu_msg)
+        self.joint_pub.publish(joint_state_msg)
+
+
+def wait_for_all_events(events, timeout):
+    """
+    Wait for all specified events to be set within a given timeout.
+    :param events: List of Event objects to wait for.
+    :param timeout: Maximum time (in seconds) to wait.
+    :return: (bool, list) where the first value indicates success, and the second is a list of indices of events that were not set.
+    """
+    start_time = time.time()
+    while time.time() - start_time < timeout:
+        if all(event.is_set() for event in events):
+            return True, []  # All events have been set
+        time.sleep(0.1)  # Short interval between checks
+
+    # Identify which events were not set
+    not_set_events = [i for i, event in enumerate(events) if not event.is_set()]
+    return False, not_set_events
+
+
+def readings_data_test(node, robot_name="ROSbot"):
+    events = [
+        node.joint_state_msg_event,
+        node.controller_odom_msg_event,
+        node.imu_msg_event,
+        node.ekf_odom_msg_event,
+    ]
+
+    event_names = [
+        "JointStates",
+        "Controller Odometry",
+        "IMU",
+        "EKF Odometry",
+    ]
+
+    msgs_received_flag, not_set_indices = wait_for_all_events(events, timeout=30.0)
+
+    if not msgs_received_flag:
+        not_set_event_names = [event_names[i] for i in not_set_indices]
+        missing_events = ", ".join(not_set_event_names)
+        raise AssertionError(
+            f"{robot_name}: Not all expected messages were received. Missing: {missing_events}."
+        )
+
+    print(f"{robot_name}: All messages received successfully.")