From 186d98c54b417f76564534c070fb91e2303420ab Mon Sep 17 00:00:00 2001
From: Jakub Delicat <109142865+delihus@users.noreply.github.com>
Date: Mon, 16 Oct 2023 07:59:52 +0200
Subject: [PATCH] Test gazebo scan (#71)

* added scan test

Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com>

* updated gazebo scan test

Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com>

* Added tests to readme

Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com>

* Update rosbot_gazebo/test/simulation_test_node.py

Co-authored-by: rafal-gorecki <126687345+rafal-gorecki@users.noreply.github.com>

* Update README.md

Co-authored-by: rafal-gorecki <126687345+rafal-gorecki@users.noreply.github.com>

---------

Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com>
Co-authored-by: rafal-gorecki <126687345+rafal-gorecki@users.noreply.github.com>
---
 README.md                                     | 21 +++++++++++++++++++
 rosbot_gazebo/test/simulation_test_node.py    |  9 ++++++++
 .../test/test_diff_drive_simulation.py        |  3 +++
 rosbot_gazebo/test/test_mecanum_simulation.py |  3 +++
 4 files changed, 36 insertions(+)

diff --git a/README.md b/README.md
index 80927f12..b9ed442e 100644
--- a/README.md
+++ b/README.md
@@ -106,6 +106,27 @@ source install/setup.bash
 ros2 launch rosbot_gazebo simulation.launch.py
 ```
 
+## Testing package
+
+### Industrial CI
+```
+colcon test
+```
+
+> [!NOTE]  
+> Command `colcon test` does not build the code. Remember to build your code after changes.
+
+If tests finish with errors print logs:
+```
+colcon test-result --verbose
+```
+
+### Format python code with [Black](https://github.com/psf/black)
+```
+cd src/
+black rosbot*
+```
+
 ## Demo
 Bellow you can find demos with ROSbots:
 - in [rosbot-docker](https://github.com/husarion/rosbot-docker/tree/ros2) you will find a simple example how to drive ROSbot with `teleop_twist_keyboard`.
diff --git a/rosbot_gazebo/test/simulation_test_node.py b/rosbot_gazebo/test/simulation_test_node.py
index 58561597..7fb5a4ac 100644
--- a/rosbot_gazebo/test/simulation_test_node.py
+++ b/rosbot_gazebo/test/simulation_test_node.py
@@ -22,6 +22,7 @@
 
 from geometry_msgs.msg import Twist
 from nav_msgs.msg import Odometry
+from sensor_msgs.msg import LaserScan
 
 from tf2_ros import TransformException
 from tf2_ros.buffer import Buffer
@@ -43,6 +44,7 @@ def __init__(self, name="test_node"):
         self.goal_y_event = Event()
         self.goal_yaw_event = Event()
         self.odom_tf_event = Event()
+        self.scan_event = Event()
 
     def clear_events(self):
         self.goal_x_event.clear()
@@ -62,6 +64,9 @@ def create_test_subscribers_and_publishers(self):
             Odometry, "/odometry/filtered", self.odometry_callback, 10
         )
 
+        self.scan_sub = self.create_subscription(
+            LaserScan, "/scan", self.scan_callback, 10
+        )
         self.tf_buffer = Buffer()
         self.tf_listener = TransformListener(self.tf_buffer, self)
 
@@ -97,6 +102,10 @@ def timer_callback(self):
         self.publish_cmd_vel_messages()
         self.lookup_transform_odom()
 
+    def scan_callback(self, data: LaserScan):
+        if data.ranges:
+            self.scan_event.set()
+
     def publish_cmd_vel_messages(self):
         twist_msg = Twist()
 
diff --git a/rosbot_gazebo/test/test_diff_drive_simulation.py b/rosbot_gazebo/test/test_diff_drive_simulation.py
index 318587eb..25dc7ed7 100644
--- a/rosbot_gazebo/test/test_diff_drive_simulation.py
+++ b/rosbot_gazebo/test/test_diff_drive_simulation.py
@@ -72,6 +72,9 @@ def test_diff_drive_simulation():
         msgs_received_flag = node.goal_yaw_event.wait(timeout=60.0)
         assert msgs_received_flag, "ROSbot does not rotate properly!"
 
+        msgs_received_flag = node.scan_event.wait(timeout=60.0)
+        assert msgs_received_flag, "ROSbot's lidar 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_mecanum_simulation.py b/rosbot_gazebo/test/test_mecanum_simulation.py
index 3db98f6a..f4c425ea 100644
--- a/rosbot_gazebo/test/test_mecanum_simulation.py
+++ b/rosbot_gazebo/test/test_mecanum_simulation.py
@@ -77,6 +77,9 @@ def test_mecanum_simulation():
         msgs_received_flag = node.goal_yaw_event.wait(timeout=60.0)
         assert msgs_received_flag, "ROSbot does not rotate properly!"
 
+        msgs_received_flag = node.scan_event.wait(timeout=60.0)
+        assert msgs_received_flag, "ROSbot's lidar does not work properly!"
+
     finally:
         # The pytest cannot kill properly the Gazebo Ignition's tasks what blocks launching
         # several tests in a row.