Ask Your Question
0

Controller manager cannot load custom controller

asked 2020-05-19 21:12:52 -0500

d_joshi gravatar image

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!

edit retag flag offensive close merge delete

Comments

"Failed to initialize the controller" is printed when the init(..) method of your controller returns false.

I would recommend to debug those lines (and their surroundings) in your controller which can return false in init(..).

gvdhoorn gravatar image gvdhoorn  ( 2020-05-20 03:17:10 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2020-05-20 05:33:53 -0500

d_joshi gravatar image

updated 2020-05-20 05:35:47 -0500

Ok this is embarrassing. Thank you very much indeed. It was simply a case of me not realizing that the init method had to return true. It wasn't returning anything at all. Thank you again

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2020-05-19 21:12:52 -0500

Seen: 251 times

Last updated: May 19 '20