From 3119e359b63506d0045a24dbef0f0f0533cf3c01 Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Tue, 30 Mar 2021 17:34:39 +0000 Subject: [PATCH 1/3] Update launch files to use ros2 control spawner --- .../launch/run_move_group.launch.py | 34 ++++++------------- .../launch/run_moveit_cpp.launch.py | 34 ++++++------------- .../launch/run_move_group.launch.py | 34 ++++++------------- .../launch/pose_tracking_example.launch.py | 34 ++++++------------- .../launch/servo_cpp_interface_demo.launch.py | 34 ++++++------------- .../launch/servo_server_demo.launch.py | 34 ++++++------------- .../launch/servo_teleop.launch.py | 34 ++++++------------- .../test/launch/servo_launch_test_common.py | 34 ++++++------------- .../launch/test_servo_pose_tracking.test.py | 34 ++++++------------- .../launch/move_group_launch_test_common.py | 34 ++++++------------- 10 files changed, 100 insertions(+), 240 deletions(-) diff --git a/moveit_demo_nodes/run_move_group/launch/run_move_group.launch.py b/moveit_demo_nodes/run_move_group/launch/run_move_group.launch.py index b76b19d4df..87ddd9b511 100644 --- a/moveit_demo_nodes/run_move_group/launch/run_move_group.launch.py +++ b/moveit_demo_nodes/run_move_group/launch/run_move_group.launch.py @@ -157,30 +157,16 @@ def generate_launch_description(): }, ) - # load joint_state_controller - load_joint_state_controller = ExecuteProcess( - cmd=["ros2 control load_start_controller joint_state_controller"], - shell=True, - output="screen", - ) - load_controllers = [load_joint_state_controller] - # load panda_arm_controller - load_controllers += [ - ExecuteProcess( - cmd=["ros2 control load_configure_controller panda_arm_controller"], - shell=True, - output="screen", - on_exit=[ - ExecuteProcess( - cmd=[ - "ros2 control switch_controllers --start-controllers panda_arm_controller" - ], - shell=True, - output="screen", - ) - ], - ) - ] + # Load controllers + load_controllers = [] + for controller in ["panda_arm_controller", "joint_state_controller"]: + load_controllers += [ + ExecuteProcess( + cmd=["ros2 run controller_manager spawner.py {}".format(controller)], + shell=True, + output="screen", + ) + ] # Warehouse mongodb server mongodb_server_node = Node( diff --git a/moveit_demo_nodes/run_moveit_cpp/launch/run_moveit_cpp.launch.py b/moveit_demo_nodes/run_moveit_cpp/launch/run_moveit_cpp.launch.py index 4f92416e6b..0ce3a33210 100644 --- a/moveit_demo_nodes/run_moveit_cpp/launch/run_moveit_cpp.launch.py +++ b/moveit_demo_nodes/run_moveit_cpp/launch/run_moveit_cpp.launch.py @@ -142,30 +142,16 @@ def generate_launch_description(): }, ) - # load joint_state_controller - load_joint_state_controller = ExecuteProcess( - cmd=["ros2 control load_start_controller joint_state_controller"], - shell=True, - output="screen", - ) - load_controllers = [load_joint_state_controller] - # load panda_arm_controller - load_controllers += [ - ExecuteProcess( - cmd=["ros2 control load_configure_controller panda_arm_controller"], - shell=True, - output="screen", - on_exit=[ - ExecuteProcess( - cmd=[ - "ros2 control switch_controllers --start-controllers panda_arm_controller" - ], - shell=True, - output="screen", - ) - ], - ) - ] + # Load controllers + load_controllers = [] + for controller in ["panda_arm_controller", "joint_state_controller"]: + load_controllers += [ + ExecuteProcess( + cmd=["ros2 run controller_manager spawner.py {}".format(controller)], + shell=True, + output="screen", + ) + ] return LaunchDescription( [ diff --git a/moveit_demo_nodes/run_ompl_constrained_planning/launch/run_move_group.launch.py b/moveit_demo_nodes/run_ompl_constrained_planning/launch/run_move_group.launch.py index 2947f9e637..6749e41f4c 100644 --- a/moveit_demo_nodes/run_ompl_constrained_planning/launch/run_move_group.launch.py +++ b/moveit_demo_nodes/run_ompl_constrained_planning/launch/run_move_group.launch.py @@ -158,30 +158,16 @@ def generate_launch_description(): }, ) - # load joint_state_controller - load_joint_state_controller = ExecuteProcess( - cmd=["ros2 control load_start_controller joint_state_controller"], - shell=True, - output="screen", - ) - load_controllers = [load_joint_state_controller] - # load panda_arm_controller - load_controllers += [ - ExecuteProcess( - cmd=["ros2 control load_configure_controller panda_arm_controller"], - shell=True, - output="screen", - on_exit=[ - ExecuteProcess( - cmd=[ - "ros2 control switch_controllers --start-controllers panda_arm_controller" - ], - shell=True, - output="screen", - ) - ], - ) - ] + # Load controllers + load_controllers = [] + for controller in ["panda_arm_controller", "joint_state_controller"]: + load_controllers += [ + ExecuteProcess( + cmd=["ros2 run controller_manager spawner.py {}".format(controller)], + shell=True, + output="screen", + ) + ] # Warehouse mongodb server mongodb_server_node = Node( diff --git a/moveit_ros/moveit_servo/launch/pose_tracking_example.launch.py b/moveit_ros/moveit_servo/launch/pose_tracking_example.launch.py index a799c3d599..7b40259f3b 100644 --- a/moveit_ros/moveit_servo/launch/pose_tracking_example.launch.py +++ b/moveit_ros/moveit_servo/launch/pose_tracking_example.launch.py @@ -123,30 +123,16 @@ def generate_launch_description(): }, ) - # load joint_state_controller - load_joint_state_controller = ExecuteProcess( - cmd=["ros2 control load_start_controller joint_state_controller"], - shell=True, - output="screen", - ) - load_controllers = [load_joint_state_controller] - # load panda_arm_controller - load_controllers += [ - ExecuteProcess( - cmd=["ros2 control load_configure_controller panda_arm_controller"], - shell=True, - output="screen", - on_exit=[ - ExecuteProcess( - cmd=[ - "ros2 control switch_controllers --start-controllers panda_arm_controller" - ], - shell=True, - output="screen", - ) - ], - ) - ] + # Load controllers + load_controllers = [] + for controller in ["panda_arm_controller", "joint_state_controller"]: + load_controllers += [ + ExecuteProcess( + cmd=["ros2 run controller_manager spawner.py {}".format(controller)], + shell=True, + output="screen", + ) + ] return LaunchDescription( [ diff --git a/moveit_ros/moveit_servo/launch/servo_cpp_interface_demo.launch.py b/moveit_ros/moveit_servo/launch/servo_cpp_interface_demo.launch.py index 2d900ffc99..903f9489d8 100644 --- a/moveit_ros/moveit_servo/launch/servo_cpp_interface_demo.launch.py +++ b/moveit_ros/moveit_servo/launch/servo_cpp_interface_demo.launch.py @@ -108,30 +108,16 @@ def generate_launch_description(): }, ) - # load joint_state_controller - load_joint_state_controller = ExecuteProcess( - cmd=["ros2 control load_start_controller joint_state_controller"], - shell=True, - output="screen", - ) - load_controllers = [load_joint_state_controller] - # load panda_arm_controller - load_controllers += [ - ExecuteProcess( - cmd=["ros2 control load_configure_controller panda_arm_controller"], - shell=True, - output="screen", - on_exit=[ - ExecuteProcess( - cmd=[ - "ros2 control switch_controllers --start-controllers panda_arm_controller" - ], - shell=True, - output="screen", - ) - ], - ) - ] + # Load controllers + load_controllers = [] + for controller in ["panda_arm_controller", "joint_state_controller"]: + load_controllers += [ + ExecuteProcess( + cmd=["ros2 run controller_manager spawner.py {}".format(controller)], + shell=True, + output="screen", + ) + ] return LaunchDescription( [rviz_node, static_tf, servo_node, ros2_control_node, robot_state_publisher] diff --git a/moveit_ros/moveit_servo/launch/servo_server_demo.launch.py b/moveit_ros/moveit_servo/launch/servo_server_demo.launch.py index 5f4af30be6..329412453d 100644 --- a/moveit_ros/moveit_servo/launch/servo_server_demo.launch.py +++ b/moveit_ros/moveit_servo/launch/servo_server_demo.launch.py @@ -84,30 +84,16 @@ def generate_launch_description(): }, ) - # load joint_state_controller - load_joint_state_controller = ExecuteProcess( - cmd=["ros2 control load_start_controller joint_state_controller"], - shell=True, - output="screen", - ) - load_controllers = [load_joint_state_controller] - # load panda_arm_controller - load_controllers += [ - ExecuteProcess( - cmd=["ros2 control load_configure_controller panda_arm_controller"], - shell=True, - output="screen", - on_exit=[ - ExecuteProcess( - cmd=[ - "ros2 control switch_controllers --start-controllers panda_arm_controller" - ], - shell=True, - output="screen", - ) - ], - ) - ] + # Load controllers + load_controllers = [] + for controller in ["panda_arm_controller", "joint_state_controller"]: + load_controllers += [ + ExecuteProcess( + cmd=["ros2 run controller_manager spawner.py {}".format(controller)], + shell=True, + output="screen", + ) + ] # Launch as much as possible in components container = ComposableNodeContainer( diff --git a/moveit_ros/moveit_servo/launch/servo_teleop.launch.py b/moveit_ros/moveit_servo/launch/servo_teleop.launch.py index eca723f20c..d376c6b069 100644 --- a/moveit_ros/moveit_servo/launch/servo_teleop.launch.py +++ b/moveit_ros/moveit_servo/launch/servo_teleop.launch.py @@ -82,30 +82,16 @@ def generate_launch_description(): }, ) - # load joint_state_controller - load_joint_state_controller = ExecuteProcess( - cmd=["ros2 control load_start_controller joint_state_controller"], - shell=True, - output="screen", - ) - load_controllers = [load_joint_state_controller] - # load panda_arm_controller - load_controllers += [ - ExecuteProcess( - cmd=["ros2 control load_configure_controller panda_arm_controller"], - shell=True, - output="screen", - on_exit=[ - ExecuteProcess( - cmd=[ - "ros2 control switch_controllers --start-controllers panda_arm_controller" - ], - shell=True, - output="screen", - ) - ], - ) - ] + # Load controllers + load_controllers = [] + for controller in ["panda_arm_controller", "joint_state_controller"]: + load_controllers += [ + ExecuteProcess( + cmd=["ros2 run controller_manager spawner.py {}".format(controller)], + shell=True, + output="screen", + ) + ] # Launch as much as possible in components container = ComposableNodeContainer( diff --git a/moveit_ros/moveit_servo/test/launch/servo_launch_test_common.py b/moveit_ros/moveit_servo/test/launch/servo_launch_test_common.py index ea2455b19f..8f18f1c671 100644 --- a/moveit_ros/moveit_servo/test/launch/servo_launch_test_common.py +++ b/moveit_ros/moveit_servo/test/launch/servo_launch_test_common.py @@ -92,30 +92,16 @@ def generate_servo_test_description( }, ) - # load joint_state_controller - load_joint_state_controller = ExecuteProcess( - cmd=["ros2 control load_start_controller joint_state_controller"], - shell=True, - output="screen", - ) - load_controllers = [load_joint_state_controller] - # load panda_arm_controller - load_controllers += [ - ExecuteProcess( - cmd=["ros2 control load_configure_controller panda_arm_controller"], - shell=True, - output="screen", - on_exit=[ - ExecuteProcess( - cmd=[ - "ros2 control switch_controllers --start-controllers panda_arm_controller" - ], - shell=True, - output="screen", - ) - ], - ) - ] + # Load controllers + load_controllers = [] + for controller in ["panda_arm_controller", "joint_state_controller"]: + load_controllers += [ + ExecuteProcess( + cmd=["ros2 run controller_manager spawner.py {}".format(controller)], + shell=True, + output="screen", + ) + ] # Component nodes for tf and Servo test_container = ComposableNodeContainer( diff --git a/moveit_ros/moveit_servo/test/launch/test_servo_pose_tracking.test.py b/moveit_ros/moveit_servo/test/launch/test_servo_pose_tracking.test.py index 52b993d00c..aaf2ea4245 100644 --- a/moveit_ros/moveit_servo/test/launch/test_servo_pose_tracking.test.py +++ b/moveit_ros/moveit_servo/test/launch/test_servo_pose_tracking.test.py @@ -91,30 +91,16 @@ def generate_servo_test_description(*args, gtest_name: SomeSubstitutionsType): }, ) - # load joint_state_controller - load_joint_state_controller = ExecuteProcess( - cmd=["ros2 control load_start_controller joint_state_controller"], - shell=True, - output="screen", - ) - load_controllers = [load_joint_state_controller] - # load panda_arm_controller - load_controllers += [ - ExecuteProcess( - cmd=["ros2 control load_configure_controller panda_arm_controller"], - shell=True, - output="screen", - on_exit=[ - ExecuteProcess( - cmd=[ - "ros2 control switch_controllers --start-controllers panda_arm_controller" - ], - shell=True, - output="screen", - ) - ], - ) - ] + # Load controllers + load_controllers = [] + for controller in ["panda_arm_controller", "joint_state_controller"]: + load_controllers += [ + ExecuteProcess( + cmd=["ros2 run controller_manager spawner.py {}".format(controller)], + shell=True, + output="screen", + ) + ] # Component nodes for tf and Servo test_container = ComposableNodeContainer( diff --git a/moveit_ros/planning_interface/test/launch/move_group_launch_test_common.py b/moveit_ros/planning_interface/test/launch/move_group_launch_test_common.py index 479cbc32ff..b115d04bb1 100644 --- a/moveit_ros/planning_interface/test/launch/move_group_launch_test_common.py +++ b/moveit_ros/planning_interface/test/launch/move_group_launch_test_common.py @@ -156,30 +156,16 @@ def generate_move_group_test_description(*args, gtest_name: SomeSubstitutionsTyp }, ) - # load joint_state_controller - load_joint_state_controller = ExecuteProcess( - cmd=["ros2 control load_start_controller joint_state_controller"], - shell=True, - output="screen", - ) - load_controllers = [load_joint_state_controller] - # load panda_arm_controller - load_controllers += [ - ExecuteProcess( - cmd=["ros2 control load_configure_controller panda_arm_controller"], - shell=True, - output="screen", - on_exit=[ - ExecuteProcess( - cmd=[ - "ros2 control switch_controllers --start-controllers panda_arm_controller" - ], - shell=True, - output="screen", - ) - ], - ) - ] + # Load controllers + load_controllers = [] + for controller in ["panda_arm_controller", "joint_state_controller"]: + load_controllers += [ + ExecuteProcess( + cmd=["ros2 run controller_manager spawner.py {}".format(controller)], + shell=True, + output="screen", + ) + ] # test executable ompl_constraint_test = launch_ros.actions.Node( From 52ef4193fdb809213b2be35d9fe2d9c8e058f854 Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Wed, 31 Mar 2021 21:36:17 +0000 Subject: [PATCH 2/3] Run the test after waiting 2 seconds to make sure the controllers are loaded --- .../moveit_servo/test/launch/servo_launch_test_common.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/moveit_ros/moveit_servo/test/launch/servo_launch_test_common.py b/moveit_ros/moveit_servo/test/launch/servo_launch_test_common.py index 8f18f1c671..bffde03bad 100644 --- a/moveit_ros/moveit_servo/test/launch/servo_launch_test_common.py +++ b/moveit_ros/moveit_servo/test/launch/servo_launch_test_common.py @@ -9,7 +9,7 @@ from launch_ros.actions import ComposableNodeContainer, Node from launch_ros.descriptions import ComposableNode from ament_index_python.packages import get_package_share_directory -from launch.actions import ExecuteProcess +from launch.actions import ExecuteProcess, TimerAction import xacro @@ -156,7 +156,7 @@ def generate_servo_test_description( ), ros2_control_node, test_container, - servo_gtest, + TimerAction(period=2.0, actions=[servo_gtest]), launch_testing.actions.ReadyToTest(), ] + load_controllers From 8406bbb19b38ac1dfdfa9dd4741a67cbdfedec67 Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Thu, 1 Apr 2021 13:23:45 +0000 Subject: [PATCH 3/3] Add timer action to all integration tests --- .../moveit_servo/test/launch/test_servo_pose_tracking.test.py | 4 ++-- .../test/launch/move_group_launch_test_common.py | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/moveit_ros/moveit_servo/test/launch/test_servo_pose_tracking.test.py b/moveit_ros/moveit_servo/test/launch/test_servo_pose_tracking.test.py index aaf2ea4245..68b53722c7 100644 --- a/moveit_ros/moveit_servo/test/launch/test_servo_pose_tracking.test.py +++ b/moveit_ros/moveit_servo/test/launch/test_servo_pose_tracking.test.py @@ -12,7 +12,7 @@ from launch import LaunchDescription from launch.some_substitutions_type import SomeSubstitutionsType from launch.substitutions import LaunchConfiguration, PathJoinSubstitution -from launch.actions import ExecuteProcess +from launch.actions import ExecuteProcess, TimerAction from ament_index_python.packages import get_package_share_directory @@ -148,7 +148,7 @@ def generate_servo_test_description(*args, gtest_name: SomeSubstitutionsType): ), ros2_control_node, test_container, - pose_tracking_gtest, + TimerAction(period=2.0, actions=[pose_tracking_gtest]), launch_testing.actions.ReadyToTest(), ] + load_controllers diff --git a/moveit_ros/planning_interface/test/launch/move_group_launch_test_common.py b/moveit_ros/planning_interface/test/launch/move_group_launch_test_common.py index b115d04bb1..b55b5e8ffa 100644 --- a/moveit_ros/planning_interface/test/launch/move_group_launch_test_common.py +++ b/moveit_ros/planning_interface/test/launch/move_group_launch_test_common.py @@ -5,7 +5,7 @@ import launch_testing import xacro from launch import LaunchDescription -from launch.actions import ExecuteProcess +from launch.actions import ExecuteProcess, TimerAction from launch.some_substitutions_type import SomeSubstitutionsType from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import ComposableNodeContainer, Node @@ -187,7 +187,7 @@ def generate_move_group_test_description(*args, gtest_name: SomeSubstitutionsTyp static_tf, robot_state_publisher, ros2_control_node, - ompl_constraint_test, + TimerAction(period=2.0, actions=[ompl_constraint_test]), launch_testing.actions.ReadyToTest(), ] + load_controllers