From 3b9348e8c3ac87f2908afe2e1f6ba6b80aa33e2d Mon Sep 17 00:00:00 2001 From: ahcorde Date: Wed, 16 Jun 2021 13:42:37 +0200 Subject: [PATCH 1/6] Added test to check behaviour Signed-off-by: ahcorde --- gazebo_ros2_control_tests/CMakeLists.txt | 16 ++ gazebo_ros2_control_tests/package.xml | 25 +++ .../src/system/CMakeLists.txt | 8 + .../src/system/test_position_controller.py | 62 +++++++ .../src/system/tester_node.py | 170 ++++++++++++++++++ 5 files changed, 281 insertions(+) create mode 100644 gazebo_ros2_control_tests/CMakeLists.txt create mode 100644 gazebo_ros2_control_tests/package.xml create mode 100644 gazebo_ros2_control_tests/src/system/CMakeLists.txt create mode 100755 gazebo_ros2_control_tests/src/system/test_position_controller.py create mode 100755 gazebo_ros2_control_tests/src/system/tester_node.py diff --git a/gazebo_ros2_control_tests/CMakeLists.txt b/gazebo_ros2_control_tests/CMakeLists.txt new file mode 100644 index 00000000..5e7c5e8b --- /dev/null +++ b/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() diff --git a/gazebo_ros2_control_tests/package.xml b/gazebo_ros2_control_tests/package.xml new file mode 100644 index 00000000..4bc7c876 --- /dev/null +++ b/gazebo_ros2_control_tests/package.xml @@ -0,0 +1,25 @@ + + + + gazebo_ros2_control_tests + 0.0.2 + Gazebo ros2 control tests + Alejandro Hernández + Apache License 2.0 + + ament_index_python + control_msgs + controller_manager_msgs + gazebo_ros2_control + launch + launch_ros + launch_testing + rclpy + trajectory_msgs + + ament_cmake_gtest + + + ament_cmake + + diff --git a/gazebo_ros2_control_tests/src/system/CMakeLists.txt b/gazebo_ros2_control_tests/src/system/CMakeLists.txt new file mode 100644 index 00000000..79a55e81 --- /dev/null +++ b/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} +) diff --git a/gazebo_ros2_control_tests/src/system/test_position_controller.py b/gazebo_ros2_control_tests/src/system/test_position_controller.py new file mode 100755 index 00000000..080775ed --- /dev/null +++ b/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()) diff --git a/gazebo_ros2_control_tests/src/system/tester_node.py b/gazebo_ros2_control_tests/src/system/tester_node.py new file mode 100755 index 00000000..8d54f661 --- /dev/null +++ b/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() From dd2b029631973abf65e00a595ae024c3a52846eb Mon Sep 17 00:00:00 2001 From: ahcorde Date: Wed, 16 Jun 2021 13:56:30 +0200 Subject: [PATCH 2/6] Added tests to CI Signed-off-by: ahcorde --- .github/workflows/ci.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 0317fcab..c1072884 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -33,11 +33,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 From bebed299260365f7c80ae1726cbfffb47ffd12c8 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Wed, 16 Jun 2021 14:05:20 +0200 Subject: [PATCH 3/6] Adde dependency Signed-off-by: ahcorde --- gazebo_ros2_control_tests/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/gazebo_ros2_control_tests/package.xml b/gazebo_ros2_control_tests/package.xml index 4bc7c876..ab48c347 100644 --- a/gazebo_ros2_control_tests/package.xml +++ b/gazebo_ros2_control_tests/package.xml @@ -11,6 +11,7 @@ control_msgs controller_manager_msgs gazebo_ros2_control + gazebo_ros2_control_demos launch launch_ros launch_testing From 39f9eb432a836a4b6495b4ecf7ff886245b5738d Mon Sep 17 00:00:00 2001 From: ahcorde Date: Wed, 16 Jun 2021 14:30:22 +0200 Subject: [PATCH 4/6] Added more dependencies Signed-off-by: ahcorde --- gazebo_ros2_control_tests/package.xml | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/gazebo_ros2_control_tests/package.xml b/gazebo_ros2_control_tests/package.xml index ab48c347..b47d8025 100644 --- a/gazebo_ros2_control_tests/package.xml +++ b/gazebo_ros2_control_tests/package.xml @@ -10,13 +10,21 @@ ament_index_python control_msgs controller_manager_msgs + gazebo_ros gazebo_ros2_control gazebo_ros2_control_demos + joint_trajectory_controller launch launch_ros launch_testing rclpy + robot_state_publisher + ros2controlcli + ros2_control + ros2_controllers trajectory_msgs + velocity_controllers + xacro ament_cmake_gtest From 8fe49a92c0f87432f98cb093904e40436fc7ee98 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Wed, 16 Jun 2021 17:19:21 +0200 Subject: [PATCH 5/6] Test with foxy testing Signed-off-by: ahcorde --- .github/workflows/ci.yaml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index c1072884..c0636c22 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -18,6 +18,7 @@ jobs: cd .. mkdir -p /home/ros2_ws/src cp -r gazebo_ros2_control /home/ros2_ws/src/ + 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 \ From 1a6e9990dd8b1c8cf4be6cf0d68230650098bb7e Mon Sep 17 00:00:00 2001 From: ahcorde Date: Wed, 16 Jun 2021 17:24:41 +0200 Subject: [PATCH 6/6] Test with foxy testing Signed-off-by: ahcorde --- .github/workflows/ci.yaml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index c0636c22..d9bf87c9 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -18,6 +18,7 @@ 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 \