About | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange

Create your own hardware interface

Description: How to implement ros_control for a custom robot

Tutorial Level: INTERMEDIATE

Next Tutorial: Loading and starting controllers through service calls

Setting up a new robot

This page walks you through the steps to set up a new robot to work with the controller manager. When you make your robot support one or more of the standard interfaces, you will be able to take advantage of a large library of controllers that work on the standard interfaces.

Using an existing interface

Suppose we have a robot with 2 joints: A & B. Both joints are position controlled. In this case, your robot should provide the standard JointPositionInterface and the JointStateInterface, so it can re-use all controllers that are already written to work with the JointPositionInterface and the JointStateInterface. The code would look like this:

#include <hardware_interface/joint_command_interface.h>
#include <hardware_interface/joint_state_interface.h>
#include <hardware_interface/robot_hw.h>

class MyRobot : public hardware_interface::RobotHW
{
public:
  MyRobot() 
 { 
   // connect and register the joint state interface
   hardware_interface::JointStateHandle state_handle_a("A", &pos[0], &vel[0], &eff[0]);
   jnt_state_interface.registerHandle(state_handle_a);

   hardware_interface::JointStateHandle state_handle_b("B", &pos[1], &vel[1], &eff[1]);
   jnt_state_interface.registerHandle(state_handle_b);

   registerInterface(&jnt_state_interface);

   // connect and register the joint position interface
   hardware_interface::JointHandle pos_handle_a(jnt_state_interface.getHandle("A"), &cmd[0]);
   jnt_pos_interface.registerHandle(pos_handle_a);

   hardware_interface::JointHandle pos_handle_b(jnt_state_interface.getHandle("B"), &cmd[1]);
   jnt_pos_interface.registerHandle(pos_handle_b);

   registerInterface(&jnt_pos_interface);
  }

private:
  hardware_interface::JointStateInterface jnt_state_interface;
  hardware_interface::PositionJointInterface jnt_pos_interface;
  double cmd[2];
  double pos[2];
  double vel[2];
  double eff[2];
};

That's it! So how is this code actually controlling your robot? The functions above are designed to give the controller manager (and the controllers inside the controller manager) access to the joint state of your robot, and to the commands of your robot. When the controller manager runs, the controllers will read from the pos, vel and eff variables in your robot, and the controller will write the desired command into the cmd variable. It's your job to make sure the pos, vel and eff variables always have the latest joint state available, and you also need to make sure that whatever is written into the cmd variable gets executed by the robot. To do this, you could for example add a read() and a write() function to your robot class. In your main(), you'd then do something like this:

main()
{
  MyRobot robot;
  controller_manager::ControllerManager cm(&robot);

  while (true)
  {
     robot.read();
     cm.update(robot.get_time(), robot.get_period());
     robot.write();
     sleep();
  }
}

As the image above suggests, you're of course not limited to inheriting from only one single interface. Your robot can provide as many interfaces as you like. Your robot could for example provide both the 'PositionJointInterface' and the 'VelocityJointInterface', and many more. Warning the hardware interface needs to load before the controller manager or the manager will likely crash.

Creating a robot-specific interface

The standard interfaces are pretty awesome if you don't want to write a whole new set of controllers for your robot, and you want to take advantage of the libraries of existing controllers. But what if your robot has some features that are not supported by the standard interfaces? Does that mean you can't use the standard interfaces at all? Guess what, turns out you can do both! You can use the standard interfaces (and re-use the standard controllers) for the features of your robot that are standard. And at the same time you can expose your robot-specific features in a robot-specific interface. Take another look at the image above, and see how it shows a robot with both standard and robot-specific interfaces.

What does the code look like for such a situation?

class MyRobot : public hardware_interface::RobotHW
{
public:
  MyRobot() 
  {
    // register the joint state and position interfaces
    ...
    ... (see the code above)
    ...

    // register some robot specific interfaces
    registerInterface(&cool_interface);
  }

  private:
    MyCustomInterface cool_interface;
};

or... you can register the MyRobot class itself:

class MyRobot : public hardware_interface::RobotHW, public hardware_interface::HardwareInterface
{
public:
  MyRobot() 
  {
    // register the joint state and position interfaces
    ...
    ... (see the code above)
    ...

    // register the MyRobot class itself to make the 'someCoolFunction' available
    // the MyRobot class inherits from HardwareInterface to make this possible
    registerInterface(this);
  }

  void someCoolFunction();
 
};

So the custom interfaces could be nothing more than adding any number of function calls to your robot class, and registering the robot class itself. These robot-specific functions will only be available to controllers that are specifically designed for your robot, but at the same time, your robot will still work with standard controllers using the standard interfaces of your robot.

Resource Management

The controller manager keeps track of which resource are in use by each of the controllers. A resource can be something like 'right_elbow_joint', 'base', 'left_arm', 'wrist_joints'. Pretty much anything you'd like to use for your specific robot. Resources are specified in the hardware interface. For example, the PositionJointInterface uses the joint names as resources. You can of course implement your own hardware interface, and define your own resources. When controllers are getting initialized, they request a number of resources from the hardware interface; these requests get recorded by the controller manager. So the controller manager knows exactly which controller has requested which resources.

The RobotHW class has a simple default implementation for resource management: it simply prevents two controllers that are using the same resource to be running at the same time. Note that two controllers that are using the same resource can be loaded at the same time, but can't be running at the same time. If this simple resource management scheme fits your robot, you don't need to do anything, the controller manager will automatically apply this scheme. If your robot needs a different scheme, you can easily create your own, by implementing one single function:

class MyRobot : public hardware_interface::RobotHW
{
public:
  MyRobot() 
  {
    // register hardware interfaces interfaces
    ...
    ... (see the code above)
    ...


    // Implement robot-specific resouce management
    bool checkForConflict(const std::list<ControllerInfo>& info) const
    {
       // this list of controllers cannot be running at the same time
       ... 
       return true;

       // this list of controller can be running at the same time
       ...
       return false;
    }
  }
};

The input to the checkForConflict method is a list of controller info objects. Each of these objects matches to one single controller, and contains all the info about that controller. This info includes the controller name, controller type, hardware interface type, and the list of resources that are claimed by the controller. Based on all this info, you can come up with your own scheme to decide if the given list of controllers is allowed to be running at the same time.