Tutorial: Writing and Executing Your Own Controller

Users can now generate new controllers by inheriting the LegController abstract class (see existing examples in robot_driver/src/controllers).

Writing a new controller

To write your own controller and execute it on the robot, follow these steps:

1. Write your source code for your controller class in the robot_driver/sr/controllers and robot_driver/include/robot_driver folders (these instructions will assume you have created a controller class in controller.cpp with header file controller.h). You can look at joint_controller.cpp as an example. You will need to implement a computeLegCommandArray method specified in robot_driver/include/robot_driver/leg_controller.h that populates a LegCommandArray with the desired commands to the legs.
2. Update the CMakeLists.txt of robot_driver to include your source code in the project library (you can look for where joint_controller.cpp appears):

add_library(robot_driver src/controller.cpp)

3. Write a test for your code (at a bare minimum, call the constructor) and add it to the CMakeLists.txt:

catkin_add_gtest(controller_test test/test_controller.cpp)
target_link_libraries(controller_test example_package ${catkin_LIBRARIES})

4. Build your code and run the tests:

(In separate terminal) roslaunch quad_utils load_global_params.launch load_robot_params:=true
(In original terminal) cd ~/catkin_ws
catkin run_tests

5. Integrate your controller into robot_driver: Include your controller in robot_driver/include/robot_driver/robot_driver.h:

#include "robot_driver/include/controllers/controller.h"

Add a case for your controller in the RobotDriver constructor in robot_driver/src/robot_driver.cpp (look for where JointController appears):

...
} else if (controller_id_ == "controller") {
   leg_controller_ = std::make_shared<controller>();
...

If required, insert any additional calls to your controller where appropriate in robot_driver.cpp. Refer to JointController and, InverseDynamicsController for examples.

Test the controller

6. Test your controller in simulation - refer to Gazebo Simulation for additional arguments:

roslaunch quad_utils quad_gazebo.launch controller:=<your-controller-arg>