0
$\begingroup$

Rosanswers logo

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 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!


Originally posted by d_joshi on ROS Answers with karma: 16 on 2020-05-19

Post score: 0


Original comments

Comment by gvdhoorn on 2020-05-20:
"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(..).

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

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


Originally posted by d_joshi with karma: 16 on 2020-05-20

This answer was ACCEPTED on the original site

Post score: 0

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.