controller_interface

Writing a new controller

In this example, we're writing a new controller for the PR2 Robot, that tracks a commanded position. Since the PR2 Robot provides the `JointEffortInterface`, we'll make our controller use that interface. This means that our controller will be usable for all robots that provide the `JointEffortInterface`, not just for the PR2 Robot.

class PositionController : public Controller<JointEffortCommandInterface>
{
public:
  bool init(JointEffortCommandInterface* hw, ros::NodeHandle &n)
  {
    # get joint name from the parameter server
    std::string my_joint;
    if (!n.param('joint', my_joint)){
      ROS_ERROR("Could not find joint name");
      return false;
    }

    # get the joint object to use in the realtime loop
    joint_ = hw->getJointCommandEffort(my_joint);
    return true;
  }

  void update(const ros::Time& time)
  {
    double error = setpoint_ - joint_.getJointState().getPosition();
    joint_cmd_.setCommand(error*gain_);
  }

  void starting(const ros::Time& time) { }
  void stopping(const ros::Time& time) { }

private:
  JointEffortCommand joint_;
  static const double gain_ = 1.25;
  static const double setpoint_ = 3.00;
}