fix(control_switcher): fix control switcher

This commit fixes the control_switcher, which has been broken after #183
was merged. It also improves the controller load behavoir in the launch
files.

panda_gazebo/cfg/controllers/effort_joint_trajectory_controllers.yaml → panda_gazebo/cfg/controllers/effort_joint_trajectory_controller.yaml

@@ -2,7 +2,7 @@
 # trajectory controllers that can control a joint effort hardware interface.
 # NOTE: Equal to Franka's 'effort_joint_trajectory_controller' but with a different 
 # goal tolerance (see https://github.com/frankaemika/franka_ros/issues/201).
-panda_arm_controller:
+panda_arm_trajectory_controller:
   type: effort_controllers/JointTrajectoryController
   joints:
     - panda_joint1

panda_gazebo/cfg/controllers/moveit_ros_control.yaml

@@ -2,7 +2,7 @@
 # Make controllers visible to MoveIt.
 move_group:
   controller_list:
-    - name: panda_arm_controller
+    - name: panda_arm_trajectory_controller
       action_ns: follow_joint_trajectory
       type: FollowJointTrajectory
       default: true

panda_gazebo/cfg/controllers/moveit_ros_control_gripper.yaml

@@ -2,7 +2,7 @@
 # Make controllers visible to MoveIt.
 move_group:
   controller_list:
-    - name: panda_arm_controller
+    - name: panda_arm_trajectory_controller
       action_ns: follow_joint_trajectory
       type: FollowJointTrajectory
       default: true

panda_gazebo/launch/load_controllers.launch.xml

@@ -18,21 +18,20 @@
 
   <!--Load controllers-->
   <!--  Joint position controllers-->
-  <group if="$(eval arg('control_type') == 'position')">
-    <include file="$(find panda_gazebo)/launch/load_joint_group_position_controller.launch.xml"/>
-    <!--  Load joint_trajectory controller in stopped mode
-            NOTE: This enabled us to set the initial robot pose using euclidean coordinates.
-        -->
-    <include file="$(find panda_gazebo)/launch/load_position_joint_trajectory_controller.launch.xml">
-      <arg name="stopped" value="true"/>
-    </include>
-  </group>
+  <include file="$(find panda_gazebo)/launch/load_joint_group_position_controller.launch.xml">
+    <arg name="start" value="$(eval arg('control_type') == 'position')"/>
+    <!--  Only initialze the controller-->
+    <arg name="load" value="false"/>
+  </include>
   <!--  Joint effort controllers-->
-  <group if="$(eval arg('control_type') == 'effort')">
-    <include file="$(find panda_gazebo)/launch/load_joint_group_effort_controller.launch.xml"/>
-  </group>
+  <include file="$(find panda_gazebo)/launch/load_joint_group_effort_controller.launch.xml">
+    <arg name="start" value="$(eval arg('control_type') == 'effort')"/>
+    <!--  Only initialze the controller-->
+    <arg name="load" value="false"/>
+  </include>
   <!--  Joint_trajectory controller-->
-  <include unless="$(eval arg('control_type') == 'position')" file="$(find panda_gazebo)/launch/load_effort_joint_trajectory_controller.launch.xml">
-    <arg name="stopped" value="$(eval arg('control_type') == 'effort')"/>
+  <include file="$(find panda_gazebo)/launch/load_effort_joint_trajectory_controller.launch.xml">
+    <arg name="load" value="$(eval arg('moveit'))"/>
+    <arg name="start" value="$(eval arg('control_type') == 'trajectory')"/>
   </include>
 </launch>

panda_gazebo/launch/load_effort_joint_trajectory_controller.launch.xml

@@ -1,12 +1,13 @@
-<!--Launch file that launches a effort joint trajectory controller-->
+<!--Launch file that initializes, loads (and starts) the panda_arm_trajectory_controller-->
 <launch>
-  <arg name="stopped" default="false" doc="Spawn the controllers in stoped mode"/>
+  <arg name="load" default="true" doc="Load the controller"/>
+  <arg name="start" default="false" doc="Start the controller"/>
 
   <!--Load controller parameters-->
-  <rosparam file="$(find panda_gazebo)/cfg/controllers/effort_joint_trajectory_controllers.yaml" command="load"/>
+  <rosparam file="$(find panda_gazebo)/cfg/controllers/effort_joint_trajectory_controller.yaml" command="load"/>
 
   <!--Load (and start) the controller-->
-  <arg if="$(arg stopped)" name="command_args" value="--stopped"/>
-  <arg unless="$(arg stopped)" name="command_args" value=""/>
-  <node name="effort_joint_trajectory_controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="$(arg command_args) panda_arm_controller"/>
+  <arg if="$(arg start)" name="command_args" value=""/>
+  <arg unless="$(arg start)" name="command_args" value="--stopped"/>
+  <node if="$(eval arg('load') or arg('start'))" name="effort_joint_trajectory_controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="$(arg command_args) panda_arm_trajectory_controller"/>
 </launch>

panda_gazebo/launch/load_joint_group_effort_controller.launch.xml

@@ -1,12 +1,13 @@
-<!--Launch file that launches a joint group effort controller-->
+<!--Launch file that initializes, loads (and starts) the panda_arm_joint_effort_controller-->
 <launch>
-  <arg name="stopped" default="false" doc="Spawn the controllers in stoped mode"/>
+  <arg name="load" default="true" doc="Load the controller"/>
+  <arg name="start" default="false" doc="Start the controller"/>
 
   <!--Load controller parameters-->
   <rosparam file="$(find panda_gazebo)/cfg/controllers/joint_group_effort_controller.yaml" command="load"/>
 
   <!--Load (and start) the controller-->
-  <arg if="$(arg stopped)" name="command_args" value="--stopped"/>
-  <arg unless="$(arg stopped)" name="command_args" value="" />
-  <node name="joint_effort_controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="$(arg command_args) panda_arm_joint_effort_controller"/>
+  <arg if="$(arg start)" name="command_args" value=""/>
+  <arg unless="$(arg start)" name="command_args" value="--stopped"/>
+  <node if="$(eval arg('load') or arg('start'))" name="joint_effort_controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="$(arg command_args) panda_arm_joint_effort_controller"/>
 </launch>

panda_gazebo/launch/load_joint_group_position_controller.launch.xml

@@ -1,12 +1,13 @@
-<!--Launch file that launches a joint group position controller-->
+<!--Launch file that initializes, loads (and starts) the panda_arm_joint_position_controller-->
 <launch>
-  <arg name="stopped" default="false" doc="Spawn the controllers in stoped mode"/>
+  <arg name="load" default="true" doc="Load the controller"/>
+  <arg name="start" default="false" doc="Start the controller"/>
 
   <!--Load controller parameters-->
   <rosparam file="$(find panda_gazebo)/cfg/controllers/joint_group_position_controller.yaml" command="load"/>
 
   <!--Load (and start) the controller-->
-  <arg if="$(arg stopped)" name="command_args" value="--stopped"/>
-  <arg unless="$(arg stopped)" name="command_args" value=""/>
-  <node name="joint_position_controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="$(arg command_args) panda_arm_joint_position_controller"/>
+  <arg if="$(arg start)" name="command_args" value=""/>
+  <arg unless="$(arg start)" name="command_args" value="--stopped"/>
+  <node if="$(eval arg('load') or arg('start'))" name="joint_position_controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="$(arg command_args) panda_arm_joint_position_controller"/>
 </launch>

panda_gazebo/src/panda_gazebo/core/control_server.py

@@ -87,7 +87,7 @@
 }
 ARM_POSITION_CONTROLLER = "panda_arm_joint_position_controller"
 ARM_EFFORT_CONTROLLER = "panda_arm_joint_effort_controller"
-ARM_TRAJ_CONTROLLER = "panda_arm_controller"
+ARM_TRAJ_CONTROLLER = "panda_arm_trajectory_controller"
 HAND_CONTROLLER = "franka_gripper"
 CONTROLLER_INFO_RATE = 1 / 10  # Rate [hz] for retrieving controller information.
 CONNECTION_TIMEOUT = 10  # Service connection timeout [s].
@@ -325,23 +325,26 @@ def __init__(  # noqa: C901
         # servers ##############################
         ########################################
         # NOTE: Here setup a new action service that serves as a wrapper around the
-        # original 'panda_arm_controller/follow_joint_trajectory'. By doing this
-        # we add the following features to the original action servers.
+        # original 'panda_arm_trajectory_controller/follow_joint_trajectory'. By doing
+        # this we add the following features to the original action servers.
         #   - The ability to send partial joint messages.
         #   - The ability to send joint trajectory messages that do not specify joints.
         #   - The ability to automatic generate a time axes when the create_time_axis
         #     field is set to True.
         if load_arm_follow_joint_trajectory_action:
-            # Connect to original 'panda_arm_controller/follow_joint_trajectory' action
-            # server.
+            # Connect to the 'panda_arm_trajectory_controller/follow_joint_trajectory'
+            # action server.
             rospy.logdebug(
-                "Connecting to 'panda_arm_controller/follow_joint_trajectory' action "
-                "service."
+                "Connecting to '{}' action service.".format(
+                    "panda_arm_trajectory_controller/follow_joint_trajectory"
+                )
             )
-            if action_server_exists("panda_arm_controller/follow_joint_trajectory"):
+            if action_server_exists(
+                "panda_arm_trajectory_controller/follow_joint_trajectory"
+            ):
                 # Connect to robot control action server.
                 self._arm_joint_traj_client = SimpleActionClient(
-                    "panda_arm_controller/follow_joint_trajectory",
+                    "panda_arm_trajectory_controller/follow_joint_trajectory",
                     control_msgs.FollowJointTrajectoryAction,
                 )
 
@@ -352,18 +355,18 @@ def __init__(  # noqa: C901
                 if not retval:
                     rospy.logwarn(
                         "No connection could be established with the "
-                        "'panda_arm_controller/follow_joint_trajectory' service. The "
-                        "Panda Robot Environment therefore can not use this action "
-                        "service to control the Panda Robot."
+                        "'panda_arm_trajectory_controller/follow_joint_trajectory' "
+                        "service. The Panda Robot Environment therefore can not use "
+                        "this action service to control the Panda Robot."
                     )
                 else:
                     self._arm_joint_traj_client_connected = True
             else:
                 rospy.logwarn(
                     "No connection could be established with the "
-                    "'panda_arm_controller/follow_joint_trajectory' service. The Panda "
-                    "Robot Environment therefore can not use this action service to "
-                    "control the Panda Robot."
+                    "'panda_arm_trajectory_controller/follow_joint_trajectory' "
+                    "service. The Panda Robot Environment therefore can not use this "
+                    "action service to control the Panda Robot."
                 )
 
             # Setup a new Panda arm joint trajectory action server.
@@ -489,7 +492,7 @@ def _wait_till_arm_control_done(
     def _create_arm_traj_action_server_msg(self, input_msg):  # noqa: C901
         """Converts the ``control_msgs.msg.FollowJointTrajectoryGoal`` message that
         is received by the ``panda_control_server`` follow joint trajectory wrapper
-        action servers into the right format for the original ``panda_arm_controller``
+        action servers into the right format for the original ``panda_arm_trajectory_controller``
         `follow_joint_trajectory <https://wiki.ros.org/joint_trajectory_action/>`_
         action server.
 
@@ -503,13 +506,13 @@ def _create_arm_traj_action_server_msg(self, input_msg):  # noqa: C901
                 message we want to validate.
 
         Returns:
-            dict: A dictionary containing the arm and hand panda_arm_controller
+            dict: A dictionary containing the arm and hand panda_arm_trajectory_controller
                 :control_msgs:`control_msgs.msg.FollowJointTrajectoryGoal <html/action/FollowJointTrajectory.html>`
                 messages.
 
         Raises:
             :obj:`panda_gazebo.exceptions.InputMessageInvalidError`: Raised when the
-                input_msg could not be converted into panda_arm_controller control
+                input_msg could not be converted into panda_arm_trajectory_controller control
                 messages.
         """  # noqa: E501
         input_joint_names = input_msg.trajectory.joint_names
@@ -1789,7 +1792,7 @@ def _arm_joint_traj_execute_cb(self, goal):
 
     def _arm_joint_traj_feedback_cb(self, feedback):
         """Relays the feedback messages from the original
-        ``panda_arm_controller/follow_joint_trajectory`` server to our to our
+        ``panda_arm_trajectory_controller/follow_joint_trajectory`` server to our to our
         ``panda_control_server/panda_arm/follow_joint_trajectory`` wrapper action
         server.
 
@@ -1802,9 +1805,10 @@ def _arm_joint_traj_feedback_cb(self, feedback):
     def _arm_joint_traj_preempt_cb(self):
         """Relays the preempt request made to the
         ``panda_control_server/panda_arm/follow_joint_trajectory`` action server wrapper
-        to the original ``panda_arm_controller/follow_joint_trajectory`` action server.
+        to the original ``panda_arm_trajectory_controller/follow_joint_trajectory``
+        action server.
         """
-        # Stop panda_arm_controller action server.
+        # Stop panda_arm_trajectory_controller action server.
         if self._arm_joint_traj_client.get_state() in [
             GoalStatus.PREEMPTING,
             GoalStatus.ACTIVE,