add test for moving away with lidar

src/drone_controls/test/test_positionchanger.py

@@ -44,6 +44,7 @@ def generate_test_description():
         }
     )
 
+
 class TestPositionChanger(unittest.TestCase):
 
     @classmethod
@@ -59,7 +60,6 @@ class TestPositionChanger(unittest.TestCase):
         self.called_positionchanger_service = False
         self.received_failsafe_callback = False
 
-
     def tearDown(self):
         self.node.destroy_node()
 
@@ -68,14 +68,17 @@ class TestPositionChanger(unittest.TestCase):
         self.received_failsafe_callback = True
 
     def move_position_callback(self, future):
-        self.assertFalse(future.result().success, "MovePosition service call was successful, but should have failed!")
+        self.assertFalse(future.result(
+        ).success, "MovePosition service call was successful, but should have failed!")
         self.called_positionchanger_service = True
 
     def test_positionchanger_no_lidar_data(self, positionchanger_node, proc_output):
         self.received_failsafe_callback = False
         self.called_positionchanger_service = False
-        failsafe_subscriber = self.node.create_subscription(FailsafeMsg,'/drone/failsafe',self.failsafe_callback,10)
+        failsafe_subscriber = self.node.create_subscription(
-        move_position_client = self.node.create_client(MovePosition,'/drone/move_position')
+            FailsafeMsg, '/drone/failsafe', self.failsafe_callback, 10)
+        move_position_client = self.node.create_client(
+            MovePosition, '/drone/move_position')
         while not move_position_client.wait_for_service(timeout_sec=1.0):
             self.node.get_logger().info('move_position service not available, waiting again...')
 
@@ -96,21 +99,24 @@ class TestPositionChanger(unittest.TestCase):
                     continue
                 else:
                     break
-            self.assertTrue(self.received_failsafe_callback, "Failsafe callback was not received!")
+            self.assertTrue(self.received_failsafe_callback,
-            self.assertTrue(self.called_positionchanger_service, "MovePosition service was not called!")
+                            "Failsafe callback was not received!")
+            self.assertTrue(self.called_positionchanger_service,
+                            "MovePosition service was not called!")
         finally:
             self.node.destroy_client(move_position_client)
             self.node.destroy_subscription(failsafe_subscriber)
 
-
-
     def test_positionchanger_lidar_stops(self, positionchanger_node, proc_output):
         self.node.get_logger().info("STARTING TEST test_positionchanger_lidar_stops")
         self.received_failsafe_callback = False
         self.called_positionchanger_service = False
-        failsafe_subscriber = self.node.create_subscription(FailsafeMsg,'/drone/failsafe',self.failsafe_callback,10)
+        failsafe_subscriber = self.node.create_subscription(
-        lidar_publisher = self.node.create_publisher(LidarReading,'/drone/object_detection',10)
+            FailsafeMsg, '/drone/failsafe', self.failsafe_callback, 10)
-        move_position_client = self.node.create_client(MovePosition,'/drone/move_position')
+        lidar_publisher = self.node.create_publisher(
+            LidarReading, '/drone/object_detection', 10)
+        move_position_client = self.node.create_client(
+            MovePosition, '/drone/move_position')
 
         while not move_position_client.wait_for_service(timeout_sec=1.0):
             self.node.get_logger().info('move_position service not available, waiting again...')
@@ -150,11 +156,47 @@ class TestPositionChanger(unittest.TestCase):
                 elif not self.received_failsafe_callback:
                     continue
             self.node.get_logger().info('END OF WHILE LOOP')
-            self.assertTrue(self.called_positionchanger_service, "MovePosition service was not called!")
+            self.assertTrue(self.called_positionchanger_service,
-            self.assertTrue(self.received_failsafe_callback, "Failsafe was not activated!")
+                            "MovePosition service was not called!")
+            self.assertTrue(self.received_failsafe_callback,
+                            "Failsafe was not activated!")
 
         finally:
             self.node.get_logger().info('Cleaning up')
             self.node.destroy_client(move_position_client)
             self.node.destroy_subscription(failsafe_subscriber)
             self.node.destroy_publisher(lidar_publisher)
+
+    def test_positionchanger_lidar_moves_away(self, positionchanger_node, px4_controller_node, proc_output):
+        self.node.get_logger().info("STARTING TEST test_positionchanger_lidar_moves_away")
+        lidar_publisher = self.node.create_publisher(
+            LidarReading, '/drone/object_detection', 10)
+        move_position_client = self.node.create_client(
+            MovePosition, '/drone/move_position')
+        while not move_position_client.wait_for_service(timeout_sec=1.0):
+            self.node.get_logger().info('move_position service not available, waiting again...')
+        request = MovePosition.Request()
+        request.front_back = 1.0
+        request.left_right = 0.0
+        request.up_down = 0.0
+        request.angle = 0.0
+
+        lidar_msg = LidarReading()
+        lidar_msg.sensor_1 = 0.5
+        lidar_msg.sensor_2 = 2.0
+        lidar_msg.sensor_3 = 2.0
+        lidar_msg.sensor_4 = 2.0
+        lidar_msg.imu_data = [1.0, 1.0, 1.0, 1.0]
+        end_time = time.time() + 10.0
+
+        try:
+            while time.time() < end_time:
+                rclpy.spin_once(self.node, timeout_sec=0.1)
+                lidar_publisher.publish(lidar_msg)
+                if not self.called_positionchanger_service:
+                    future = move_position_client.call_async(request)
+                    future.add_done_callback(self.move_position_callback)
+            proc_output.assertWaitFor(expected_output='0.5',process=px4_controller_node)
+        finally:
+            self.node.destroy_client(move_position_client)
+            self.node.destroy_publisher(lidar_publisher)