From 473a6e25756a61eafe054ff0d8de0724d7b8c94b Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Mon, 11 Sep 2023 12:30:17 +0200 Subject: [PATCH 1/5] added scan test Signed-off-by: Jakub Delicat --- rosbot_gazebo/test/simulation_test_node.py | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/rosbot_gazebo/test/simulation_test_node.py b/rosbot_gazebo/test/simulation_test_node.py index f21a1788..f2ba5c16 100644 --- a/rosbot_gazebo/test/simulation_test_node.py +++ b/rosbot_gazebo/test/simulation_test_node.py @@ -14,6 +14,7 @@ # limitations under the License. import rclpy +import tf_transformations from threading import Event from threading import Thread @@ -22,10 +23,11 @@ from geometry_msgs.msg import Twist from nav_msgs.msg import Odometry -import tf_transformations +from sensor_msgs.msg import LaserScan class SimulationTestNode(Node): + MINIMAL_LASER_SCAN_RANGE = 0.1 __test__ = False def __init__(self, name="test_node"): @@ -37,6 +39,7 @@ def __init__(self, name="test_node"): self.goal_x_event = Event() self.goal_y_event = Event() self.goal_theta_event = Event() + self.scan_event = Event() def set_and_publish_velocities( self, goal_x_distance, goal_y_distance, goal_theta_angle @@ -52,6 +55,11 @@ def create_test_subscribers_and_publishers(self): self.odom_sub = self.create_subscription( Odometry, "/odometry/filtered", self.odometry_callback, 10 ) + + self.scan_sub = self.create_subscription( + LaserScan, "/scan", self.scan_callback, 10 + ) + self.timer = None def start_node_thread(self): @@ -83,6 +91,12 @@ def odometry_callback(self, data: Odometry): if yaw > self.goal_theta_angle: self.goal_theta_event.set() + def scan_callback(self, data: LaserScan): + if min(data.ranges) < self.MINIMAL_LASER_SCAN_RANGE: + print(f"Ranges: {data.ranges}") + return + self.scan_event.set() + def publish_cmd_vel_messages(self): twist_msg = Twist() twist_msg.linear.x = self.goal_x_distance From b3534d78d92f5c10475cf537c0005f817335f9e1 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Wed, 11 Oct 2023 11:33:41 +0200 Subject: [PATCH 2/5] updated gazebo scan test Signed-off-by: Jakub Delicat --- rosbot_gazebo/test/simulation_test_node.py | 5 ----- rosbot_gazebo/test/test_diff_drive_simulation.py | 3 +++ rosbot_gazebo/test/test_mecanum_simulation.py | 3 +++ 3 files changed, 6 insertions(+), 5 deletions(-) diff --git a/rosbot_gazebo/test/simulation_test_node.py b/rosbot_gazebo/test/simulation_test_node.py index 357f0b96..16b4db85 100644 --- a/rosbot_gazebo/test/simulation_test_node.py +++ b/rosbot_gazebo/test/simulation_test_node.py @@ -14,7 +14,6 @@ # limitations under the License. import rclpy -import tf_transformations from threading import Event from threading import Thread @@ -31,7 +30,6 @@ class SimulationTestNode(Node): - MINIMAL_LASER_SCAN_RANGE = 0.1 __test__ = False XY_TOLERANCE = 0.05 YAW_TOLERANCE = 0.1 @@ -105,9 +103,6 @@ def timer_callback(self): self.lookup_transform_odom() def scan_callback(self, data: LaserScan): - if min(data.ranges) < self.MINIMAL_LASER_SCAN_RANGE: - print(f"Ranges: {data.ranges}") - return self.scan_event.set() def publish_cmd_vel_messages(self): 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. From 23c5c836d94d95b51c7a72b7719b377f80cb8957 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Wed, 11 Oct 2023 11:53:52 +0200 Subject: [PATCH 3/5] Added tests to readme Signed-off-by: Jakub Delicat --- README.md | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/README.md b/README.md index 80927f12..c2a604dc 100644 --- a/README.md +++ b/README.md @@ -106,6 +106,23 @@ source install/setup.bash ros2 launch rosbot_gazebo simulation.launch.py ``` +## Testing package + +### Industrial CI +``` +colcon test +``` +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`. From cd248646dcf590ca21ba6fd458b7ec244abe5390 Mon Sep 17 00:00:00 2001 From: Jakub Delicat <109142865+delihus@users.noreply.github.com> Date: Mon, 16 Oct 2023 07:38:41 +0200 Subject: [PATCH 4/5] Update rosbot_gazebo/test/simulation_test_node.py Co-authored-by: rafal-gorecki <126687345+rafal-gorecki@users.noreply.github.com> --- rosbot_gazebo/test/simulation_test_node.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/rosbot_gazebo/test/simulation_test_node.py b/rosbot_gazebo/test/simulation_test_node.py index 16b4db85..7fb5a4ac 100644 --- a/rosbot_gazebo/test/simulation_test_node.py +++ b/rosbot_gazebo/test/simulation_test_node.py @@ -103,7 +103,8 @@ def timer_callback(self): self.lookup_transform_odom() def scan_callback(self, data: LaserScan): - self.scan_event.set() + if data.ranges: + self.scan_event.set() def publish_cmd_vel_messages(self): twist_msg = Twist() From ea369a0ef5cb088a889c13ea697e1a75eb96ed6d Mon Sep 17 00:00:00 2001 From: Jakub Delicat <109142865+delihus@users.noreply.github.com> Date: Mon, 16 Oct 2023 07:39:00 +0200 Subject: [PATCH 5/5] Update README.md Co-authored-by: rafal-gorecki <126687345+rafal-gorecki@users.noreply.github.com> --- README.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/README.md b/README.md index c2a604dc..b9ed442e 100644 --- a/README.md +++ b/README.md @@ -112,6 +112,10 @@ ros2 launch rosbot_gazebo simulation.launch.py ``` 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