From c38a59af4ca94c008b3677e1ee1787a64bc6fea3 Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Date: Fri, 12 Nov 2021 13:08:06 +0100
Subject: [PATCH] Load ROS controllers

- Provide a transmission argument to switch between position, velocity, and effort controllers
- Implements https://github.com/frankaemika/franka_ros/pull/186#issuecomment-965338478
---
 config/simple_moveit_controllers.yaml              |  3 +--
 launch/demo.launch                                 | 14 +++++---------
 launch/demo_gazebo.launch                          |  2 +-
 launch/franka_control.launch                       |  5 ++---
 launch/move_group.launch                           |  4 +---
 launch/ros_controllers.launch                      |  7 +++----
 launch/simple_moveit_controller_manager.launch.xml |  5 +++--
 7 files changed, 16 insertions(+), 24 deletions(-)

diff --git a/config/simple_moveit_controllers.yaml b/config/simple_moveit_controllers.yaml
index ac804cc..454ca42 100644
--- a/config/simple_moveit_controllers.yaml
+++ b/config/simple_moveit_controllers.yaml
@@ -1,5 +1,5 @@
 controller_list:
-  - name: joint_trajectory_controller
+  - name: $(arg transmission)_joint_trajectory_controller
     action_ns: follow_joint_trajectory
     type: FollowJointTrajectory
     default: True
@@ -17,4 +17,3 @@ controller_list:
     default: True
     joints:
       - panda_finger_joint1
-
diff --git a/launch/demo.launch b/launch/demo.launch
index 05eb309..3a6db41 100644
--- a/launch/demo.launch
+++ b/launch/demo.launch
@@ -21,6 +21,8 @@
   <arg name="moveit_controller_manager" default="fake" />
   <!-- Set execution mode for fake execution controllers -->
   <arg name="fake_execution_type" default="interpolate" />
+  <!-- Transmission used for joint control: position, velocity, or effort -->
+  <arg name="transmission" />
 
   <!-- By default, hide joint_state_publisher's GUI in 'fake' controller_manager mode -->
   <arg name="use_gui" default="false" />
@@ -50,15 +52,9 @@
   </group>
 
   <!-- Run the main MoveIt executable without trajectory execution (we do not have controllers configured by default) -->
-  <include file="$(dirname)/move_group.launch">
-    <arg name="allow_trajectory_execution" value="true"/>
-    <arg name="moveit_controller_manager" value="$(arg moveit_controller_manager)" />
-    <arg name="fake_execution_type" value="$(arg fake_execution_type)"/>
-    <arg name="info" value="true"/>
-    <arg name="debug" value="$(arg debug)"/>
-    <arg name="pipeline" value="$(arg pipeline)"/>
-    <arg name="load_gripper" value="$(arg load_gripper)"/>
-    <arg name="load_robot_description" value="$(arg load_robot_description)"/>
+  <include file="$(dirname)/move_group.launch" pass_all_args="true">
+    <arg name="allow_trajectory_execution" value="true" />
+    <arg name="info" value="true" />
   </include>
 
   <!-- Run Rviz and load the default config to see the state of the move_group node -->
diff --git a/launch/demo_gazebo.launch b/launch/demo_gazebo.launch
index 2f63ceb..2907f8f 100644
--- a/launch/demo_gazebo.launch
+++ b/launch/demo_gazebo.launch
@@ -14,7 +14,7 @@
   <include file="$(find franka_gazebo)/launch/panda.launch" pass_all_args="true">
     <arg name="headless" value="$(eval not arg('gazebo_gui'))" />
     <arg name="use_gripper" default="$(arg load_gripper)" />
-    <arg name="controller" default="joint_trajectory_controller" />
+    <arg name="controller" default="$(arg transmission)_joint_trajectory_controller" />
   </include>
 
   <include file="$(dirname)/demo.launch" pass_all_args="true">
diff --git a/launch/franka_control.launch b/launch/franka_control.launch
index 9534461..efb0c20 100644
--- a/launch/franka_control.launch
+++ b/launch/franka_control.launch
@@ -2,9 +2,8 @@
 <launch>
   <!-- Launch real-robot control -->
   <include file="$(find franka_control)/launch/franka_control.launch" pass_all_args="true" />
-
-  <!-- Load joint-trajectory controller into ROS controller_manager -->
-  <node name="trajectory_controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="joint_trajectory_controller" />
+  <!-- By default use joint position controllers -->
+  <arg name="transmission" default="position" />
 
   <!-- as well as MoveIt demo -->
   <include file="$(dirname)/demo.launch" pass_all_args="true">
diff --git a/launch/move_group.launch b/launch/move_group.launch
index 1dc4a5b..7cb7e8e 100644
--- a/launch/move_group.launch
+++ b/launch/move_group.launch
@@ -74,10 +74,8 @@
   </group>
 
   <!-- Trajectory Execution Functionality -->
-  <include ns="move_group" file="$(dirname)/trajectory_execution.launch.xml" if="$(arg allow_trajectory_execution)">
+  <include ns="move_group" file="$(dirname)/trajectory_execution.launch.xml" pass_all_args="true" if="$(arg allow_trajectory_execution)">
     <arg name="moveit_manage_controllers" value="true" />
-    <arg name="moveit_controller_manager" value="$(arg moveit_controller_manager)" />
-    <arg name="fake_execution_type" value="$(arg fake_execution_type)" />
   </include>
 
   <!-- Sensors Functionality -->
diff --git a/launch/ros_controllers.launch b/launch/ros_controllers.launch
index 91a84de..0354d48 100644
--- a/launch/ros_controllers.launch
+++ b/launch/ros_controllers.launch
@@ -4,8 +4,7 @@
   <!-- Load joint controller configurations from YAML file to parameter server -->
   <rosparam file="$(find panda_moveit_config)/config/ros_controllers.yaml" command="load"/>
 
-  <!-- Load the controllers -->
-  <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
-    output="screen" args="franka_gripper "/>
-
+  <!-- Load and start the controllers -->
+  <node ns="/" name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen"
+        args="$(arg transmission)_joint_trajectory_controller franka_gripper" />
 </launch>
diff --git a/launch/simple_moveit_controller_manager.launch.xml b/launch/simple_moveit_controller_manager.launch.xml
index 5b18aca..3640908 100644
--- a/launch/simple_moveit_controller_manager.launch.xml
+++ b/launch/simple_moveit_controller_manager.launch.xml
@@ -3,6 +3,7 @@
   <param name="moveit_controller_manager" value="moveit_simple_controller_manager/MoveItSimpleControllerManager" />
 
   <!-- Load controller list to the parameter server -->
-  <rosparam file="$(find panda_moveit_config)/config/simple_moveit_controllers.yaml" />
-  <rosparam file="$(find panda_moveit_config)/config/ros_controllers.yaml" />
+  <rosparam subst_value="true" file="$(find panda_moveit_config)/config/simple_moveit_controllers.yaml" />
+  <!-- Start ROS controllers -->
+  <include file="$(dirname)/ros_controllers.launch" pass_all_args="true" />
 </launch>
-- 
GitLab