From a4382c98714009653c7faaf71aabf863ccb2a8e0 Mon Sep 17 00:00:00 2001 From: Jonathan Date: Tue, 28 Jan 2025 09:02:46 +0100 Subject: [PATCH 01/11] Add --remap argument to launch command Signed-off-by: Jonathan Signed-off-by: Markus Kramer --- ros2launch/ros2launch/api/api.py | 13 ++++++++++--- ros2launch/ros2launch/command/launch.py | 7 ++++++- 2 files changed, 16 insertions(+), 4 deletions(-) diff --git a/ros2launch/ros2launch/api/api.py b/ros2launch/ros2launch/api/api.py index 3bdb5bd1b..42f89620b 100644 --- a/ros2launch/ros2launch/api/api.py +++ b/ros2launch/ros2launch/api/api.py @@ -25,6 +25,7 @@ import launch from launch.frontend import Parser from launch.launch_description_sources import get_launch_description_from_any_launch_file +from launch_ros.actions import SetRemap class MultipleLaunchFilesError(Exception): @@ -145,7 +146,8 @@ def launch_a_launch_file( noninteractive=False, args=None, option_extensions={}, - debug=False + debug=False, + remap_rules=None ): """Launch a given launch file (by path) and pass it the given launch file arguments.""" for name in sorted(option_extensions.keys()): @@ -167,14 +169,19 @@ def launch_a_launch_file( parsed_launch_arguments = parse_launch_arguments(launch_file_arguments) # Include the user provided launch file using IncludeLaunchDescription so that the # location of the current launch file is set. - launch_description = launch.LaunchDescription([ + launch_description = launch.LaunchDescription() + if remap_rules is not None: + for remap_rule in remap_rules: + from_name, to_name = remap_rule.split(':=', maxsplit=1) + launch_description.add_action(SetRemap(src=from_name, dst=to_name)) + launch_description.add_action( launch.actions.IncludeLaunchDescription( launch.launch_description_sources.AnyLaunchDescriptionSource( launch_file_path ), launch_arguments=parsed_launch_arguments, ), - ]) + ) for name in sorted(option_extensions.keys()): result = option_extensions[name].prelaunch( launch_description, diff --git a/ros2launch/ros2launch/command/launch.py b/ros2launch/ros2launch/command/launch.py index 1767d6e35..549e41a29 100644 --- a/ros2launch/ros2launch/command/launch.py +++ b/ros2launch/ros2launch/command/launch.py @@ -102,6 +102,10 @@ def add_arguments(self, parser, cli_name): help=('Regex pattern for filtering which executables the --launch-prefix is applied ' 'to by matching the executable name.') ) + parser.add_argument( + '-r', '--remap', action='append', dest='remap_rules', + help=("Topics remapping rules, in the 'from:=to' form") + ) arg = parser.add_argument( 'package_name', help='Name of the ROS package which contains the launch file') @@ -175,5 +179,6 @@ def main(self, *, parser, args): noninteractive=args.noninteractive, args=args, option_extensions=self._option_extensions, - debug=args.debug + debug=args.debug, + remap_rules=args.remap_rules ) From c4c1193992ef3aa3add2882d45a1169b09a9bb5c Mon Sep 17 00:00:00 2001 From: Jonathan Date: Thu, 30 Jan 2025 08:30:37 +0100 Subject: [PATCH 02/11] Make --remap help more general Signed-off-by: Jonathan Signed-off-by: Markus Kramer --- ros2launch/ros2launch/command/launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros2launch/ros2launch/command/launch.py b/ros2launch/ros2launch/command/launch.py index 549e41a29..08caed1e6 100644 --- a/ros2launch/ros2launch/command/launch.py +++ b/ros2launch/ros2launch/command/launch.py @@ -104,7 +104,7 @@ def add_arguments(self, parser, cli_name): ) parser.add_argument( '-r', '--remap', action='append', dest='remap_rules', - help=("Topics remapping rules, in the 'from:=to' form") + help=("Remapping rules, in the 'from:=to' form") ) arg = parser.add_argument( 'package_name', From da2aebd79b95115cfb441f39562886f3e77f07e9 Mon Sep 17 00:00:00 2001 From: Markus Kramer Date: Thu, 6 Mar 2025 15:26:20 +0100 Subject: [PATCH 03/11] added test case Signed-off-by: Markus Kramer --- launch_ros/test/test_cli_remap.py | 62 +++++++++++++++++++++++++++++++ 1 file changed, 62 insertions(+) create mode 100644 launch_ros/test/test_cli_remap.py diff --git a/launch_ros/test/test_cli_remap.py b/launch_ros/test/test_cli_remap.py new file mode 100644 index 000000000..75c63ab7c --- /dev/null +++ b/launch_ros/test/test_cli_remap.py @@ -0,0 +1,62 @@ +#!/usr/bin/env python3 + +# Copyright 2025 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 unittest +import subprocess + +class TestRemapArgument(unittest.TestCase): + """Test the --remap command line argument for ros2 launch.""" + + def test_remap_argument(self): + """Test that the --remap argument correctly remaps topics.""" + try: + # Start the talker_listener launch file with remapping + launch_proc_remapped = subprocess.Popen( + ['ros2', 'launch', 'demo_nodes_cpp', 'talker_listener_launch.py', + '--remap', '/chatter:=/chatter_remapped'], + stdout=subprocess.PIPE, + stderr=subprocess.PIPE + ) + + # Run ros2 topic list to get the remapped topics + result = subprocess.run(['ros2', 'topic', 'list'], + stdout=subprocess.PIPE, + stderr=subprocess.PIPE, + text=True, + check=True) + remapped_topics = result.stdout.strip().split('\n') + + # Verify /chatter is NOT in the list + self.assertNotIn('/chatter', remapped_topics, + f"Did not expect to find /chatter after remapping. Topics: {remapped_topics}") + + # Verify /chatter_remapped IS in the list + self.assertIn('/chatter_remapped', remapped_topics, + f"Expected to find /chatter_remapped after remapping. Topics: {remapped_topics}") + + finally: + # Clean up + if 'launch_proc_remapped' in locals(): + launch_proc_remapped.terminate() + try: + launch_proc_remapped.wait(timeout=5) + except subprocess.TimeoutExpired: + launch_proc_remapped.kill() + launch_proc_remapped.wait() + + +if __name__ == '__main__': + unittest.main() \ No newline at end of file From 076398d436ebf349ccea8c421831b2799d3433cd Mon Sep 17 00:00:00 2001 From: Jonathan Date: Fri, 11 Apr 2025 08:42:00 +0200 Subject: [PATCH 04/11] Add last line feed Signed-off-by: Jonathan --- launch_ros/test/test_cli_remap.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/launch_ros/test/test_cli_remap.py b/launch_ros/test/test_cli_remap.py index 75c63ab7c..3b3971eb7 100644 --- a/launch_ros/test/test_cli_remap.py +++ b/launch_ros/test/test_cli_remap.py @@ -59,4 +59,5 @@ def test_remap_argument(self): if __name__ == '__main__': - unittest.main() \ No newline at end of file + unittest.main() + \ No newline at end of file From d170b41f025e32987f6106b519a28bc79f728ef4 Mon Sep 17 00:00:00 2001 From: Jonathan Date: Fri, 11 Apr 2025 09:35:32 +0200 Subject: [PATCH 05/11] Format test file Signed-off-by: Jonathan --- launch_ros/test/test_cli_remap.py | 50 ++++++++++++++++++++----------- 1 file changed, 33 insertions(+), 17 deletions(-) diff --git a/launch_ros/test/test_cli_remap.py b/launch_ros/test/test_cli_remap.py index 3b3971eb7..2abdb2a6a 100644 --- a/launch_ros/test/test_cli_remap.py +++ b/launch_ros/test/test_cli_remap.py @@ -14,8 +14,9 @@ # See the License for the specific language governing permissions and # limitations under the License. -import unittest import subprocess +import unittest + class TestRemapArgument(unittest.TestCase): """Test the --remap command line argument for ros2 launch.""" @@ -25,31 +26,47 @@ def test_remap_argument(self): try: # Start the talker_listener launch file with remapping launch_proc_remapped = subprocess.Popen( - ['ros2', 'launch', 'demo_nodes_cpp', 'talker_listener_launch.py', - '--remap', '/chatter:=/chatter_remapped'], + [ + "ros2", + "launch", + "demo_nodes_cpp", + "talker_listener_launch.py", + "--remap", + "/chatter:=/chatter_remapped", + ], stdout=subprocess.PIPE, - stderr=subprocess.PIPE + stderr=subprocess.PIPE, ) # Run ros2 topic list to get the remapped topics - result = subprocess.run(['ros2', 'topic', 'list'], - stdout=subprocess.PIPE, - stderr=subprocess.PIPE, - text=True, - check=True) - remapped_topics = result.stdout.strip().split('\n') + result = subprocess.run( + ["ros2", "topic", "list"], + stdout=subprocess.PIPE, + stderr=subprocess.PIPE, + text=True, + check=True, + ) + remapped_topics = result.stdout.strip().split("\n") # Verify /chatter is NOT in the list - self.assertNotIn('/chatter', remapped_topics, - f"Did not expect to find /chatter after remapping. Topics: {remapped_topics}") + self.assertNotIn( + "/chatter", + remapped_topics, + f"Did not expect to find /chatter after remapping. Topics: { + remapped_topics}", + ) # Verify /chatter_remapped IS in the list - self.assertIn('/chatter_remapped', remapped_topics, - f"Expected to find /chatter_remapped after remapping. Topics: {remapped_topics}") + self.assertIn( + "/chatter_remapped", + remapped_topics, + f"Expected to find /chatter_remapped after remapping. Topics: { + remapped_topics}", + ) finally: # Clean up - if 'launch_proc_remapped' in locals(): + if "launch_proc_remapped" in locals(): launch_proc_remapped.terminate() try: launch_proc_remapped.wait(timeout=5) @@ -58,6 +75,5 @@ def test_remap_argument(self): launch_proc_remapped.wait() -if __name__ == '__main__': +if __name__ == "__main__": unittest.main() - \ No newline at end of file From 89c0ce0e1655fff8d8240f48d829d4961a6fa17b Mon Sep 17 00:00:00 2001 From: kramer-sim Date: Fri, 11 Apr 2025 09:50:19 +0200 Subject: [PATCH 06/11] added a sleep command and better error handling Signed-off-by: kramer-sim --- launch_ros/test/test_cli_remap.py | 30 ++++++++++++++++++------------ 1 file changed, 18 insertions(+), 12 deletions(-) diff --git a/launch_ros/test/test_cli_remap.py b/launch_ros/test/test_cli_remap.py index 2abdb2a6a..2d9ace04b 100644 --- a/launch_ros/test/test_cli_remap.py +++ b/launch_ros/test/test_cli_remap.py @@ -1,5 +1,4 @@ #!/usr/bin/env python3 - # Copyright 2025 Open Source Robotics Foundation, Inc. # # Licensed under the Apache License, Version 2.0 (the "License"); @@ -13,9 +12,9 @@ # 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 subprocess import unittest +import time class TestRemapArgument(unittest.TestCase): @@ -23,6 +22,7 @@ class TestRemapArgument(unittest.TestCase): def test_remap_argument(self): """Test that the --remap argument correctly remaps topics.""" + launch_proc_remapped = None try: # Start the talker_listener launch file with remapping launch_proc_remapped = subprocess.Popen( @@ -37,36 +37,41 @@ def test_remap_argument(self): stdout=subprocess.PIPE, stderr=subprocess.PIPE, ) - + + # Give the nodes time to start and register their topics + time.sleep(5) + # Run ros2 topic list to get the remapped topics + # Using run without check=True to handle errors result = subprocess.run( ["ros2", "topic", "list"], stdout=subprocess.PIPE, stderr=subprocess.PIPE, text=True, - check=True, ) + + # Check if the command was successful + if result.returncode != 0: + self.fail(f"'ros2 topic list' command failed with error: {result.stderr}") + remapped_topics = result.stdout.strip().split("\n") - + # Verify /chatter is NOT in the list self.assertNotIn( "/chatter", remapped_topics, - f"Did not expect to find /chatter after remapping. Topics: { - remapped_topics}", + f"Did not expect to find /chatter after remapping. Topics: {remapped_topics}", ) - + # Verify /chatter_remapped IS in the list self.assertIn( "/chatter_remapped", remapped_topics, - f"Expected to find /chatter_remapped after remapping. Topics: { - remapped_topics}", + f"Expected to find /chatter_remapped after remapping. Topics: {remapped_topics}", ) - finally: # Clean up - if "launch_proc_remapped" in locals(): + if launch_proc_remapped is not None: launch_proc_remapped.terminate() try: launch_proc_remapped.wait(timeout=5) @@ -77,3 +82,4 @@ def test_remap_argument(self): if __name__ == "__main__": unittest.main() + From 4d8b4514131a0be216411e3ecc13ec2a34ed07ed Mon Sep 17 00:00:00 2001 From: Markus Kramer Date: Wed, 23 Apr 2025 16:59:40 +0200 Subject: [PATCH 07/11] use of ros node to get available topics Signed-off-by: Markus Kramer --- launch_ros/test/test_cli_remap.py | 89 +++++++++++++++++++------------ 1 file changed, 55 insertions(+), 34 deletions(-) diff --git a/launch_ros/test/test_cli_remap.py b/launch_ros/test/test_cli_remap.py index 2d9ace04b..24cfab100 100644 --- a/launch_ros/test/test_cli_remap.py +++ b/launch_ros/test/test_cli_remap.py @@ -15,6 +15,8 @@ import subprocess import unittest import time +import rclpy +from rclpy.node import Node class TestRemapArgument(unittest.TestCase): @@ -37,40 +39,60 @@ def test_remap_argument(self): stdout=subprocess.PIPE, stderr=subprocess.PIPE, ) - - # Give the nodes time to start and register their topics - time.sleep(5) - - # Run ros2 topic list to get the remapped topics - # Using run without check=True to handle errors - result = subprocess.run( - ["ros2", "topic", "list"], - stdout=subprocess.PIPE, - stderr=subprocess.PIPE, - text=True, - ) - - # Check if the command was successful - if result.returncode != 0: - self.fail(f"'ros2 topic list' command failed with error: {result.stderr}") - - remapped_topics = result.stdout.strip().split("\n") - - # Verify /chatter is NOT in the list - self.assertNotIn( - "/chatter", - remapped_topics, - f"Did not expect to find /chatter after remapping. Topics: {remapped_topics}", - ) - - # Verify /chatter_remapped IS in the list - self.assertIn( - "/chatter_remapped", - remapped_topics, - f"Expected to find /chatter_remapped after remapping. Topics: {remapped_topics}", - ) + + # Initialize rclpy and create node for checking topics + rclpy.init() + node = None + try: + node = Node('__test_remap_checker', use_global_arguments=False) + + # Wait for the remapped topic to appear, poll for it + start_time = time.time() + timeout = 15.0 # seconds + remapped_topic_found = False + + while time.time() - start_time < timeout: + topic_names_and_types = node.get_topic_names_and_types() + current_topics = [name for name, + types in topic_names_and_types] + + if '/chatter_remapped' in current_topics: + remapped_topic_found = True + break # Found the target topic + + # Avoid busy-waiting + time.sleep(0.5) + + # Get the final list of topics after the loop/timeout + final_topic_names_and_types = node.get_topic_names_and_types() + final_topics = [name for name, + types in final_topic_names_and_types] + + # Verify /chatter_remapped IS in the list + self.assertTrue( + remapped_topic_found, + f"Expected topic '/chatter_remapped' not found within { + timeout}s. " + f"Final topics found: {final_topics}" + ) + + # Verify /chatter is NOT in the list + self.assertNotIn( + '/chatter', + final_topics, + f"Unexpectedly found original topic '/chatter'. Final topics: { + final_topics}" + ) + + finally: + # Clean up node and rclpy + if node is not None: + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + finally: - # Clean up + # Clean up the launch process if launch_proc_remapped is not None: launch_proc_remapped.terminate() try: @@ -82,4 +104,3 @@ def test_remap_argument(self): if __name__ == "__main__": unittest.main() - From 3555f8508fefe6799b3d7c3aafa518e7e37b31c5 Mon Sep 17 00:00:00 2001 From: Markus Kramer Date: Wed, 23 Apr 2025 17:08:35 +0200 Subject: [PATCH 08/11] changed formatting Signed-off-by: Markus Kramer --- launch_ros/test/test_cli_remap.py | 26 +++++++++++++------------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/launch_ros/test/test_cli_remap.py b/launch_ros/test/test_cli_remap.py index 24cfab100..bd00fe2a0 100644 --- a/launch_ros/test/test_cli_remap.py +++ b/launch_ros/test/test_cli_remap.py @@ -13,8 +13,9 @@ # See the License for the specific language governing permissions and # limitations under the License. import subprocess -import unittest import time +import unittest + import rclpy from rclpy.node import Node @@ -29,12 +30,12 @@ def test_remap_argument(self): # Start the talker_listener launch file with remapping launch_proc_remapped = subprocess.Popen( [ - "ros2", - "launch", - "demo_nodes_cpp", - "talker_listener_launch.py", - "--remap", - "/chatter:=/chatter_remapped", + 'ros2', + 'launch', + 'demo_nodes_cpp', + 'talker_listener_launch.py', + '--remap', + '/chatter:=/chatter_remapped', ], stdout=subprocess.PIPE, stderr=subprocess.PIPE, @@ -71,17 +72,16 @@ def test_remap_argument(self): # Verify /chatter_remapped IS in the list self.assertTrue( remapped_topic_found, - f"Expected topic '/chatter_remapped' not found within { - timeout}s. " - f"Final topics found: {final_topics}" + f'Expected topic "/chatter_remapped" not found within {timeout}s. ' + f'Final topics found: {final_topics}' ) # Verify /chatter is NOT in the list self.assertNotIn( '/chatter', final_topics, - f"Unexpectedly found original topic '/chatter'. Final topics: { - final_topics}" + f'Unexpectedly found original topic "/chatter". Final topics: { + final_topics}' ) finally: @@ -102,5 +102,5 @@ def test_remap_argument(self): launch_proc_remapped.wait() -if __name__ == "__main__": +if __name__ == '__main__': unittest.main() From 8ae0feff1c9071019bc4f1fffcd6b8fe2a56ffe4 Mon Sep 17 00:00:00 2001 From: Markus Kramer Date: Wed, 23 Apr 2025 17:25:33 +0200 Subject: [PATCH 09/11] added further debugging Signed-off-by: Markus Kramer --- launch_ros/test/test_cli_remap.py | 50 +++++++++++++++++++++++-------- 1 file changed, 37 insertions(+), 13 deletions(-) diff --git a/launch_ros/test/test_cli_remap.py b/launch_ros/test/test_cli_remap.py index bd00fe2a0..8a1e9a550 100644 --- a/launch_ros/test/test_cli_remap.py +++ b/launch_ros/test/test_cli_remap.py @@ -28,20 +28,24 @@ def test_remap_argument(self): launch_proc_remapped = None try: # Start the talker_listener launch file with remapping + # Note: Using shell=True to allow sourcing the setup file. + # Ensure ROS_DISTRO is set in the environment where this test runs. + command_str = ( + 'source /opt/ros/$ROS_DISTRO/setup.bash && ' + 'ros2 launch demo_nodes_cpp talker_listener_launch.py ' + '--remap /chatter:=/chatter_remapped' + ) launch_proc_remapped = subprocess.Popen( - [ - 'ros2', - 'launch', - 'demo_nodes_cpp', - 'talker_listener_launch.py', - '--remap', - '/chatter:=/chatter_remapped', - ], + command_str, stdout=subprocess.PIPE, stderr=subprocess.PIPE, + shell=True, # Execute through the shell + executable='/bin/bash' # Be explicit about the shell ) # Initialize rclpy and create node for checking topics + stdout_log = '' + stderr_log = '' rclpy.init() node = None try: @@ -69,11 +73,29 @@ def test_remap_argument(self): final_topics = [name for name, types in final_topic_names_and_types] + # Capture stdout and stderr before asserting + if launch_proc_remapped and not remapped_topic_found: + # Ensure the process is terminated before reading streams + if launch_proc_remapped.poll() is None: + launch_proc_remapped.terminate() + try: + launch_proc_remapped.wait(timeout=1) + except subprocess.TimeoutExpired: + if launch_proc_remapped.poll() is None: + launch_proc_remapped.kill() + launch_proc_remapped.wait(timeout=1) + # Now read the streams + stdout_bytes, stderr_bytes = launch_proc_remapped.communicate() + stdout_log = stdout_bytes.decode('utf-8', errors='replace') + stderr_log = stderr_bytes.decode('utf-8', errors='replace') + # Verify /chatter_remapped IS in the list self.assertTrue( remapped_topic_found, - f'Expected topic "/chatter_remapped" not found within {timeout}s. ' - f'Final topics found: {final_topics}' + f'Expected topic "/chatter_remapped" not found within {timeout}s.\n' + f'Final topics found: {final_topics}\n' + f'--- Launch STDOUT ---\n{stdout_log}\n' + f'--- Launch STDERR ---\n{stderr_log}' ) # Verify /chatter is NOT in the list @@ -93,13 +115,15 @@ def test_remap_argument(self): finally: # Clean up the launch process - if launch_proc_remapped is not None: + if launch_proc_remapped is not None and launch_proc_remapped.poll() is None: + # Ensure process is terminated if not already done for logging launch_proc_remapped.terminate() try: launch_proc_remapped.wait(timeout=5) except subprocess.TimeoutExpired: - launch_proc_remapped.kill() - launch_proc_remapped.wait() + if launch_proc_remapped.poll() is None: # Check again if terminate failed + launch_proc_remapped.kill() + launch_proc_remapped.wait() if __name__ == '__main__': From aef0f2eb186548fed0e3b769027bd9c6022fd4e4 Mon Sep 17 00:00:00 2001 From: Markus Kramer Date: Thu, 24 Apr 2025 10:39:31 +0200 Subject: [PATCH 10/11] directly test launch_a_launch_file Signed-off-by: Markus Kramer --- launch_ros/test/test_cli_remap.py | 232 ++++++++++++++++++++---------- 1 file changed, 159 insertions(+), 73 deletions(-) diff --git a/launch_ros/test/test_cli_remap.py b/launch_ros/test/test_cli_remap.py index 8a1e9a550..9ea9e1676 100644 --- a/launch_ros/test/test_cli_remap.py +++ b/launch_ros/test/test_cli_remap.py @@ -12,118 +12,204 @@ # 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 subprocess + +import logging +import os +import signal +import sys +import threading import time import unittest +from ament_index_python.packages import PackageNotFoundError import rclpy from rclpy.node import Node +from ros2launch.api.api import ( + get_share_file_path_from_package, + launch_a_launch_file, + MultipleLaunchFilesError +) + +# Configure basic logging +logging.basicConfig(level=logging.INFO, stream=sys.stderr, + format='%(asctime)s - %(levelname)s - [%(threadName)s] %(message)s') + class TestRemapArgument(unittest.TestCase): """Test the --remap command line argument for ros2 launch.""" def test_remap_argument(self): - """Test that the --remap argument correctly remaps topics.""" - launch_proc_remapped = None - try: - # Start the talker_listener launch file with remapping - # Note: Using shell=True to allow sourcing the setup file. - # Ensure ROS_DISTRO is set in the environment where this test runs. - command_str = ( - 'source /opt/ros/$ROS_DISTRO/setup.bash && ' - 'ros2 launch demo_nodes_cpp talker_listener_launch.py ' - '--remap /chatter:=/chatter_remapped' - ) - launch_proc_remapped = subprocess.Popen( - command_str, - stdout=subprocess.PIPE, - stderr=subprocess.PIPE, - shell=True, # Execute through the shell - executable='/bin/bash' # Be explicit about the shell - ) + """Test that the --remap argument correctly remaps topics using direct API call.""" + logging.info('Starting test_remap_argument...') + + checker_thread = None + # Flag to indicate test success from the checker thread + test_successful = threading.Event() + test_failed_assertion = None + + def check_topics_target(): + nonlocal test_failed_assertion + logging.info('Checker thread started.') # Initialize rclpy and create node for checking topics - stdout_log = '' - stderr_log = '' - rclpy.init() + logging.info('Checker thread: Initializing rclpy...') + # Ensure rclpy initializes in this thread context if necessary + # It might inherit context or need specific args depending on ROS setup + try: + rclpy.init() + except Exception as e: + logging.error(f'Checker thread: rclpy.init() failed: {e}', exc_info=True) + # Can't proceed without rclpy + os.kill(os.getpid(), signal.SIGINT) # Signal main thread to stop + return + node = None try: + logging.info('Checker thread: Creating node __test_remap_checker...') node = Node('__test_remap_checker', use_global_arguments=False) + logging.info('Checker thread: Node created.') # Wait for the remapped topic to appear, poll for it start_time = time.time() - timeout = 15.0 # seconds + timeout = 25.0 remapped_topic_found = False + iteration = 0 + logging.info('Checker thread: Starting topic polling loop...') while time.time() - start_time < timeout: + iteration += 1 + # Check if main thread is still alive (optional, sanity check) + topic_names_and_types = node.get_topic_names_and_types() - current_topics = [name for name, - types in topic_names_and_types] + current_topics = [name for name, types in topic_names_and_types] + logging.info(f'Poll {iteration}. Topic count: {len(current_topics)}') if '/chatter_remapped' in current_topics: + logging.info("Checker thread: Found target topic '/chatter_remapped'!") remapped_topic_found = True - break # Found the target topic + break - # Avoid busy-waiting time.sleep(0.5) - # Get the final list of topics after the loop/timeout - final_topic_names_and_types = node.get_topic_names_and_types() - final_topics = [name for name, - types in final_topic_names_and_types] - - # Capture stdout and stderr before asserting - if launch_proc_remapped and not remapped_topic_found: - # Ensure the process is terminated before reading streams - if launch_proc_remapped.poll() is None: - launch_proc_remapped.terminate() - try: - launch_proc_remapped.wait(timeout=1) - except subprocess.TimeoutExpired: - if launch_proc_remapped.poll() is None: - launch_proc_remapped.kill() - launch_proc_remapped.wait(timeout=1) - # Now read the streams - stdout_bytes, stderr_bytes = launch_proc_remapped.communicate() - stdout_log = stdout_bytes.decode('utf-8', errors='replace') - stderr_log = stderr_bytes.decode('utf-8', errors='replace') - - # Verify /chatter_remapped IS in the list - self.assertTrue( - remapped_topic_found, - f'Expected topic "/chatter_remapped" not found within {timeout}s.\n' - f'Final topics found: {final_topics}\n' - f'--- Launch STDOUT ---\n{stdout_log}\n' - f'--- Launch STDERR ---\n{stderr_log}' - ) + log_msg = f'Polling finished. Found remapped topic: {remapped_topic_found}' + logging.info(f'Checker thread: {log_msg}') - # Verify /chatter is NOT in the list - self.assertNotIn( - '/chatter', - final_topics, - f'Unexpectedly found original topic "/chatter". Final topics: { - final_topics}' - ) + final_topic_names_and_types = node.get_topic_names_and_types() + final_topics = [name for name, types in final_topic_names_and_types] + logging.info(f'Checker thread: Final topics: {final_topics}') + # Perform assertions within this thread using self.assertX methods + try: + logging.info('Checker thread: Asserting /chatter_remapped is present...') + msg = ( + f'Expected topic "/chatter_remapped" not found within {timeout}s. ' + f'Final topics: {final_topics}' + ) + self.assertTrue(remapped_topic_found, msg) + logging.info('Checker thread: Assertion passed.') + + logging.info('Checker thread: Asserting /chatter is NOT present...') + msg = ( + f'Unexpectedly found original topic "/chatter". ' + f'Final topics: {final_topics}' + ) + self.assertNotIn('/chatter', final_topics, msg) + logging.info('Checker thread: Assertion passed.') + + # If assertions pass, set the success flag + test_successful.set() + except AssertionError as e: + logging.error(f'Checker thread: Assertion failed: {e}') + test_failed_assertion = e # Store assertion for main thread reporting + except Exception as e: + # Catch and print any other exceptions from the launch thread + logging.error(f'Checker thread: Error during checks: {e}', exc_info=True) finally: # Clean up node and rclpy if node is not None: + logging.info('Checker thread: Destroying node...') node.destroy_node() if rclpy.ok(): + logging.info('Checker thread: Shutting down rclpy...') rclpy.shutdown() + logging.info('Checker thread: rclpy cleanup finished.') + + # Signal the main thread (running launch) to stop + log_msg = 'Signaling main thread (SIGINT) to stop launch service...' + logging.info(f'Checker thread: {log_msg}') + os.kill(os.getpid(), signal.SIGINT) + logging.info('Checker thread: Exiting.') + + # Main thread execution starts here + try: + logging.info('Main thread: Creating checker thread...') + # Make checker non-daemon so main thread waits for it via join() + checker_thread = threading.Thread(target=check_topics_target, daemon=False) + logging.info('Main thread: Starting checker thread...') + checker_thread.start() + launch_return_code = None + try: + # Find launch file (moved here from background thread) + logging.info('Main thread: Finding launch file...') + package = 'demo_nodes_cpp' + file = 'talker_listener_launch.py' + launch_file_path = get_share_file_path_from_package( + package_name=package, file_name=file + ) + logging.info(f'Main thread: Found launch file: {launch_file_path}') + + remap_rules = ['/chatter:=/chatter_remapped'] + + # Call launch_a_launch_file in the main thread + logging.info('Main thread: Calling launch_a_launch_file (blocking)...') + launch_return_code = launch_a_launch_file( + launch_file_path=launch_file_path, + launch_file_arguments=[], + noninteractive=True, + debug=False, + remap_rules=remap_rules + ) + log_msg = f'launch_a_launch_file returned with code: {launch_return_code}' + logging.info(f'Main thread: {log_msg}') + + except (PackageNotFoundError, FileNotFoundError, MultipleLaunchFilesError) as e: + logging.error(f'Main thread: Error finding launch file: {e}') + # Ensure checker thread is stopped if launch setup failed + if checker_thread and checker_thread.is_alive(): + log_msg = 'Signaling checker thread to stop due to launch error...' + logging.info(f'Main thread: {log_msg}') + # Ideally have a cleaner way, but SIGINT might work if thread handles it + # Or rely on join timeout below + except Exception as e: + logging.error(f'Main thread: Error during launch: {e}', exc_info=True) + finally: + # Wait for the checker thread to finish its checks and signal + logging.info('Main thread: Joining checker thread...') + if checker_thread is not None: + checker_thread.join(timeout=30.0) # Increased timeout to allow checks + if checker_thread.is_alive(): + logging.warning('Checker thread is still alive after timeout.') + else: + logging.info('Checker thread joined successfully.') + + # Outer try block needs a finally or except + # Re-adding the final check logic here which belongs to the outer try finally: - # Clean up the launch process - if launch_proc_remapped is not None and launch_proc_remapped.poll() is None: - # Ensure process is terminated if not already done for logging - launch_proc_remapped.terminate() - try: - launch_proc_remapped.wait(timeout=5) - except subprocess.TimeoutExpired: - if launch_proc_remapped.poll() is None: # Check again if terminate failed - launch_proc_remapped.kill() - launch_proc_remapped.wait() + # After launch and checker thread have finished, check assertion results + logging.info('Main thread: Checking test results...') + if test_failed_assertion: + # Re-raise the assertion failure captured from the checker thread + raise test_failed_assertion + elif not test_successful.is_set(): + # Fail if the checker thread didn't explicitly signal success + msg = 'Test failed: Checker thread did not signal success' + self.fail(msg) + else: + logging.info('Main thread: Test success confirmed by checker thread.') + + logging.info('Main thread: Test finished.') if __name__ == '__main__': From 56736957442fe470fcf404c46cd4aa37ab686785 Mon Sep 17 00:00:00 2001 From: Markus Kramer Date: Thu, 24 Apr 2025 15:09:50 +0200 Subject: [PATCH 11/11] moved script to new location Signed-off-by: Markus Kramer --- {launch_ros => ros2launch}/test/test_cli_remap.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) rename {launch_ros => ros2launch}/test/test_cli_remap.py (99%) diff --git a/launch_ros/test/test_cli_remap.py b/ros2launch/test/test_cli_remap.py similarity index 99% rename from launch_ros/test/test_cli_remap.py rename to ros2launch/test/test_cli_remap.py index 9ea9e1676..270147949 100644 --- a/launch_ros/test/test_cli_remap.py +++ b/ros2launch/test/test_cli_remap.py @@ -213,4 +213,4 @@ def check_topics_target(): if __name__ == '__main__': - unittest.main() + unittest.main() \ No newline at end of file