Controller manager cannot load custom controller
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:
- A control_plugin xml file specifying the library path and the class name, type and base_class
- Dependency on controller_interface and the required <export> tags in the package.xml file
- Required changes made to the CMakeLists.txt file
- 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!
"Failed to initialize the controller" is printed when the
init(..)
method of your controller returnsfalse
.I would recommend to debug those lines (and their surroundings) in your controller which can return
false
ininit(..)
.