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

add test for back and more for front

Browse Source
This commit is contained in:
Sem van der Hoeven
2023-06-12 15:52:59 +02:00
parent 5b7f239f8f
commit 8eea606db3
1 changed files with 53 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files

77
src/drone_controls/test/test_positionchanger_lidar.py
View File

@@ -58,10 +58,23 @@ class TestPositionChanger(unittest.TestCase):
self.node = rclpy.create_node('test_positionchanger') self.node = rclpy.create_node('test_positionchanger')
self.called_positionchanger_service = False self.called_positionchanger_service = False
self.received_failsafe_callback = False self.received_failsafe_callback = False
self.lidar_publisher = self.node.create_publisher(
LidarReading, '/drone/object_detection', 10)
self.move_position_client = self.node.create_client(
MovePosition, '/drone/move_position')
while not self.move_position_client.wait_for_service(timeout_sec=1.0):
self.node.get_logger().info('move_position service not available, waiting again...')
self.request = MovePosition.Request()
self.node.destroy_client(self.move_position_client)
self.node.destroy_publisher(self.lidar_publisher)
def tearDown(self): def tearDown(self):
self.node.destroy_node() self.node.destroy_node()
def toRadians(self, degrees) -> float:
return degrees * 3.14159265359 / 180
def move_position_callback(self, future): def move_position_callback(self, future):
self.node.get_logger().info("Callback called") self.node.get_logger().info("Callback called")
self.called_positionchanger_service = True self.called_positionchanger_service = True
@@ -69,37 +82,53 @@ class TestPositionChanger(unittest.TestCase):
def validate_output(self, output): def validate_output(self, output):
assert len(output) > 0, 'Output is empty' assert len(output) > 0, 'Output is empty'
def test_positionchanger_lidar_moves_away(self, px4_controller_node, proc_output): def test_positionchanger_lidar_moves_away_front(self, px4_controller_node, proc_output):
self.node.get_logger().info("STARTING TEST test_positionchanger_lidar_moves_away") self.node.get_logger().info("STARTING TEST test_positionchanger_lidar_moves_away")
lidar_publisher = self.node.create_publisher( self.request.front_back = 1.0
LidarReading, '/drone/object_detection', 10) self.request.left_right = 0.0
move_position_client = self.node.create_client( self.request.up_down = 0.0
MovePosition, '/drone/move_position') self.request.angle = 0.0
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 = LidarReading()
lidar_msg.sensor_1 = 0.5 lidar_msg.sensor_1 = 0.5
lidar_msg.sensor_2 = 2.0 lidar_msg.sensor_2 = 0.3
lidar_msg.sensor_3 = 2.0 lidar_msg.sensor_3 = 2.0
lidar_msg.sensor_4 = 2.0 lidar_msg.sensor_4 = 2.0
lidar_msg.imu_data = [1.0, 1.0, 1.0, 1.0] lidar_msg.imu_data = [1.0, 1.0, 1.0, 1.0]
end_time = time.time() + 10.0 end_time = time.time() + 10.0
self.node.get_logger().info("STARTING while loop test") self.node.get_logger().info("STARTING while loop test")
try:
while time.time() < end_time: while time.time() < end_time:
rclpy.spin_once(self.node, timeout_sec=0.1) rclpy.spin_once(self.node, timeout_sec=0.1)
lidar_publisher.publish(lidar_msg) self.lidar_publisher.publish(lidar_msg)
if not self.called_positionchanger_service: if not self.called_positionchanger_service:
future = move_position_client.call_async(request) future = self.move_position_client.call_async(self.request)
future.add_done_callback(self.move_position_callback) future.add_done_callback(self.move_position_callback)
launch_testing.asserts.assertInStderr(proc_output, "Collision prevention front", 'position_changer-1') launch_testing.asserts.assertInStderr(proc_output, "Collision prevention front: -0.5", 'position_changer-1')
finally: launch_testing.asserts.assertInStderr(proc_output, "Collision prevention front: -0.7", 'position_changer-1')
self.node.destroy_client(move_position_client)
self.node.destroy_publisher(lidar_publisher) def test_positionchanger_lidar_moves_away_back(self, px4_controller_node, proc_output):
self.node.get_logger().info("STARTING TEST test_positionchanger_lidar_moves_away")
self.request.front_back = -1.0
self.request.left_right = 0.0
self.request.up_down = 0.0
self.request.angle = 0.0
lidar_msg = LidarReading()
lidar_msg.sensor_1 = 2.0
lidar_msg.sensor_2 = 2.0
lidar_msg.sensor_3 = 0.5
lidar_msg.sensor_4 = 2.0
lidar_msg.imu_data = [1.0, 1.0, 1.0, 1.0]
end_time = time.time() + 10.0
self.node.get_logger().info("STARTING while loop test")
while time.time() < end_time:
rclpy.spin_once(self.node, timeout_sec=0.1)
self.lidar_publisher.publish(lidar_msg)
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 front: -0.5", 'position_changer-1')