diff --git a/README.md b/README.md index 61d3799..d2f87a2 100644 --- a/README.md +++ b/README.md @@ -64,7 +64,7 @@ ros2 launch ur_robotiq_moveit_config ur_robotiq_moveit.launch.py use_fake_hardwa ### use_fake_hardware:=true Terminal 1.: ```bash -ros2 launch ur_robotiq_description ur_robotiq_control.launch.py robot_ip:=aaa.bbb.ccc.ddd use_fake_hardware:=true launch_rviz:=false initial_joint_controller:=joint_trajectory_controller +ros2 launch ur_robotiq_description ur_robotiq_control.launch.py robot_ip:=aaa.bbb.ccc.ddd use_fake_hardware:=true launch_rviz:=false ``` Terminal 2.: ```bash diff --git a/src/ur_robotiq_description/launch/ur_robotiq_control.launch.py b/src/ur_robotiq_description/launch/ur_robotiq_control.launch.py index 7b01ac9..b91cee9 100644 --- a/src/ur_robotiq_description/launch/ur_robotiq_control.launch.py +++ b/src/ur_robotiq_description/launch/ur_robotiq_control.launch.py @@ -25,7 +25,6 @@ def launch_setup(context, *args, **kwargs): use_fake_hardware = LaunchConfiguration("use_fake_hardware") fake_sensor_commands = LaunchConfiguration("fake_sensor_commands") controller_spawner_timeout = LaunchConfiguration("controller_spawner_timeout") - initial_joint_controller = LaunchConfiguration("initial_joint_controller") activate_joint_controller = LaunchConfiguration("activate_joint_controller") launch_rviz = LaunchConfiguration("launch_rviz") headless_mode = LaunchConfiguration("headless_mode") @@ -170,6 +169,11 @@ def launch_setup(context, *args, **kwargs): robot_description = {"robot_description": robot_description_content} + if use_fake_hardware.perform(context): + initial_joint_controller = "joint_trajectory_controller" + else: + initial_joint_controller = "scaled_joint_trajectory_controller" + initial_joint_controllers = PathJoinSubstitution( [FindPackageShare(description_package), "config", controllers_file] ) @@ -481,13 +485,7 @@ def generate_launch_description(): description="Timeout used when spawning controllers.", ) ) - declared_arguments.append( - DeclareLaunchArgument( - "initial_joint_controller", - default_value="scaled_joint_trajectory_controller", - description="Initially loaded robot controller.", - ) - ) + declared_arguments.append( DeclareLaunchArgument( "activate_joint_controller",