From e55a2cf53a713c6555bcd434538b76be739f53d4 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Mon, 12 Jun 2023 16:46:07 +0200 Subject: [PATCH] add left and back tests --- .../test/test_positionchanger_lidar.py | 62 ++++++++++++++++++- 1 file changed, 60 insertions(+), 2 deletions(-) diff --git a/src/drone_controls/test/test_positionchanger_lidar.py b/src/drone_controls/test/test_positionchanger_lidar.py index 55f0bc5f..bc8237df 100644 --- a/src/drone_controls/test/test_positionchanger_lidar.py +++ b/src/drone_controls/test/test_positionchanger_lidar.py @@ -143,10 +143,68 @@ class TestPositionChanger(unittest.TestCase): launch_testing.asserts.assertInStderr(proc_output, "Collision prevention right: -0.71", 'position_changer-1') def test_positionchanger_lidar_moves_away_left(self, proc_output): - self.assertTrue(False, "Not implemented yet") + self.node.get_logger().info("starting left test") + self.request.front_back = 0.0 + self.request.left_right = -1.0 + self.request.up_down = 0.0 + self.request.angle = 0.0 + + lidar_msgs_sent = 0 + + lidar_msg = LidarReading() + lidar_msg.sensor_1 = 2.0 # front right + lidar_msg.sensor_2 = 0.11 # front left + lidar_msg.sensor_3 = 2.0 # rear left + lidar_msg.sensor_4 = 2.0 # rear right + lidar_msg.imu_data = [1.0, 1.0, 1.0, 1.0] + end_time = time.time() + 10.0 + + while time.time() < end_time: + rclpy.spin_once(self.node, timeout_sec=0.1) + self.lidar_publisher.publish(lidar_msg) + lidar_msgs_sent += 1 + if (lidar_msgs_sent == 10): + lidar_msg.sensor_2 = 2.0 + lidar_msg.sensor_3 = 0.78 + elif (lidar_msgs_sent == 20): + break + if not self.called_positionchanger_service: + future = self.move_position_client.call_async(self.request) + future.add_done_callback(self.move_position_callback) + launch_testing.asserts.assertInStderr(proc_output, "Collision prevention left: 0.89", 'position_changer-1') + launch_testing.asserts.assertInStderr(proc_output, "Collision prevention left: 0.22", 'position_changer-1') def test_positionchanger_lidar_moves_away_back(self, proc_output): - self.assertTrue(False, "Not implemented yet") + self.node.get_logger().info("starting back test") + self.request.front_back = -1.0 + self.request.left_right = 0.0 + self.request.up_down = 0.0 + self.request.angle = 0.0 + + lidar_msgs_sent = 0 + + lidar_msg = LidarReading() + lidar_msg.sensor_1 = 2.0 # front right + lidar_msg.sensor_2 = 2.0 # front left + lidar_msg.sensor_3 = 0.36 # rear left + lidar_msg.sensor_4 = 2.0 # rear right + lidar_msg.imu_data = [1.0, 1.0, 1.0, 1.0] + end_time = time.time() + 10.0 + + while time.time() < end_time: + rclpy.spin_once(self.node, timeout_sec=0.1) + self.lidar_publisher.publish(lidar_msg) + lidar_msgs_sent += 1 + if (lidar_msgs_sent == 10): + lidar_msg.sensor_3 = 2.0 + lidar_msg.sensor_4 = 0.12 + elif (lidar_msgs_sent == 20): + break + if not self.called_positionchanger_service: + future = self.move_position_client.call_async(self.request) + future.add_done_callback(self.move_position_callback) + launch_testing.asserts.assertInStderr(proc_output, "Collision prevention left: 0.64", 'position_changer-1') + launch_testing.asserts.assertInStderr(proc_output, "Collision prevention left: 0.88", 'position_changer-1') def test_positionchanger_lidar_moves_away_still(self, proc_output): self.assertTrue(False, "Not implemented yet")