Xacro and controllers runtime test

rosbot_bringup/launch/bringup.launch.py

@@ -32,7 +32,6 @@ def generate_launch_description():
         choices=["ignition-gazebo", "gazebo-classic", "webots"]
     )
 
-
     rosbot_controller = get_package_share_directory("rosbot_controller")
     rosbot_bringup = get_package_share_directory("rosbot_bringup")
 

rosbot_bringup/package.xml

@@ -3,7 +3,7 @@
 <package format="3">
   <name>rosbot_bringup</name>
   <version>0.8.1</version>
-  <description>Rosbot bringup package</description>
+  <description>ROSbot 2, 2R, PRO bringup package</description>
   <license>Apache License 2.0</license>
 
   <author email="[email protected]">Jakub Delicat</author>
@@ -13,14 +13,12 @@
   <url type="repository">https://github.com/husarion/rosbot_ros</url>
   <url type="bugtracker">https://github.com/husarion/rosbot_ros/issues</url>
 
-  <buildtool_depend>ament_cmake</buildtool_depend>
-
   <exec_depend>launch</exec_depend>
   <exec_depend>launch_ros</exec_depend>
   <exec_depend>rosbot_controller</exec_depend>
   <exec_depend>robot_localization</exec_depend>
 
   <export>
-    <build_type>ament_cmake</build_type>
+    <build_type>ament_python</build_type>
   </export>
 </package>

rosbot_bringup/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/rosbot_bringup
+[install]
+install_scripts=$base/lib/rosbot_bringup

rosbot_bringup/setup.py

@@ -0,0 +1,29 @@
+import os
+from glob import glob
+from setuptools import find_packages, setup
+
+package_name = 'rosbot_bringup'
+
+setup(
+    name=package_name,
+    version='0.8.0',
+    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='[email protected]',
+    description='ROSbot 2, 2R, PRO bringup package',
+    license='Apache License 2.0',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+        ],
+    },
+)

rosbot_controller/package.xml

@@ -4,7 +4,7 @@
   <name>rosbot_controller</name>
   <version>0.8.1</version>
 
-  <description>Hardware configuration for ROSbot XL</description>
+  <description>Hardware configuration for ROSbot 2, 2R, PRO</description>
   <license>Apache License 2.0</license>
 
   <author email="[email protected]">Jakub Delicat</author>
@@ -16,22 +16,41 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
-  <exec_depend>rosbot_description</exec_depend>
-
   <exec_depend>launch</exec_depend>
   <exec_depend>launch_ros</exec_depend>
 
   <exec_depend>xacro</exec_depend>
   <exec_depend>controller_manager</exec_depend>
   <exec_depend>robot_state_publisher</exec_depend>
 
+  <exec_depend>rosbot_description</exec_depend>
+  <exec_depend>ros_components_description</exec_depend>
+  <exec_depend>rosbot_hardware_interfaces</exec_depend>
+
   <exec_depend>joint_state_broadcaster</exec_depend>
   <exec_depend>imu_sensor_broadcaster</exec_depend>
   <exec_depend>diff_drive_controller</exec_depend>
+  <exec_depend>mecanum_drive_controller</exec_depend>
 
-  <exec_depend>rosbot_hardware_interfaces</exec_depend>
+  <test_depend>python3-pytest</test_depend>
+  <test_depend>launch</test_depend>
+  <test_depend>launch_ros</test_depend>
+  <test_depend>launch_pytest</test_depend>
+
+  <test_depend>xacro</test_depend>
+  <test_depend>sensor_msgs</test_depend>
+  <test_depend>nav_msgs</test_depend>
+
+  <test_depend>rosbot_description</test_depend>
+  <test_depend>ros_components_description</test_depend>
+
+  <test_depend>controller_manager</test_depend>
+  <test_depend>joint_state_broadcaster</test_depend>
+  <test_depend>imu_sensor_broadcaster</test_depend>
+  <test_depend>diff_drive_controller</test_depend>
+  <test_depend>mecanum_drive_controller</test_depend>
 
   <export>
-    <build_type>ament_cmake</build_type>
+    <build_type>ament_python</build_type>
   </export>
 </package>

rosbot_controller/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/rosbot_controller
+[install]
+install_scripts=$base/lib/rosbot_controller

rosbot_controller/setup.py

@@ -0,0 +1,29 @@
+import os
+from glob import glob
+from setuptools import find_packages, setup
+
+package_name = 'rosbot_controller'
+
+setup(
+    name=package_name,
+    version='0.8.0',
+    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='[email protected]',
+    description='Hardware configuration for ROSbot 2, 2R, PRO',
+    license='Apache License 2.0',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+        ],
+    },
+)

rosbot_controller/test/controllers_test_node.py

@@ -0,0 +1,97 @@
+# 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
+from nav_msgs.msg import Odometry
+
+ROSBOT_HARDWARE_PUBLISHERS_RATE = 10.0
+
+
+class ControllersTestNode(Node):
+    __test__ = False
+
+    def __init__(self, name='test_node'):
+        super().__init__(name)
+        self.joint_state_msg_event = Event()
+        self.odom_msg_event = Event()
+        self.imu_msg_event = Event()
+
+    def create_test_subscribers_and_publishers(self):
+        self.joint_state_sub = self.create_subscription(
+            JointState,
+            '/joint_states',
+            self.joint_states_callback,
+            10
+        )
+
+        self.odom_sub = self.create_subscription(
+            Odometry,
+            '/rosbot_base_controller/odom',
+            self.odometry_callback,
+            10
+        )
+
+        self.imu_sub = self.create_subscription(
+            Imu,
+            '/imu_broadcaster/imu',
+            self.joint_states_callback,
+            10
+        )
+
+        self.imu_publisher = self.create_publisher(Imu, '_imu/data_raw', 10)
+
+        self.joint_states_publisher = self.create_publisher(
+            JointState, '_motors_response', 10)
+
+        self.timer = None
+
+    def start_node_thread(self):
+        self.ros_spin_thread = Thread(
+            target=lambda node: rclpy.spin(node), args=(self,))
+        self.ros_spin_thread.start()
+
+    def joint_states_callback(self, data):
+        self.joint_state_msg_event.set()
+
+    def odometry_callback(self, data):
+        self.odom_msg_event.set()
+
+    def imu_callback(self, data):
+        self.imu_msg_event.set()
+
+    def start_publishing_fake_hardware(self):
+        self.timer = self.create_timer(
+            1/ROSBOT_HARDWARE_PUBLISHERS_RATE, self.publish_fake_hardware_messages)
+
+    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)

rosbot_controller/test/test_diff_drive_controllers.py

@@ -0,0 +1,93 @@
+# 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 launch.substitutions import PathJoinSubstitution
+
+# Import submodule from this directory
+import sys
+import os
+sys.path.append(os.path.abspath(os.path.dirname(__file__)+"./"))
+from controllers_test_node import ControllersTestNode
+
+@launch_pytest.fixture
+def generate_test_description():
+    rosbot_controller = get_package_share_directory("rosbot_controller")
+    bringup_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            PathJoinSubstitution(
+                [
+                    rosbot_controller,
+                    "launch",
+                    "controller.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_controllers_startup_fail():
+    rclpy.init()
+    try:
+        node = ControllersTestNode('test_controllers_bringup')
+        node.create_test_subscribers_and_publishers()
+
+        node.start_node_thread()
+        msgs_received_flag = node.joint_state_msg_event.wait(timeout=10.0)
+        assert not msgs_received_flag, 'Received JointStates message, check joint_state_broadcaster!'
+        msgs_received_flag = node.odom_msg_event.wait(timeout=10.0)
+        assert not msgs_received_flag, 'Received Odom message, check rosbot_base_controller!'
+        msgs_received_flag = node.joint_state_msg_event.wait(timeout=10.0)
+        assert not msgs_received_flag, 'Received Imu message, check imu_broadcaster!'
+    finally:
+        rclpy.shutdown()
+
+
+@pytest.mark.launch(fixture=generate_test_description)
+def test_controllers_startup_success():
+    rclpy.init()
+    try:
+        node = ControllersTestNode('test_controllers_bringup')
+        node.create_test_subscribers_and_publishers()
+        node.start_publishing_fake_hardware()
+
+        node.start_node_thread()
+        msgs_received_flag = node.joint_state_msg_event.wait(timeout=10.0)
+        assert msgs_received_flag, 'Did not receive JointStates message, check joint_state_broadcaster!'
+        msgs_received_flag = node.odom_msg_event.wait(timeout=10.0)
+        assert msgs_received_flag, 'Did not receive Odom message, check rosbot_base_controller!'
+        msgs_received_flag = node.joint_state_msg_event.wait(timeout=10.0)
+        assert msgs_received_flag, 'Did not receive Imu message, check imu_broadcaster!'
+    finally:
+        rclpy.shutdown()