Pass trajectory controller as parameter

Remapping actions doesn't work on ROS humble

Author: urfeex

ur_robot_driver/launch/ur_control.launch.py

@@ -163,11 +163,10 @@ def launch_setup(context):
         executable="trajectory_until_node",
         name="trajectory_until_node",
         output="screen",
-        remappings=[
-            (
-                "/motion_controller/follow_joint_trajectory",
-                f"/{initial_joint_controller.perform(context)}/follow_joint_trajectory",
-            ),
+        parameters=[
+            {
+                "motion_controller": initial_joint_controller,
+            },
         ],
     )
 

ur_robot_driver/src/trajectory_until_node.cpp

@@ -66,11 +66,14 @@ TrajectoryUntilNode::TrajectoryUntilNode(const rclcpp::NodeOptions& options)
   server_callback_group = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
   clients_callback_group = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
 
+  this->declare_parameter("motion_controller", "scaled_joint_trajectory_controller");
+  std::string motion_controller = this->get_parameter("motion_controller").as_string();
+
   // Initialize a trajectory action client, to a generic action that does not exist. This is remapped via ros-args when
   // launching the node
   trajectory_action_client_ = rclcpp_action::create_client<FollowJointTrajectory>(this,
-                                                                                  "/motion_controller/"
-                                                                                  "follow_joint_"
+                                                                                  motion_controller +
+                                                                                  "/follow_joint_"
                                                                                   "trajectory",
                                                                                   clients_callback_group);
 

ur_robot_driver/test/integration_test_trajectory_until.py

@@ -105,7 +105,8 @@ def setUp(self):
     # Tests
     #
 
-    def test_trajectory_with_tool_contact_no_trigger_succeeds(self, tf_prefix):
+    def test_trajectory_with_tool_contact_no_trigger_succeeds(self, tf_prefix, initial_joint_controller):
+        self._controller_manager_interface.wait_for_controller(initial_joint_controller)
         self.assertTrue(
             self._controller_manager_interface.switch_controller(
                 strictness=SwitchController.Request.BEST_EFFORT,
@@ -133,18 +134,13 @@ def test_trajectory_with_tool_contact_no_trigger_succeeds(self, tf_prefix):
         self.assertEqual(
             result.until_condition_result, FollowJointTrajectoryUntil.Result.NOT_TRIGGERED
         )
-        self.assertTrue(
-            self._controller_manager_interface.switch_controller(
-                strictness=SwitchController.Request.BEST_EFFORT,
-                deactivate_controllers=["tool_contact_controller"],
-            ).ok
-        )
 
-    def test_trajectory_until_can_cancel(self, tf_prefix):
+    def test_trajectory_until_can_cancel(self, tf_prefix, initial_joint_controller):
+        self._controller_manager_interface.wait_for_controller(initial_joint_controller)
         self.assertTrue(
             self._controller_manager_interface.switch_controller(
                 strictness=SwitchController.Request.BEST_EFFORT,
-                activate_controllers=["tool_contact_controller"],
+                activate_controllers=["tool_contact_controller", initial_joint_controller],
             ).ok
         )
         trajectory = JointTrajectory()
@@ -162,9 +158,3 @@ def test_trajectory_until_can_cancel(self, tf_prefix):
         self.assertTrue(goal_handle.accepted)
         result = self._trajectory_until_interface.cancel_goal(goal_handle)
         self.assertEqual(result.return_code, CancelGoal_Response.ERROR_NONE)
-        self.assertTrue(
-            self._controller_manager_interface.switch_controller(
-                strictness=SwitchController.Request.BEST_EFFORT,
-                deactivate_controllers=["tool_contact_controller"],
-            ).ok
-        )