5. How To Add New Controllers

This repository is rather complex and adding a new robot controller involves editing many files! So to help others, and as a reminder to myself, here are all places you would need to edit to add a new controller. I did not create this structure of the repository (credit to the original author).

Let's say you want to add a new controller called ExampleController. You would implement the header and source file (in C++) as:

franka_ros_controllers/include/franka_ros_controllers/example_controller.h
franka_ros_controllers/src/example_controller.cpp

Within the franka_ros_controllers folder, there are three places where we add this new controller to the list of controllers:

1. In franka_ros_controllers/CMakeLists.txt under the command add_library(franka_ros_controllers, ...) add src/example_controller.cpp.
2. In franka_ros_controllers/config/ros_controllers.yaml, add the parameters. As an example, this could look like:

example_controller:
    type: franka_ros_controllers/ExampleController
    arm_id: panda
    joint_names:
        - panda_joint1
        - panda_joint2
        - panda_joint3
        - panda_joint4
        - panda_joint5
        - panda_joint6
        - panda_joint7

Based on your controller there may be other relevant parameters, such as gains.

3. In franka_ros_controllers/controller_plugins.xml add your controller to the list:

  <class name="franka_ros_controllers/ExampleController"
         type="franka_ros_controllers::ExampleController"
         base_class_type="controller_interface::ControllerBase">
    <description>
      Some description for your controller
    </description>
  </class>  

Within franka_interface there are six files to modify:

1. In franka_interface/config/robot_config.yaml under controllers_config: add the controller

example_controller: "franka_ros_interface/example_controller"

2. In franka_interface/launch/interface.launch, add the controller to the list of controllers loaded by the launch file:

 <node name="load_controllers" pkg="controller_manager" type="controller_manager" respawn="false"
	             output="screen" args="load
                                           ...
                                           franka_ros_interface/example_controller

3. In franka_interface/src/motion_controller_interface.cpp, there are multiple places where the controller must be added, to specify the default name. Rather than enumerating the several C++ edits, I recommend using the previously implemented controllers as an example of what needs to be added

4. Edit the corresponding header file, franka_interface/include/franka_interface/motion_controller_interface.h to include the variable for the controller name used in franka_interface/src/motion_controller_interface.cpp:

std::string example_controller_name_;

5. In franka_interaface/src/franka_tools/controller_manager.py, within the FrankaControllerManagerInterface class we add the controller to the controller manager by both adding the controller's parameters:

self._controller_names_from_rosparam = {
             ...
            'example_controller': _get_controller_name_from_rosparam_server('/controllers_config/example_controller'),

And adding a function for the controller:

    @property
    def example_controller(self):
        return self._controller_names_from_rosparam['example_controller']
        #return self._ns[1:] + "/example_pose_controller"

6. Finally, add the desired python interface in franka_interface/src/franka_interface/arm.py. For any new ROS messages, there publishers would be added. Within the controller's function, to switch to the new controller:

 if self._ctrl_manager.current_controller != self._ctrl_manager.example_controller: 
            self.switchToController(self._ctrl_manager.example_controller)