Controller manager cannot load custom controller

edit

Hello all, I created a bare-bones custom controller plugin to use with controller_manager. I am having trouble using the control plugin to control a robot joint in gazebo, or even load the controller at all. The joint works properly when using an existing ros-controller. However, when using my custom controller, the terminal window running gazebo comes up with :

Failed to initialize the controller

The plugin compiles successfully, the controller parameters load successfully, and the controller_manager even recognizes the new controller. Running rospack plugins --attrib=plugin controller_interface and rosservice call /controller_manager/list_controller_types list my controller type.

In the control plugin package, I have:

1. A control_plugin xml file specifying the library path and the class name, type and base_class
2. Dependency on controller_interface and the required <export> tags in the package.xml file
3. Required changes made to the CMakeLists.txt file
4. The plugin source file inside the src folder

Since the controller_manager seems to recognize the controller, I cannot help bu think there is something wrong with my source code. I will put it here for reference.

#include <controller_interface/controller.h>
#include <hardware_interface/joint_command_interface.h>
#include <pluginlib/class_list_macros.h>
#include <std_msgs/Float64.h>

namespace my_controller_ns {
class MyPositionController : public controller_interface::Controller<hardware_interface::PositionJointInterface> {

  bool init(hardware_interface::PositionJointInterface* hw, ros::NodeHandle& n) {

    std::string my_joint;
    if (!n.getParam("joint", my_joint)) {
      ROS_ERROR("Could not find joint name");
      return false;
    }
    joint_ = hw->getHandle(my_joint);
    command_ = joint_.getPosition();

    if (!n.getParam("gain", gain_)) {
      ROS_WARN("Could not find the gain parameter value. Using default");
      gain_ = 10.0;

      return true;
    }

    sub_command_ = n.subscribe("cmd_vel", 1, &MyPositionController::setCommandCB, this);
  }

  void update(const ros::Time &time, const ros::Duration &period) 
{
    double error = command_ - joint_.getPosition();
    double commanded_effort = error * gain_;
    joint_.setCommand(commanded_effort);
  }

  void setCommandCB(const std_msgs::Float64::ConstPtr &msg) 
    { 
    command_ = msg->data; 
    }

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

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

private:
  hardware_interface::JointHandle joint_;
  double gain_;
  double command_;
  ros::Subscriber sub_command_;
};
PLUGINLIB_EXPORT_CLASS(my_controller_ns::MyPositionController, controller_interface::ControllerBase);
} // namespace my_controller_ns

I am using ros melodic running on Ubuntu 18.04. Thanks in advance for your help!