Added test to check behaviour

.github/workflows/ci.yaml

@@ -18,6 +18,8 @@ jobs:
         cd ..
         mkdir -p /home/ros2_ws/src
         cp -r gazebo_ros2_control /home/ros2_ws/src/
+        sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key  -o /usr/share/keyrings/ros-archive-keyring.gpg
+        echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2-testing/ubuntu $(lsb_release -cs) main" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null
         apt-get update && apt-get upgrade -q -y
         apt-get update && apt-get install -q -y --no-install-recommends \
           dirmngr \
@@ -33,11 +35,11 @@ jobs:
       run: |
         cd /home/ros2_ws/
         . /opt/ros/foxy/local_setup.sh
-        colcon build --packages-up-to gazebo_ros2_control_demos
+        colcon build --packages-up-to gazebo_ros2_control_demos gazebo_ros2_control_tests
     - name: Run tests
       id: test
       run: |
         cd /home/ros2_ws/
         . /opt/ros/foxy/local_setup.sh
-        colcon test --event-handlers console_direct+ --packages-select gazebo_ros2_control gazebo_ros2_control_demos
+        colcon test --event-handlers console_direct+ --packages-select gazebo_ros2_control gazebo_ros2_control_demos gazebo_ros2_control_tests
         colcon test-result

gazebo_ros2_control_tests/CMakeLists.txt

@@ -0,0 +1,16 @@
+cmake_minimum_required(VERSION 3.5)
+project(gazebo_ros2_control_tests)
+
+find_package(ament_cmake REQUIRED)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+
+  find_package(ament_cmake_gtest REQUIRED)
+  find_package(ament_cmake_pytest REQUIRED)
+
+  add_subdirectory(src/system)
+endif()
+
+ament_package()

gazebo_ros2_control_tests/package.xml

@@ -0,0 +1,34 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>gazebo_ros2_control_tests</name>
+  <version>0.0.2</version>
+  <description>Gazebo ros2 control tests</description>
+  <maintainer email="[email protected]">Alejandro Hernández</maintainer>
+  <license>Apache License 2.0</license>
+
+  <exec_depend>ament_index_python</exec_depend>
+  <exec_depend>control_msgs</exec_depend>
+  <exec_depend>controller_manager_msgs</exec_depend>
+  <exec_depend>gazebo_ros</exec_depend>
+  <exec_depend>gazebo_ros2_control</exec_depend>
+  <exec_depend>gazebo_ros2_control_demos</exec_depend>
+  <exec_depend>joint_trajectory_controller</exec_depend>
+  <exec_depend>launch</exec_depend>
+  <exec_depend>launch_ros</exec_depend>
+  <exec_depend>launch_testing</exec_depend>
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>robot_state_publisher</exec_depend>
+  <exec_depend>ros2controlcli</exec_depend>
+  <exec_depend>ros2_control</exec_depend>
+  <exec_depend>ros2_controllers</exec_depend>
+  <exec_depend>trajectory_msgs</exec_depend>
+  <exec_depend>velocity_controllers</exec_depend>
+  <exec_depend>xacro</exec_depend>
+
+  <test_depend>ament_cmake_gtest</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

gazebo_ros2_control_tests/src/system/CMakeLists.txt

@@ -0,0 +1,8 @@
+ament_add_test(test_position_controller
+  GENERATE_RESULT_FOR_RETURN_CODE_ZERO
+  COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_position_controller.py"
+  WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
+  TIMEOUT 20
+  ENV
+    TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR}
+)

gazebo_ros2_control_tests/src/system/test_position_controller.py

@@ -0,0 +1,62 @@
+#!/usr/bin/env python3
+# Copyright 2021 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.
+
+
+import os
+import sys
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch import LaunchService
+from launch.actions import ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+from launch_testing.legacy import LaunchTestService
+
+
+def generate_launch_description():
+
+    gazebo_dir = get_package_share_directory('gazebo_ros2_control_demos')
+    included_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(gazebo_dir + '/launch/cart_example_position.launch.py'),
+        launch_arguments={'paused': 'false',
+                          'physics': 'ode',
+                          'gui': 'false'}.items()
+        )
+
+    return LaunchDescription([
+        SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'),
+        included_launch,
+    ])
+
+
+def main(argv=sys.argv[1:]):
+    ld = generate_launch_description()
+
+    test1_action = ExecuteProcess(
+        cmd=[os.path.join(os.getenv('TEST_DIR'), 'tester_node.py')],
+        name='tester_node',
+        output='screen')
+
+    lts = LaunchTestService()
+    lts.add_test_action(ld, test1_action)
+    ls = LaunchService(argv=argv)
+    ls.include_launch_description(ld)
+    return lts.run(ls)
+
+
+if __name__ == '__main__':
+    sys.exit(main())

gazebo_ros2_control_tests/src/system/tester_node.py

@@ -0,0 +1,170 @@
+#! /usr/bin/env python3
+# Copyright 2021 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.
+
+import argparse
+import time
+
+from control_msgs.action import FollowJointTrajectory
+
+from controller_manager_msgs.srv import ListControllers
+
+import rclpy
+from rclpy.action import ActionClient
+from rclpy.executors import MultiThreadedExecutor
+from rclpy.node import Node
+
+from trajectory_msgs.msg import JointTrajectoryPoint
+
+
+class GazeboRos2ControlTester(Node):
+
+    def __init__(
+        self,
+        timeout: [60],
+    ):
+        super().__init__(node_name='GazeboRos2ControlTester')
+
+        self.timeout = timeout
+        self.executor = MultiThreadedExecutor()
+
+    def controller_list(self):
+        self.info_msg('Starting controller_list test')
+        controller_service = self.create_client(
+            ListControllers, '/controller_manager/list_controllers')
+        if not controller_service.wait_for_service(timeout_sec=5.0):
+            return False
+        req = ListControllers.Request()
+        future = controller_service.call_async(req)
+        while rclpy.ok():
+            rclpy.spin_once(self)
+            if future.done():
+                try:
+                    response = future.result()
+                except Exception as e:
+                    self.get_logger().info(
+                        'Service call failed %r' % (e,))
+                else:
+                    for controller in response.controller:
+                        if controller.state != 'active':
+                            return False
+                    return True
+            break
+
+    def send_goal(self):
+        points = []
+        point = JointTrajectoryPoint()
+        point.time_from_start.sec = 0
+        point.time_from_start.nanosec = 0
+        point.positions.append(0.0)
+        points.append(point)
+
+        point = JointTrajectoryPoint()
+        point.time_from_start.sec = 1
+        point.time_from_start.nanosec = 0
+        point.positions.append(1.0)
+        points.append(point)
+
+        goal_msg = FollowJointTrajectory.Goal()
+        point.time_from_start.sec = 1
+        point.time_from_start.nanosec = 0
+        goal_msg.trajectory.joint_names = ['slider_to_cart']
+        goal_msg.trajectory.points = points
+        return self.action_client.send_goal_async(goal_msg)
+
+    def position_controller_test(self):
+        self.info_msg('Starting position_controller_test')
+
+        self.action_client = ActionClient(self,
+                                          FollowJointTrajectory,
+                                          '/joint_trajectory_controller/follow_joint_trajectory')
+        if (not self.action_client.wait_for_server(timeout_sec=5)):
+            return False
+
+        future = self.send_goal()
+        rclpy.spin_until_future_complete(self, future, self.executor)
+        result = future.result()
+        self.info_msg(str(result))
+
+        if not result.accepted:
+            self.info_msg('Position Goal was not accepted')
+            return False
+        if result.status == 0:
+            return True
+        self.info_msg('Position status is not successful')
+        return False
+
+    def info_msg(self, msg: str):
+        self.get_logger().info('\033[1;37;44m' + msg + '\033[0m')
+
+    def warn_msg(self, msg: str):
+        self.get_logger().warn('\033[1;37;43m' + msg + '\033[0m')
+
+    def error_msg(self, msg: str):
+        self.get_logger().error('\033[1;37;41m' + msg + '\033[0m')
+
+
+def test_ArmVehicle(robot_tester):
+    return robot_tester.controller_list()
+
+
+def test_Position(robot_tester):
+    return robot_tester.position_controller_test()
+
+
+def run_all_tests(robot_tester):
+    # wait to set up everything
+    time.sleep(6)
+    result = True
+    if (result):
+        result = test_ArmVehicle(robot_tester)
+    if (result):
+        result = test_Position(robot_tester)
+
+    if (result):
+        robot_tester.info_msg('Test PASSED')
+    else:
+        robot_tester.error_msg('Test FAILED')
+
+    return result
+
+
+def get_testers(args):
+    testers = []
+    tester = GazeboRos2ControlTester(timeout=args.timeout)
+    testers.append(tester)
+    return testers
+
+
+def main(args=None):
+    parser = argparse.ArgumentParser(description='System-level drone demo tester node')
+    parser.add_argument('-t', '--timeout', action='append', nargs=1,
+                        metavar=('time_out'),
+                        help='Time out')
+
+    args_par, unknown = parser.parse_known_args()
+    rclpy.init(args=args)
+
+    # Create testers
+    testers = get_testers(args_par)
+
+    # run tests
+    for tester in testers:
+        passed = run_all_tests(tester)
+        if not passed:
+            exit(1)
+
+
+if __name__ == '__main__':
+    main()