From 3f7cea414a75a936c6dc8e0ea757fbda740f55d9 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Thu, 19 Oct 2023 05:24:57 +0200 Subject: [PATCH] added tests from rosbot_ros | added scan_filter test Signed-off-by: Jakub Delicat --- rosbot_xl_bringup/CMakeLists.txt | 12 -- rosbot_xl_bringup/launch/bringup.launch.py | 19 ++- rosbot_xl_bringup/package.xml | 15 ++- rosbot_xl_bringup/resource/rosbot_xl_bringup | 0 rosbot_xl_bringup/setup.cfg | 4 + rosbot_xl_bringup/setup.py | 27 ++++ rosbot_xl_bringup/test/bringup_test_node.py | 123 ++++++++++++++++++ rosbot_xl_bringup/test/test_copyright.py | 23 ++++ rosbot_xl_bringup/test/test_diff_drive_ekf.py | 67 ++++++++++ rosbot_xl_bringup/test/test_flake8.py | 25 ++++ rosbot_xl_bringup/test/test_mecanum_ekf.py | 67 ++++++++++ rosbot_xl_bringup/test/test_pep257.py | 23 ++++ rosbot_xl_bringup/test/test_scan_filter.py | 65 +++++++++ 13 files changed, 452 insertions(+), 18 deletions(-) delete mode 100644 rosbot_xl_bringup/CMakeLists.txt create mode 100644 rosbot_xl_bringup/resource/rosbot_xl_bringup create mode 100644 rosbot_xl_bringup/setup.cfg create mode 100644 rosbot_xl_bringup/setup.py create mode 100644 rosbot_xl_bringup/test/bringup_test_node.py create mode 100644 rosbot_xl_bringup/test/test_copyright.py create mode 100644 rosbot_xl_bringup/test/test_diff_drive_ekf.py create mode 100644 rosbot_xl_bringup/test/test_flake8.py create mode 100644 rosbot_xl_bringup/test/test_mecanum_ekf.py create mode 100644 rosbot_xl_bringup/test/test_pep257.py create mode 100644 rosbot_xl_bringup/test/test_scan_filter.py diff --git a/rosbot_xl_bringup/CMakeLists.txt b/rosbot_xl_bringup/CMakeLists.txt deleted file mode 100644 index f7ec6cdb..00000000 --- a/rosbot_xl_bringup/CMakeLists.txt +++ /dev/null @@ -1,12 +0,0 @@ -cmake_minimum_required(VERSION 3.10.2) -project(rosbot_xl_bringup) - -find_package(ament_cmake REQUIRED) - -install(DIRECTORY - launch - config - DESTINATION share/${PROJECT_NAME} -) - -ament_package() diff --git a/rosbot_xl_bringup/launch/bringup.launch.py b/rosbot_xl_bringup/launch/bringup.launch.py index 09bbb1c9..8907b631 100644 --- a/rosbot_xl_bringup/launch/bringup.launch.py +++ b/rosbot_xl_bringup/launch/bringup.launch.py @@ -1,4 +1,16 @@ -#!/usr/bin/env python3 +# Copyright 2023 Husarion +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. from launch import LaunchDescription from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument @@ -18,7 +30,10 @@ def generate_launch_description(): declare_mecanum_arg = DeclareLaunchArgument( "mecanum", default_value="False", - description="Whether to use mecanum drive controller (otherwise diff drive controller is used)", + description=( + "Whether to use mecanum drive controller" + "(otherwise diff drive controller is used)" + ), ) lidar_model = LaunchConfiguration("lidar_model") diff --git a/rosbot_xl_bringup/package.xml b/rosbot_xl_bringup/package.xml index 82b5a572..a2192375 100644 --- a/rosbot_xl_bringup/package.xml +++ b/rosbot_xl_bringup/package.xml @@ -4,18 +4,17 @@ rosbot_xl_bringup 0.8.2 - The rosbot_xl_bringup package + ROSbot XL bringup package Apache License 2.0 Maciej Stepien + Jakub Delicat Husarion https://husarion.com/ https://github.com/husarion/rosbot_xl_ros https://github.com/husarion/rosbot_xl_ros/issues - ament_cmake - rosbot_xl_controller launch @@ -24,7 +23,15 @@ laser_filters robot_localization + python3-pytest + launch + launch_ros + launch_pytest + + robot_localization + rosbot_xl_controller + - ament_cmake + ament_python diff --git a/rosbot_xl_bringup/resource/rosbot_xl_bringup b/rosbot_xl_bringup/resource/rosbot_xl_bringup new file mode 100644 index 00000000..e69de29b diff --git a/rosbot_xl_bringup/setup.cfg b/rosbot_xl_bringup/setup.cfg new file mode 100644 index 00000000..08240e7c --- /dev/null +++ b/rosbot_xl_bringup/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/rosbot_xl_bringup +[install] +install_scripts=$base/lib/rosbot_xl_bringup diff --git a/rosbot_xl_bringup/setup.py b/rosbot_xl_bringup/setup.py new file mode 100644 index 00000000..ede6375c --- /dev/null +++ b/rosbot_xl_bringup/setup.py @@ -0,0 +1,27 @@ +import os +from glob import glob +from setuptools import find_packages, setup + +package_name = "rosbot_xl_bringup" + +setup( + name=package_name, + version="0.8.2", + packages=find_packages(exclude=["test"]), + data_files=[ + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), + (os.path.join("share", package_name, "launch"), glob("launch/*.launch.py")), + (os.path.join("share", package_name, "config"), glob("config/*.yaml")), + ], + install_requires=["setuptools"], + zip_safe=True, + maintainer="Husarion", + maintainer_email="contact@husarion.com", + description="ROSbot XL bringup package", + license="Apache License 2.0", + tests_require=["pytest"], + entry_points={ + "console_scripts": [], + }, +) diff --git a/rosbot_xl_bringup/test/bringup_test_node.py b/rosbot_xl_bringup/test/bringup_test_node.py new file mode 100644 index 00000000..29816aae --- /dev/null +++ b/rosbot_xl_bringup/test/bringup_test_node.py @@ -0,0 +1,123 @@ +# Copyright 2021 Open Source Robotics Foundation, Inc. +# Copyright 2023 Husarion +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import rclpy + +from threading import Event +from threading import Thread + +from rclpy.node import Node + +from sensor_msgs.msg import JointState, Imu, LaserScan +from tf2_ros import TransformException +from tf2_ros.buffer import Buffer +from tf2_ros.transform_listener import TransformListener + + +class BringupTestNode(Node): + ROSBOT_HARDWARE_PUBLISHERS_RATE = 10.0 + + __test__ = False + + def __init__(self, name="test_node"): + super().__init__(name) + self.odom_tf_event = Event() + self.scan_filter_event = Event() + + def create_test_subscribers_and_publishers(self): + self.imu_publisher = self.create_publisher(Imu, "_imu/data_raw", 10) + + self.joint_states_publisher = self.create_publisher( + JointState, "_motors_response", 10 + ) + + self.tf_buffer = Buffer() + self.tf_listener = TransformListener(self.tf_buffer, self) + + self.scan_publisher = self.create_publisher(LaserScan, "scan", 10) + self.filtered_scan_subscriber = self.create_subscription( + LaserScan, "scan_filtered", self.filtered_scan_callback, 10 + ) + + self.timer = None + + def lookup_transform_odom(self): + try: + self.tf_buffer.lookup_transform("odom", "base_link", rclpy.time.Time()) + self.odom_tf_event.set() + except TransformException as ex: + self.get_logger().error(f"Could not transform odom to base_link: {ex}") + + def start_node_thread(self): + self.ros_spin_thread = Thread( + target=lambda node: rclpy.spin(node), args=(self,) + ) + self.ros_spin_thread.start() + + def start_publishing_fake_hardware(self): + self.timer = self.create_timer( + 1.0 / self.ROSBOT_HARDWARE_PUBLISHERS_RATE, + self.timer_callback, + ) + + def filtered_scan_callback(self, msg): + if msg.ranges[0] == 5.0 and msg.ranges[-1] == 5.0: + for i in range(len(msg.ranges) - 2): + # when is not .nan + if msg.ranges[i + 1] == msg.ranges[i + 1]: + return + + self.scan_filter_event.set() + + def timer_callback(self): + self.publish_fake_hardware_messages() + self.lookup_transform_odom() + self.publish_scan() + + def publish_fake_hardware_messages(self): + imu_msg = Imu() + imu_msg.header.stamp = self.get_clock().now().to_msg() + imu_msg.header.frame_id = "imu_link" + + joint_state_msg = JointState() + joint_state_msg.header.stamp = self.get_clock().now().to_msg() + joint_state_msg.name = [ + "fl_wheel_joint", + "fr_wheel_joint", + "rl_wheel_joint", + "rr_wheel_joint", + ] + joint_state_msg.position = [0.0, 0.0, 0.0, 0.0] + joint_state_msg.velocity = [0.0, 0.0, 0.0, 0.0] + + self.imu_publisher.publish(imu_msg) + self.joint_states_publisher.publish(joint_state_msg) + + def publish_scan(self): + msg = LaserScan() + msg.header.frame_id = "base_link" + msg.angle_min = 0.0 + msg.angle_max = 2.0 * 3.14159265 + msg.angle_increment = 0.05 + msg.time_increment = 0.1 + msg.scan_time = 0.1 + msg.range_min = 0.0 + msg.range_max = 10.0 + + # All points inside filtered box + msg.ranges = [0.1] * 126 + msg.ranges[0] = 5.0 + msg.ranges[-1] = 5.0 + self.scan_publisher.publish(msg) diff --git a/rosbot_xl_bringup/test/test_copyright.py b/rosbot_xl_bringup/test/test_copyright.py new file mode 100644 index 00000000..f46f861d --- /dev/null +++ b/rosbot_xl_bringup/test/test_copyright.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=[".", "test"]) + assert rc == 0, "Found errors" diff --git a/rosbot_xl_bringup/test/test_diff_drive_ekf.py b/rosbot_xl_bringup/test/test_diff_drive_ekf.py new file mode 100644 index 00000000..1d391097 --- /dev/null +++ b/rosbot_xl_bringup/test/test_diff_drive_ekf.py @@ -0,0 +1,67 @@ +# Copyright 2021 Open Source Robotics Foundation, Inc. +# Copyright 2023 Intel Corporation. All Rights Reserved. +# Copyright 2023 Husarion +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import launch_pytest +import pytest +import rclpy + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.substitutions import PathJoinSubstitution +from launch.launch_description_sources import PythonLaunchDescriptionSource +from bringup_test_node import BringupTestNode + + +@launch_pytest.fixture +def generate_test_description(): + rosbot_xl_bringup = get_package_share_directory("rosbot_xl_bringup") + bringup_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + rosbot_xl_bringup, + "launch", + "bringup.launch.py", + ] + ) + ), + launch_arguments={ + "use_sim": "False", + "mecanum": "False", + "use_gpu": "False", + }.items(), + ) + + return LaunchDescription([bringup_launch]) + + +@pytest.mark.launch(fixture=generate_test_description) +def test_bringup_startup_success(): + rclpy.init() + try: + node = BringupTestNode("test_bringup") + node.create_test_subscribers_and_publishers() + node.start_publishing_fake_hardware() + + node.start_node_thread() + msgs_received_flag = node.odom_tf_event.wait(timeout=10.0) + assert ( + msgs_received_flag + ), "Expected odom to base_link tf but it was not received. Check robot_localization!" + + finally: + rclpy.shutdown() diff --git a/rosbot_xl_bringup/test/test_flake8.py b/rosbot_xl_bringup/test/test_flake8.py new file mode 100644 index 00000000..ee79f31a --- /dev/null +++ b/rosbot_xl_bringup/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, "Found %d code style errors / warnings:\n" % len( + errors + ) + "\n".join(errors) diff --git a/rosbot_xl_bringup/test/test_mecanum_ekf.py b/rosbot_xl_bringup/test/test_mecanum_ekf.py new file mode 100644 index 00000000..cd18525f --- /dev/null +++ b/rosbot_xl_bringup/test/test_mecanum_ekf.py @@ -0,0 +1,67 @@ +# Copyright 2021 Open Source Robotics Foundation, Inc. +# Copyright 2023 Intel Corporation. All Rights Reserved. +# Copyright 2023 Husarion +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import launch_pytest +import pytest +import rclpy + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.substitutions import PathJoinSubstitution +from launch.launch_description_sources import PythonLaunchDescriptionSource +from bringup_test_node import BringupTestNode + + +@launch_pytest.fixture +def generate_test_description(): + rosbot_xl_bringup = get_package_share_directory("rosbot_xl_bringup") + bringup_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + rosbot_xl_bringup, + "launch", + "bringup.launch.py", + ] + ) + ), + launch_arguments={ + "use_sim": "False", + "mecanum": "True", + "use_gpu": "False", + }.items(), + ) + + return LaunchDescription([bringup_launch]) + + +@pytest.mark.launch(fixture=generate_test_description) +def test_bringup_startup_success(): + rclpy.init() + try: + node = BringupTestNode("test_bringup") + node.create_test_subscribers_and_publishers() + node.start_publishing_fake_hardware() + + node.start_node_thread() + msgs_received_flag = node.odom_tf_event.wait(timeout=10.0) + assert ( + msgs_received_flag + ), "Expected odom to base_link tf but it was not received. Check robot_localization!" + + finally: + rclpy.shutdown() diff --git a/rosbot_xl_bringup/test/test_pep257.py b/rosbot_xl_bringup/test/test_pep257.py new file mode 100644 index 00000000..a2c3deb8 --- /dev/null +++ b/rosbot_xl_bringup/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=[".", "test"]) + assert rc == 0, "Found code style errors / warnings" diff --git a/rosbot_xl_bringup/test/test_scan_filter.py b/rosbot_xl_bringup/test/test_scan_filter.py new file mode 100644 index 00000000..c29cec8a --- /dev/null +++ b/rosbot_xl_bringup/test/test_scan_filter.py @@ -0,0 +1,65 @@ +# Copyright 2021 Open Source Robotics Foundation, Inc. +# Copyright 2023 Intel Corporation. All Rights Reserved. +# Copyright 2023 Husarion +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import launch_pytest +import pytest +import rclpy + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.substitutions import PathJoinSubstitution +from launch.launch_description_sources import PythonLaunchDescriptionSource +from bringup_test_node import BringupTestNode + + +@launch_pytest.fixture +def generate_test_description(): + rosbot_xl_bringup = get_package_share_directory("rosbot_xl_bringup") + bringup_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + rosbot_xl_bringup, + "launch", + "bringup.launch.py", + ] + ) + ), + launch_arguments={ + "use_gpu": "False", + }.items(), + ) + + return LaunchDescription([bringup_launch]) + + +@pytest.mark.launch(fixture=generate_test_description) +def test_scan_filter(): + rclpy.init() + try: + node = BringupTestNode("test_bringup") + node.create_test_subscribers_and_publishers() + node.start_publishing_fake_hardware() + + node.start_node_thread() + msgs_received_flag = node.scan_filter_event.wait(timeout=10.0) + assert ( + msgs_received_flag + ), "Expected filtered scan but it is not filtered properly. Check laser_filter!" + + finally: + rclpy.shutdown()