sem/5g_drone_ROS2
1
0
Fork 0
Code Issues 1 Pull Requests 1 Actions Packages Projects Releases Wiki Activity

add test for moving away with lidar

Browse Source
This commit is contained in:
Sem van der Hoeven
2023-06-12 11:31:22 +02:00
parent bcc14c9e0d
commit e706a38ea4
1 changed files with 68 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files

68
src/drone_controls/test/test_positionchanger.py
View File

@@ -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)
move_position_client = self.node.create_client(MovePosition,'/drone/move_position')
failsafe_subscriber = self.node.create_subscription(
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.called_positionchanger_service, "MovePosition service was not called!")
self.assertTrue(self.received_failsafe_callback,
"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)
lidar_publisher = self.node.create_publisher(LidarReading,'/drone/object_detection',10)
move_position_client = self.node.create_client(MovePosition,'/drone/move_position')
failsafe_subscriber = self.node.create_subscription(
FailsafeMsg, '/drone/failsafe', self.failsafe_callback, 10)
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.received_failsafe_callback, "Failsafe was not activated!")
self.assertTrue(self.called_positionchanger_service,
"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)