Added tests for ranges and camera

delihus · delihus · commit f00b6d9b4dbb · 2024-01-10T16:10:05.000+01:00
Signed-off-by: Jakub Delicat &lt;jakub.delicat@husarion.com&gt;
diff --git a/rosbot_description/urdf/components/vl53lox.urdf.xacro b/rosbot_description/urdf/components/vl53lox.urdf.xacro
@@ -5,11 +5,9 @@
 
     <xacro:if value="${namespace == 'None'}">
       <xacro:property name="namespace_ext" value="" />
-      <xacro:property name="gz_control_namespace" value="/" />
     </xacro:if>
     <xacro:unless value="${namespace == 'None'}">
         <xacro:property name="namespace_ext" value="${namespace}/" />
-        <xacro:property name="gz_control_namespace" value="${namespace}" />
     </xacro:unless>
 
     <joint name="${parent}_${prefix}_range_joint" type="fixed">
@@ -26,7 +24,7 @@
       <gazebo reference="${prefix}_range">
         <sensor name="${namespace_ext}${prefix}_range" type='gpu_lidar'>
 
-          <topic>${gz_control_namespace}range/${prefix}</topic>
+          <topic>${namespace_ext}range/${prefix}</topic>
           <frame_id>${prefix}_range</frame_id>
           <ignition_frame_id>${prefix}_range</ignition_frame_id>
 
@@ -55,12 +53,9 @@
           <visualize>false</visualize>
 
           <ros>
-            <namespace></namespace>
+            <namespace>${namespace_ext}</namespace>
           </ros>
         </sensor>
-        <plugin filename="libignition-gazebo-sensors-system.so"  name="ignition::gazebo::systems::Sensors">
-          <render_engine>ogre2</render_engine>
-        </plugin>
       </gazebo>
     </xacro:if>
   </xacro:macro>
diff --git a/rosbot_description/urdf/rosbot.urdf.xacro b/rosbot_description/urdf/rosbot.urdf.xacro
@@ -16,20 +16,20 @@
     namespace="$(arg namespace)" />
 
     <xacro:include filename="$(find ros_components_description)/urdf/slamtec_rplidar_a2.urdf.xacro" ns="lidar" />
-      <xacro:lidar.slamtec_rplidar_a2
-        parent_link="cover_link"
-        xyz="0.02 0.0 0.0"
-        rpy="0.0 0.0 0.0"
-        use_gpu="$(arg use_gpu)"
-        namespace="$(arg namespace)"
-        simulation_engine="$(arg simulation_engine)" />
+    <xacro:lidar.slamtec_rplidar_a2
+      parent_link="cover_link"
+      xyz="0.02 0.0 0.0"
+      rpy="0.0 0.0 0.0"
+      use_gpu="$(arg use_gpu)"
+      namespace="$(arg namespace)"
+      simulation_engine="$(arg simulation_engine)" />
 
-      <xacro:include filename="$(find ros_components_description)/urdf/orbbec_astra.urdf.xacro" ns="camera" />
-      <xacro:camera.orbbec_astra
-        parent_link="camera_link"
-        xyz="0.0 0.0 0.0"
-        rpy="0.0 0.0 0.0"
-        namespace="$(arg namespace)"
-        simulation_engine="$(arg simulation_engine)" />
+    <xacro:include filename="$(find ros_components_description)/urdf/orbbec_astra.urdf.xacro" ns="camera" />
+    <xacro:camera.orbbec_astra
+      parent_link="camera_link"
+      xyz="0.0 0.0 0.0"
+      rpy="0.0 0.0 0.0"
+      namespace="$(arg namespace)"
+      simulation_engine="$(arg simulation_engine)" />
 
 </robot>
diff --git a/rosbot_gazebo/launch/spawn.launch.py b/rosbot_gazebo/launch/spawn.launch.py
@@ -17,7 +17,11 @@
     IncludeLaunchDescription,
     DeclareLaunchArgument,
 )
-from launch.substitutions import PathJoinSubstitution, LaunchConfiguration, PythonExpression
+from launch.substitutions import (
+    PathJoinSubstitution,
+    LaunchConfiguration,
+    PythonExpression,
+)
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from nav2_common.launch import ReplaceString
 from launch_ros.actions import Node, SetParameter
diff --git a/rosbot_gazebo/test/test_diff_drive_simulation.py b/rosbot_gazebo/test/test_diff_drive_simulation.py
@@ -105,11 +105,25 @@ def test_diff_drive_simulation():
         assert (
             node.controller_odom_flag
         ), "ROSbot does not rotate properly. Check rosbot_base_controller!"
-        assert node.ekf_odom_flag, "ROSbot does not rotate properly. Check ekf_filter_node!"
+        assert (
+            node.ekf_odom_flag
+        ), "ROSbot does not rotate properly. Check ekf_filter_node!"
 
         flag = node.scan_event.wait(timeout=20.0)
         assert flag, "ROSbot's lidar does not work properly!"
 
+        for i in range(len(node.RANGE_SENSORS_TOPICS)):
+            flag = node.ranges_events[i].wait(timeout=20.0)
+            assert (
+                flag
+            ), f"ROSbot's range sensor {node.RANGE_SENSORS_TOPICS[i]} does not work properly!"
+
+        flag = node.camera_color_event.wait(timeout=20.0)
+        assert flag, "ROSbot's camera color image does not work properly!"
+
+        flag = node.camera_points_event.wait(timeout=20.0)
+        assert flag, "ROSbot's camera point cloud does not work properly!"
+
     finally:
         # The pytest cannot kill properly the Gazebo Ignition's tasks what blocks launching
         # several tests in a row.
diff --git a/rosbot_gazebo/test/test_flake8.py b/rosbot_gazebo/test/test_flake8.py
@@ -20,4 +20,6 @@
 @pytest.mark.linter
 def test_flake8():
     rc, errors = main_with_errors(argv=[])
-    assert rc == 0, "Found %d code style errors / warnings:\n" % len(errors) + "\n".join(errors)
+    assert rc == 0, "Found %d code style errors / warnings:\n" % len(
+        errors
+    ) + "\n".join(errors)
diff --git a/rosbot_gazebo/test/test_mecanum_simulation.py b/rosbot_gazebo/test/test_mecanum_simulation.py
@@ -118,11 +118,25 @@ def test_mecanum_simulation():
         assert (
             node.controller_odom_flag
         ), "ROSbot does not rotate properly. Check rosbot_base_controller!"
-        assert node.ekf_odom_flag, "ROSbot does not rotate properly. Check ekf_filter_node!"
+        assert (
+            node.ekf_odom_flag
+        ), "ROSbot does not rotate properly. Check ekf_filter_node!"
 
         flag = node.scan_event.wait(timeout=20.0)
         assert flag, "ROSbot's lidar does not work properly!"
 
+        for i in range(len(node.RANGE_SENSORS_TOPICS)):
+            flag = node.ranges_events[i].wait(timeout=20.0)
+            assert (
+                flag
+            ), f"ROSbot's range sensor {node.RANGE_SENSORS_TOPICS[i]} does not work properly!"
+
+        flag = node.camera_color_event.wait(timeout=20.0)
+        assert flag, "ROSbot's camera color image does not work properly!"
+
+        flag = node.camera_points_event.wait(timeout=20.0)
+        assert flag, "ROSbot's camera point cloud does not work properly!"
+
     finally:
         # The pytest cannot kill properly the Gazebo Ignition's tasks what blocks launching
         # several tests in a row.
diff --git a/rosbot_gazebo/test/test_multirobot_diff_drive_simulation.py b/rosbot_gazebo/test/test_multirobot_diff_drive_simulation.py
@@ -78,7 +78,9 @@ def test_multirobot_simulation():
             nodes.append(node)
             executor.add_node(node)
 
-        ros_spin_thread = Thread(target=lambda executor: executor.spin(), args=(executor,))
+        ros_spin_thread = Thread(
+            target=lambda executor: executor.spin(), args=(executor,)
+        )
         ros_spin_thread.start()
 
         for node in nodes:
@@ -116,6 +118,18 @@ def test_multirobot_simulation():
             flag = node.scan_event.wait(timeout=20.0)
             assert flag, f"{robot_name}'s lidar does not work properly!"
 
+            for i in range(len(node.RANGE_SENSORS_TOPICS)):
+                flag = node.ranges_events[i].wait(timeout=20.0)
+                assert (
+                    flag
+                ), f"ROSbot's range sensor {node.RANGE_SENSORS_TOPICS[i]} does not work properly!"
+
+            flag = node.camera_color_event.wait(timeout=20.0)
+            assert flag, "ROSbot's camera color image does not work properly!"
+
+            flag = node.camera_points_event.wait(timeout=20.0)
+            assert flag, "ROSbot's camera point cloud does not work properly!"
+
             node.destroy_node()
 
     finally:
diff --git a/rosbot_gazebo/test/test_multirobot_mecanum_simulation.py b/rosbot_gazebo/test/test_multirobot_mecanum_simulation.py
@@ -81,7 +81,9 @@ def test_multirobot_mecanum_simulation():
             nodes.append(node)
             executor.add_node(node)
 
-        ros_spin_thread = Thread(target=lambda executor: executor.spin(), args=(executor,))
+        ros_spin_thread = Thread(
+            target=lambda executor: executor.spin(), args=(executor,)
+        )
         ros_spin_thread.start()
 
         for node in nodes:
@@ -136,6 +138,18 @@ def test_multirobot_mecanum_simulation():
             flag = node.scan_event.wait(timeout=20.0)
             assert flag, f"{robot_name}'s lidar does not work properly!"
 
+            for i in range(len(node.RANGE_SENSORS_TOPICS)):
+                flag = node.ranges_events[i].wait(timeout=20.0)
+                assert (
+                    flag
+                ), f"ROSbot's range sensor {node.RANGE_SENSORS_TOPICS[i]} does not work properly!"
+
+            flag = node.camera_color_event.wait(timeout=20.0)
+            assert flag, "ROSbot's camera color image does not work properly!"
+
+            flag = node.camera_points_event.wait(timeout=20.0)
+            assert flag, "ROSbot's camera point cloud does not work properly!"
+
             node.destroy_node()
 
     finally:
diff --git a/rosbot_gazebo/test/test_namespaced_diff_drive_simulation.py b/rosbot_gazebo/test/test_namespaced_diff_drive_simulation.py
@@ -101,11 +101,25 @@ def test_namespaced_diff_drive_simulation():
         assert (
             node.controller_odom_flag
         ), "ROSbot does not rotate properly. Check rosbot_base_controller!"
-        assert node.ekf_odom_flag, "ROSbot does not rotate properly. Check ekf_filter_node!"
+        assert (
+            node.ekf_odom_flag
+        ), "ROSbot does not rotate properly. Check ekf_filter_node!"
 
         flag = node.scan_event.wait(timeout=20.0)
         assert flag, "ROSbot's lidar does not work properly!"
 
+        for i in range(len(node.RANGE_SENSORS_TOPICS)):
+            flag = node.ranges_events[i].wait(timeout=20.0)
+            assert (
+                flag
+            ), f"ROSbot's range sensor {node.RANGE_SENSORS_TOPICS[i]} does not work properly!"
+
+        flag = node.camera_color_event.wait(timeout=20.0)
+        assert flag, "ROSbot's camera color image does not work properly!"
+
+        flag = node.camera_points_event.wait(timeout=20.0)
+        assert flag, "ROSbot's camera point cloud does not work properly!"
+
     finally:
         # The pytest cannot kill properly the Gazebo Ignition's tasks what blocks launching
         # several tests in a row.
diff --git a/rosbot_gazebo/test/test_namespaced_mecanum_simulation.py b/rosbot_gazebo/test/test_namespaced_mecanum_simulation.py
@@ -114,11 +114,25 @@ def test_namespaced_mecanum_simulation():
         assert (
             node.controller_odom_flag
         ), "ROSbot does not rotate properly. Check rosbot_base_controller!"
-        assert node.ekf_odom_flag, "ROSbot does not rotate properly. Check ekf_filter_node!"
+        assert (
+            node.ekf_odom_flag
+        ), "ROSbot does not rotate properly. Check ekf_filter_node!"
 
         flag = node.scan_event.wait(timeout=20.0)
         assert flag, "ROSbot's lidar does not work properly!"
 
+        for i in range(len(node.RANGE_SENSORS_TOPICS)):
+            flag = node.ranges_events[i].wait(timeout=20.0)
+            assert (
+                flag
+            ), f"ROSbot's range sensor {node.RANGE_SENSORS_TOPICS[i]} does not work properly!"
+
+        flag = node.camera_color_event.wait(timeout=20.0)
+        assert flag, "ROSbot's camera color image does not work properly!"
+
+        flag = node.camera_points_event.wait(timeout=20.0)
+        assert flag, "ROSbot's camera point cloud does not work properly!"
+
     finally:
         # The pytest cannot kill properly the Gazebo Ignition's tasks what blocks launching
         # several tests in a row.
diff --git a/rosbot_gazebo/test/test_utils.py b/rosbot_gazebo/test/test_utils.py
@@ -22,7 +22,7 @@
 
 from geometry_msgs.msg import Twist
 from nav_msgs.msg import Odometry
-from sensor_msgs.msg import LaserScan
+from sensor_msgs.msg import LaserScan, Image, PointCloud2
 
 
 class SimulationTestNode(Node):
@@ -31,11 +31,16 @@ class SimulationTestNode(Node):
     # cause the rosbot_base_controller to determine inaccurate odometry.
     ACCURACY = 0.10  # 10% accuracy
 
+    RANGE_SENSORS_TOPICS = ["range/fl", "range/fr", "range/rl", "range/rr"]
+    RANGE_SENSORS_FRAMES = ["fl_range", "fr_range", "rl_range", "rr_range"]
+
     def __init__(self, name="test_node", namespace=None):
         super().__init__(name, namespace=namespace)
 
         # Use simulation time to correct run on slow machine
-        use_sim_time = rclpy.parameter.Parameter("use_sim_time", rclpy.Parameter.Type.BOOL, True)
+        use_sim_time = rclpy.parameter.Parameter(
+            "use_sim_time", rclpy.Parameter.Type.BOOL, True
+        )
         self.set_parameters([use_sim_time])
 
         self.VELOCITY_STABILIZATION_DELAY = 3
@@ -51,6 +56,9 @@ def __init__(self, name="test_node", namespace=None):
         self.ekf_odom_flag = False
         self.odom_tf_event = Event()
         self.scan_event = Event()
+        self.ranges_events = [Event() for _ in range(len(self.RANGE_SENSORS_TOPICS))]
+        self.camera_color_event = Event()
+        self.camera_points_event = Event()
         self.ros_node_spin_event = Event()
 
     def clear_odom_flag(self):
@@ -76,16 +84,37 @@ def create_test_subscribers_and_publishers(self):
             Odometry, "odometry/filtered", self.ekf_callback, 10
         )
 
-        self.scan_sub = self.create_subscription(LaserScan, "scan", self.scan_callback, 10)
+        self.scan_sub = self.create_subscription(
+            LaserScan, "scan", self.scan_callback, 10
+        )
+
+        self.range_subs = []
+        for range_topic_name in self.RANGE_SENSORS_TOPICS:
+            sub = self.create_subscription(
+                LaserScan, range_topic_name, self.ranges_callback, 10
+            )
+            self.range_subs.append(sub)
+
+        self.camera_color_sub = self.create_subscription(
+            Image, "camera/image", self.camera_image_callback, 10
+        )
+
+        self.camera_points_sub = self.create_subscription(
+            PointCloud2, "camera/points", self.camera_points_callback, 10
+        )
 
         self.timer = self.create_timer(1.0 / 10.0, self.timer_callback)
 
     def start_node_thread(self):
-        self.ros_spin_thread = Thread(target=lambda node: rclpy.spin(node), args=(self,))
+        self.ros_spin_thread = Thread(
+            target=lambda node: rclpy.spin(node), args=(self,)
+        )
         self.ros_spin_thread.start()
 
     def is_twist_ok(self, twist: Twist):
-        def are_close_to_each_other(current_value, dest_value, tolerance=self.ACCURACY, eps=0.01):
+        def are_close_to_each_other(
+            current_value, dest_value, tolerance=self.ACCURACY, eps=0.01
+        ):
             acceptable_range = dest_value * tolerance
             return abs(current_value - dest_value) <= acceptable_range + eps
 
@@ -109,14 +138,30 @@ def timer_callback(self):
         self.publish_cmd_vel_messages()
 
         self.current_time = 1e-9 * self.get_clock().now().nanoseconds
-        if self.current_time > self.goal_received_time + self.VELOCITY_STABILIZATION_DELAY:
+        if (
+            self.current_time
+            > self.goal_received_time + self.VELOCITY_STABILIZATION_DELAY
+        ):
             self.vel_stabilization_time_event.set()
 
     def scan_callback(self, data: LaserScan):
         self.get_logger().debug(f"Received scan length: {len(data.ranges)}")
         if data.ranges:
             self.scan_event.set()
 
+    def ranges_callback(self, data: LaserScan):
+        index = self.RANGE_SENSORS_FRAMES.index(data.header.frame_id)
+        if len(data.ranges) == 1:
+            self.ranges_events[index].set()
+
+    def camera_image_callback(self, data: Image):
+        if data.data:
+            self.camera_color_event.set()
+
+    def camera_points_callback(self, data: PointCloud2):
+        if data.data:
+            self.camera_points_event.set()
+
     def publish_cmd_vel_messages(self):
         twist_msg = Twist()
 
