diff --git a/docker/extras/.thumbs/256x256/spirit_uav_fire_academy.usd.png b/docker/extras/.thumbs/256x256/spirit_uav_fire_academy.usd.png new file mode 100644 index 00000000..a98a0890 Binary files /dev/null and b/docker/extras/.thumbs/256x256/spirit_uav_fire_academy.usd.png differ diff --git a/docker/extras/.thumbs/256x256/test_drone_world.usd.png b/docker/extras/.thumbs/256x256/test_drone_world.usd.png new file mode 100644 index 00000000..76098abc Binary files /dev/null and b/docker/extras/.thumbs/256x256/test_drone_world.usd.png differ diff --git a/docker/extras/spirit_uav_fire_academy.usd b/docker/extras/spirit_uav_fire_academy.usd new file mode 100644 index 00000000..af17efc4 Binary files /dev/null and b/docker/extras/spirit_uav_fire_academy.usd differ diff --git a/docker/extras/test_drone_world.usd b/docker/extras/test_drone_world.usd index 9c0305fe..e540687f 100644 Binary files a/docker/extras/test_drone_world.usd and b/docker/extras/test_drone_world.usd differ diff --git a/ros_ws/src/end2end_tests/launch/isaac_test_env_bringup.xml b/ros_ws/src/end2end_tests/launch/isaac_test_env_bringup.xml new file mode 100644 index 00000000..2c420d3d --- /dev/null +++ b/ros_ws/src/end2end_tests/launch/isaac_test_env_bringup.xml @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + diff --git a/ros_ws/src/end2end_tests/package.xml b/ros_ws/src/end2end_tests/package.xml index 7d492703..26dccd71 100644 --- a/ros_ws/src/end2end_tests/package.xml +++ b/ros_ws/src/end2end_tests/package.xml @@ -1,15 +1,17 @@ - robot_system_tests + end2end_tests 0.0.0 - Tests for the full robot system + Tests in simulation for full systems uav TODO: License declaration + ament_python + rclpy - + ament_copyright ament_flake8 ament_pep257 diff --git a/ros_ws/src/end2end_tests/setup.py b/ros_ws/src/end2end_tests/setup.py index 8213bb11..fdfe10aa 100644 --- a/ros_ws/src/end2end_tests/setup.py +++ b/ros_ws/src/end2end_tests/setup.py @@ -1,26 +1,28 @@ from setuptools import setup, find_packages -package_name = 'end2end_tests' +package_name = "end2end_tests" setup( - name=package_name, - version='0.0.0', - packages=find_packages(), -data_files=[ -('share/' + package_name, ['package.xml']), -('share/ament_index/resource_index/packages', - ['resource/' + package_name]), -], - install_requires=['setuptools', 'launch', 'launch_ros', 'launch_testing', 'launch_testing_ros', 'pytest'], - zip_safe=True, - maintainer='TODO', - maintainer_email='TODO', - description='TODO: Package description', - license='TODO: License declaration', - tests_require=['pytest'], -# entry_points={ -# 'console_scripts': [ -# 'my_node = my_py_pkg.my_node:main' -# ], -# }, -) \ No newline at end of file + name=package_name, + version="0.0.0", + packages=find_packages(), + data_files=[ + ("share/" + package_name, ["package.xml"]), + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name + "/launch", ["launch/isaac_test_env_bringup.xml"]), + ], + install_requires=[ + "setuptools", + "launch", + "launch_ros", + "launch_testing", + "launch_testing_ros", + "pytest", + ], + zip_safe=True, + maintainer="TODO", + maintainer_email="TODO", + description="Tests in simulation for full systems", + license="TODO: License declaration", + tests_require=["pytest"], +) diff --git a/ros_ws/src/end2end_tests/tests/end2end_tests.py b/ros_ws/src/end2end_tests/tests/end2end_tests.py deleted file mode 100644 index b74738e9..00000000 --- a/ros_ws/src/end2end_tests/tests/end2end_tests.py +++ /dev/null @@ -1,7 +0,0 @@ -# file to launch system tests in sim from test_drone_world.usd -# run with: python tests/system_tests.py - -import sys -import os -import time - diff --git a/ros_ws/src/end2end_tests/tests/test_end2end.py b/ros_ws/src/end2end_tests/tests/test_end2end.py new file mode 100644 index 00000000..d51965f1 --- /dev/null +++ b/ros_ws/src/end2end_tests/tests/test_end2end.py @@ -0,0 +1,107 @@ +import os +import sys +import time +import unittest +import uuid + +import launch +from launch.launch_service import LaunchService +import launch_ros +import launch_ros.actions +import launch_testing.actions +from launch_testing.io_handler import ActiveIoHandler +import launch_testing_ros + +import pytest + +import rclpy +from rclpy.node import Node + +from std_msgs.msg import String + +import time + +class End2EndTestNode(Node): + def __init__(self): + super().__init__('end2end_test_node') + self.subscription = self.create_subscription(String, '/sim_status', self.sim_status_callback, 10) + self.sim_status = None + + def sim_status_callback(self, msg): + self.sim_status = msg.data + self.get_logger().info(f'variable is now: {self.sim_status}') + +@pytest.mark.rostest +# this is the test descriptioon used to launch the full robot system with launch_robot_headless.yaml +def generate_test_description(): + robot_launch_path = '/root/AirStack/ros_ws/src/end2end_tests/launch/isaac_test_env_bringup.xml' + + robot_launch = launch.actions.IncludeLaunchDescription( launch.launch_description_sources.AnyLaunchDescriptionSource(robot_launch_path)) + + return ( + launch.LaunchDescription([ + robot_launch, + launch_testing.actions.ReadyToTest(), + ]), + {} + ) + + +class TestEnd2End(unittest.TestCase): + + @classmethod + def setUpClass(cls): + # Initialize the ROS context for the test node + rclpy.init() + + @classmethod + def tearDownClass(cls): + # Shutdown the ROS context + rclpy.shutdown() + + def setUp(self): + # Create a ROS node for tests + self.node = End2EndTestNode() + + def tearDown(self): + self.node.destroy_node() + + def test_wait_for_sim(self, timeout=30): + # Wait for the isaac sim to be available + start_time = time.time() + while time.time() - start_time < timeout: + rclpy.spin_once(self.node) + if self.node.sim_status == "ready": + self.assertTrue(True, "Simulation was initialized successfully") + return + self.assertTrue(False, "Timeout waiting for isaac sim to be ready") + + + # def test_arming(self): + # # Create a service call to existing "mavros/cmd/arming" service + # client = self.node.create_client(mavros_msgs.srv.CommandBool, '/mavros/cmd/arming') + # client.wait_for_service() + # request = mavros_msgs.srv.CommandBool.Request() + # request.value = True + # future = client.call_async(request) + # rclpy.spin_until_future_complete(self.node, future) + # response = future.result() + # self.assertTrue(response.success) + + # def test_disarming(self): + # # Create a service call to existing "mavros/cmd/arming" service + # client = self.node.create_client(mavros_msgs.srv.CommandBool, '/mavros/cmd/arming') + # client.wait_for_service() + # request = mavros_msgs.srv.CommandBool.Request() + # request.value = False + # future = client.call_async(request) + # rclpy.spin_until_future_complete(self.node, future) + # response = future.result() + # self.assertTrue(response.success) + + + + + + + diff --git a/ros_ws/src/robot/autonomy/5_behavior_managers/central/launch/ascent_isaac_omni_drone.xml b/ros_ws/src/robot/autonomy/5_behavior_managers/central/launch/ascent_isaac_omni_drone.xml index af3d3a87..33369e25 100644 --- a/ros_ws/src/robot/autonomy/5_behavior_managers/central/launch/ascent_isaac_omni_drone.xml +++ b/ros_ws/src/robot/autonomy/5_behavior_managers/central/launch/ascent_isaac_omni_drone.xml @@ -4,7 +4,7 @@ - + diff --git a/ros_ws/src/robot/autonomy/5_behavior_managers/central/launch/ascent_isaac_one_drone.xml b/ros_ws/src/robot/autonomy/5_behavior_managers/central/launch/ascent_isaac_one_drone.xml index 7f5742aa..e1c32690 100644 --- a/ros_ws/src/robot/autonomy/5_behavior_managers/central/launch/ascent_isaac_one_drone.xml +++ b/ros_ws/src/robot/autonomy/5_behavior_managers/central/launch/ascent_isaac_one_drone.xml @@ -4,7 +4,7 @@ - + diff --git a/ros_ws/src/robot/autonomy/5_behavior_managers/central/launch/ascent_isaac_two_drones.xml b/ros_ws/src/robot/autonomy/5_behavior_managers/central/launch/ascent_isaac_two_drones.xml index 5a5b1a11..232005fd 100644 --- a/ros_ws/src/robot/autonomy/5_behavior_managers/central/launch/ascent_isaac_two_drones.xml +++ b/ros_ws/src/robot/autonomy/5_behavior_managers/central/launch/ascent_isaac_two_drones.xml @@ -4,7 +4,7 @@ - + diff --git a/ros_ws/src/robot/robot_systems_tests/package.xml b/ros_ws/src/robot/robot_systems_tests/package.xml index 7d492703..f8ac19b7 100644 --- a/ros_ws/src/robot/robot_systems_tests/package.xml +++ b/ros_ws/src/robot/robot_systems_tests/package.xml @@ -3,7 +3,7 @@ robot_system_tests 0.0.0 - Tests for the full robot system + Robot system tests for one singular robot uav TODO: License declaration diff --git a/ros_ws/src/robot/robot_systems_tests/setup.py b/ros_ws/src/robot/robot_systems_tests/setup.py index f2d5d3fc..babcf5bb 100644 --- a/ros_ws/src/robot/robot_systems_tests/setup.py +++ b/ros_ws/src/robot/robot_systems_tests/setup.py @@ -1,26 +1,27 @@ from setuptools import setup, find_packages -package_name = 'robot_system_tests' +package_name = "robot_system_tests" setup( - name=package_name, - version='0.0.0', - packages=find_packages(), -data_files=[ -('share/' + package_name, ['package.xml']), -('share/ament_index/resource_index/packages', - ['resource/' + package_name]), -], - install_requires=['setuptools', 'launch', 'launch_ros', 'launch_testing', 'launch_testing_ros', 'pytest'], - zip_safe=True, - maintainer='TODO', - maintainer_email='TODO', - description='TODO: Package description', - license='TODO: License declaration', - tests_require=['pytest'], -# entry_points={ -# 'console_scripts': [ -# 'my_node = my_py_pkg.my_node:main' -# ], -# }, -) \ No newline at end of file + name=package_name, + version="0.0.0", + packages=find_packages(), + data_files=[ + ("share/" + package_name, ["package.xml"]), + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ], + install_requires=[ + "setuptools", + "launch", + "launch_ros", + "launch_testing", + "launch_testing_ros", + "pytest", + ], + zip_safe=True, + maintainer="TODO", + maintainer_email="TODO", + description="Robot system tests for one singular robot", + license="TODO: License declaration", + tests_require=["pytest"], +) diff --git a/ros_ws/src/simulation/isaacsim/CMakeLists.txt b/ros_ws/src/simulation/isaacsim/CMakeLists.txt index 968b4382..24b6527e 100644 --- a/ros_ws/src/simulation/isaacsim/CMakeLists.txt +++ b/ros_ws/src/simulation/isaacsim/CMakeLists.txt @@ -3,6 +3,8 @@ project(isaacsim) find_package(ament_cmake REQUIRED) find_package(rclpy REQUIRED) +find_package(std_msgs REQUIRED) + install(DIRECTORY scripts diff --git a/ros_ws/src/simulation/isaacsim/package.xml b/ros_ws/src/simulation/isaacsim/package.xml index d5f2f87a..0547b26f 100644 --- a/ros_ws/src/simulation/isaacsim/package.xml +++ b/ros_ws/src/simulation/isaacsim/package.xml @@ -21,6 +21,8 @@ ament_cmake + std_msgs + launch launch_ros diff --git a/ros_ws/src/simulation/isaacsim/scripts/run_isaacsim.py b/ros_ws/src/simulation/isaacsim/scripts/run_isaacsim.py index a50eb2a8..91ec33ef 100755 --- a/ros_ws/src/simulation/isaacsim/scripts/run_isaacsim.py +++ b/ros_ws/src/simulation/isaacsim/scripts/run_isaacsim.py @@ -10,6 +10,7 @@ import rclpy from rclpy.node import Node +from std_msgs.msg import String import argparse import os import subprocess @@ -17,6 +18,8 @@ import sys import atexit import psutil +import enum +import time # Default values defaults = { @@ -24,7 +27,7 @@ "isaac_sim_path": "", "use_internal_libs": False, "dds_type": "fastdds", - "gui": "", + "env_path": "", "standalone": "", "play_sim_on_start": False, "ros_distro_var": "foxy", @@ -33,6 +36,13 @@ "isaac_args": "", } +# Enum for simulation status +class sim_status_types(enum.Enum): + initializing = "initializing" + not_ready = "not_ready" + ready = "ready" + error = "error" + # List to keep track of subprocesses subprocesses = [] @@ -69,6 +79,12 @@ def update_env_vars(version_to_remove, specified_path_to_remove, env_var_name): class IsaacSimLauncherNode(Node): def __init__(self): super().__init__('isaac_sim_launcher_node') + + self.sim_status = sim_status_types.initializing + self.pub_sim_status = self.create_publisher(String, '/sim_status', 10) + self.pub_sim_status.publish(String(data=self.sim_status.value)) + self.timer = self.create_timer(1.0, self.timer_callback) + self.declare_parameters( namespace='', parameters=[ @@ -76,7 +92,7 @@ def __init__(self): ('install_path', defaults['isaac_sim_path']), ('use_internal_libs', defaults['use_internal_libs']), ('dds_type', defaults['dds_type']), - ('gui', defaults['gui']), + ('env_path', defaults['env_path']), ('standalone', defaults['standalone']), ('play_sim_on_start', defaults['play_sim_on_start']), ('ros_distro', defaults['ros_distro_var']), @@ -87,13 +103,17 @@ def __init__(self): ) self.execute_launch() + def timer_callback(self): + + self.pub_sim_status.publish(String(data=self.sim_status.value)) + def execute_launch(self): args = argparse.Namespace() args.version = self.get_parameter('version').get_parameter_value().string_value args.install_path = self.get_parameter('install_path').get_parameter_value().string_value args.use_internal_libs = self.get_parameter('use_internal_libs').get_parameter_value().bool_value args.dds_type = self.get_parameter('dds_type').get_parameter_value().string_value - args.gui = self.get_parameter('gui').get_parameter_value().string_value + args.env_path = self.get_parameter('env_path').get_parameter_value().string_value args.standalone = self.get_parameter('standalone').get_parameter_value().string_value args.play_sim_on_start = self.get_parameter('play_sim_on_start').get_parameter_value().bool_value args.ros_distro = self.get_parameter('ros_distro').get_parameter_value().string_value @@ -157,24 +177,42 @@ def execute_launch(self): # Default command executable_command = f'{os.path.join(filepath_root, "isaac-sim.sh" if sys.platform != "win32" else "isaac-sim.bat")} --/isaac/startup/ros_bridge_extension=omni.isaac.ros2_bridge' - # TODO make an additional args parameter for this - print(executable_command) - executable_command += ' ' + args.isaac_args#' --/app/scripting/ignoreWarningDialog=true' - if args.headless == "native": executable_command = f'{os.path.join(filepath_root, "isaac-sim.headless.native.sh" if sys.platform != "win32" else "isaac-sim.headless.native.bat")} --/isaac/startup/ros_bridge_extension=omni.isaac.ros2_bridge' elif args.headless == "webrtc": executable_command = f'{os.path.join(filepath_root, "isaac-sim.headless.webrtc.sh" if sys.platform != "win32" else "isaac-sim.headless.webrtc.bat")} --/isaac/startup/ros_bridge_extension=omni.isaac.ros2_bridge' + + # TODO make an additional args parameter for this + print(executable_command) + executable_command += ' ' + args.isaac_args #' --/app/scripting/ignoreWarningDialog=true' - if args.gui != "": + if args.env_path != "": script_dir = os.path.dirname(__file__) - file_arg = os.path.join(script_dir, "open_isaacsim_stage.py") + f" --path {args.gui} {play_sim_on_start_arg}" + file_arg = os.path.join(script_dir, "open_isaacsim_stage.py") + f" --path {args.env_path} {play_sim_on_start_arg}" command = f"{executable_command} --exec '{file_arg}'" - proc = subprocess.Popen(command, shell=True, start_new_session=True) + # proc = subprocess.Popen(command, shell=True, start_new_session=True) + proc = subprocess.Popen(command, shell=True, start_new_session=True, stdout=subprocess.PIPE, stderr=subprocess.PIPE) subprocesses.append(proc.pid) else: proc = subprocess.Popen(executable_command, shell=True, start_new_session=True) subprocesses.append(proc.pid) + + # continue checking to see if the sim is ready + + if args.headless == "native": + self.get_logger().info("Waiting for Isaac Sim to be ready...") + self.sim_status = sim_status_types.not_ready + while True: + try: + output = proc.stdout.readline() + if "Isaac Sim Headless Native App is loaded" in output.decode('utf-8'): + self.sim_status = sim_status_types.ready + self.get_logger().info("Isaac Sim is ready!") + break + # else: + # self.pub_sim_status.publish(String(data=sim_status_types.not_ready.value)) + except: + pass def main(args=None): rclpy.init(args=args)