Could not load custom controller, controller type does not exist
Hello,
I'm a ROS novice trying to run a Gazebo simulation with a user defined Joint Position Controller and I'm experiencing some difficulties loading the controller.
I have the code running successfully on another machine and now I'm trying to migrate to a new computer. I duplicated the configuration as best I could but upon running my controller launch file, I get the following error which I'm not sure how to resolve:
[ERROR] [1569865552.085248704]: Could not load class 'user_joint_position_controller/userJointPositionController': MultiLibraryClassLoader: Could not create object of class type controller_ns::userJointPositionController as no factory exists for it. Make sure that the library exists and was explicitly loaded through MultiLibraryClassLoader::loadLibrary() [ERROR] [1569865552.085297045]: Could not load controller 'steering_position_controller' because controller type 'user_joint_position_controller/userJointPositionController' does not exist. [ERROR] [1569865552.085313300]: Use 'rosservice call controller_manager/list_controller_types to get the available types`
Here is the output of running rosservice call /toninus_rdt/controller_manager/list_controller_types
:
userjointposition_controller/userJointPositionController is there, 5th from the end
types: [ackermann_steering_controller/AckermannSteeringController, controller_manager_tests/DerivedController,
controller_manager_tests/EffortTestController, controller_manager_tests/ExtensibleController,
controller_manager_tests/MyDummyController, controller_manager_tests/PosEffController,
controller_manager_tests/PosEffOptController, controller_manager_tests/VelEffController,
diff_drive_controller/DiffDriveController, effort_controllers/GripperActionController,
effort_controllers/JointEffortController, effort_controllers/JointGroupEffortController,
effort_controllers/JointGroupPositionController, effort_controllers/JointPositionController,
effort_controllers/JointTrajectoryController, effort_controllers/JointVelocityController,
force_torque_sensor_controller/ForceTorqueSensorController, imu_sensor_controller/ImuSensorController,
joint_state_controller/JointStateController, pos_vel_acc_controllers/JointTrajectoryController,
pos_vel_controllers/JointTrajectoryController, position_controllers/GripperActionController,
position_controllers/JointGroupPositionController, position_controllers/JointPositionController,
position_controllers/JointTrajectoryController, user_joint_position_controller/userJointPositionController,
velocity_controllers/JointGroupVelocityController, velocity_controllers/JointPositionController,
velocity_controllers/JointTrajectoryController, velocity_controllers/JointVelocityController]
base_classes: ['controller_interface::ControllerBase', 'controller_interface::ControllerBase', 'controller_interface::ControllerBase',
'controller_interface::ControllerBase', 'controller_interface::ControllerBase',
'controller_interface::ControllerBase', 'controller_interface::ControllerBase',
'controller_interface::ControllerBase', 'controller_interface::ControllerBase',
'controller_interface::ControllerBase', 'controller_interface::ControllerBase',
'controller_interface::ControllerBase', 'controller_interface::ControllerBase',
'controller_interface::ControllerBase', 'controller_interface::ControllerBase',
'controller_interface::ControllerBase', 'controller_interface::ControllerBase',
'controller_interface::ControllerBase', 'controller_interface::ControllerBase',
'controller_interface::ControllerBase', 'controller_interface::ControllerBase',
'controller_interface::ControllerBase', 'controller_interface::ControllerBase',
'controller_interface::ControllerBase', 'controller_interface::ControllerBase',
'controller_interface::ControllerBase', 'controller_interface::ControllerBase',
'controller_interface::ControllerBase', 'controller_interface::ControllerBase',
'controller_interface::ControllerBase']
Here is my .yaml:
toninus_rdt:
# Publish all joint states -----------------------------------
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 50
# Position Controllers ---------------------------------------
steering_position_controller:
type: user_joint_position_controller/userJointPositionController
joint: steering
pid: {p: 300.0, i: 0.0, d: 7.5}
# Delay time = delayQueueLength * max_step_size from .world file
delayQueueLength: 1660
deadzoneThreshold: 0.0
rear_velocity_controller:
type: effort_controllers/JointVelocityController
joint: rear_wheel_joint
pid: {p: 40.0, i: 0.0, d: 1.00}
Here is the .launch file:
<?xml version="1.0" encoding="UTF-8"?> <launch>
<!-- Load joint controller configurations from YAML file to parameter server --> <rosparam file="$(find toninus_rear_asm)/control/config/toninus_rear_asm_control.yaml" command="load"/>
<node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" ns="/toninus_rdt" args="steering_position_controller rear_velocity_controller >joint_state_controller"/>
<!-- convert joint states to TF transforms for rviz, etc --> <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="false" output="screen"> <remap from="/joint_states" to="/toninus_rdt/joint_states" /> </node>
</launch>
Here is my controller_plugins.xml:
<library path="lib/libeffort_controllers">
<class name="user_joint_position_controller/userJointPositionController"
type="controller_ns::userJointPositionController"
base_class_type="controller_interface::ControllerBase">
</class>
</library>
Here is the controller source from userjointposition_controller.cpp, it's barely modified from the standard position controller :
#include <effort_controllers/user_joint_position_controller.h>
#include <angles/angles.h>
#include <pluginlib/class_list_macros.hpp>
#include <queue>
static std::queue<double> delayQueue;
static double deadzoneThreshold = 0.0;
namespace controller_ns {
userJointPositionController::userJointPositionController()
: loop_count_(0)
{}
userJointPositionController::~userJointPositionController()
{
sub_command_.shutdown();
}
bool userJointPositionController::init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n)
{
// Get joint name from parameter server
std::string joint_name;
if (!n.getParam("joint", joint_name))
{
ROS_ERROR("No joint given (namespace: %s)", n.getNamespace().c_str());
return false;
}
// Load parameters from yaml
int delayQueueLength = 0;
n.getParam("delayQueueLength", delayQueueLength);
n.getParam("deadzoneThreshold", deadzoneThreshold);
// Load PID Controller using gains set on parameter server
if (!pid_controller_.init(ros::NodeHandle(n, "pid")))
return false;
// Start realtime state publisher
controller_state_publisher_.reset(
new realtime_tools::RealtimePublisher<control_msgs::JointControllerState>(n, "state", 1));
// Start command subscriber
sub_command_ = n.subscribe<std_msgs::Float64>("command", 1, &userJointPositionController::setCommandCB, this);
// Get joint handle from hardware interface
joint_ = robot->getHandle(joint_name);
// Fill delay queue with zeros
for(int lv = 0; lv < delayQueueLength; lv++)
{
delayQueue.push(0.0);
}
std::cout << "queue initialized " << std::endl;
// Get URDF info about joint
urdf::Model urdf;
if (!urdf.initParamWithNodeHandle("robot_description", n))
{
ROS_ERROR("Failed to parse urdf file");
return false;
}
joint_urdf_ = urdf.getJoint(joint_name);
if (!joint_urdf_)
{
ROS_ERROR("Could not find joint '%s' in urdf", joint_name.c_str());
return false;
}
return true;
}
void userJointPositionController::setGains(const double &p, const double &i, const double &d, const double &i_max, const double &i_min, const bool &antiwindup)
{
pid_controller_.setGains(p,i,d,i_max,i_min,antiwindup);
}
void userJointPositionController::getGains(double &p, double &i, double &d, double &i_max, double &i_min, bool &antiwindup)
{
pid_controller_.getGains(p,i,d,i_max,i_min,antiwindup);
}
void userJointPositionController::getGains(double &p, double &i, double &d, double &i_max, double &i_min)
{
bool dummy;
pid_controller_.getGains(p,i,d,i_max,i_min,dummy);
}
void userJointPositionController::printDebug()
{
pid_controller_.printValues();
}
std::string userJointPositionController::getJointName()
{
return joint_.getName();
}
double userJointPositionController::getPosition()
{
return joint_.getPosition();
}
// Set the joint position command
void userJointPositionController::setCommand(double pos_command)
{
command_struct_.position_ = pos_command;
command_struct_.has_velocity_ = false; // Flag to ignore the velocity command since our setCommand method did not include it
// the writeFromNonRT can be used in RT, if you have the guarantee that
// * no non-rt thread is calling the same function (we're not subscribing to ros callbacks)
// * there is only one single rt thread
command_.writeFromNonRT(command_struct_);
}
// Set the joint position command with a velocity command as well
void userJointPositionController::setCommand(double pos_command, double vel_command)
{
command_struct_.position_ = pos_command;
command_struct_.velocity_ = vel_command;
command_struct_.has_velocity_ = true;
command_.writeFromNonRT(command_struct_);
}
void userJointPositionController::starting(const ros::Time& time)
{
double pos_command = joint_.getPosition();
// Make sure joint is within limits if applicable
enforceJointLimits(pos_command);
command_struct_.position_ = pos_command;
command_struct_.has_velocity_ = false;
command_.initRT(command_struct_);
pid_controller_.reset();
}
void userJointPositionController::update(const ros::Time& time, const ros::Duration& period)
{
command_struct_ = *(command_.readFromRT());
double command_position = command_struct_.position_;
double command_velocity = command_struct_.velocity_;
bool has_velocity_ = command_struct_.has_velocity_;
double error, vel_error;
double commanded_effort;
double current_position = joint_.getPosition();
// Delay input/reference command
delayQueue.push(command_position);
command_position = delayQueue.front();
delayQueue.pop();
// Make sure joint is within limits if applicable
enforceJointLimits(command_position);
// Compute position error
if (joint_urdf_->type == urdf::Joint::REVOLUTE)
{
angles::shortest_angular_distance_with_limits(
current_position,
command_position,
joint_urdf_->limits->lower,
joint_urdf_->limits->upper,
error);
}
else if (joint_urdf_->type == urdf::Joint::CONTINUOUS)
{
error = angles::shortest_angular_distance(current_position, command_position);
}
else //prismatic
{
error = command_position - current_position;
}
// Decide which of the two PID computeCommand() methods to call
if (has_velocity_)
{
// Compute velocity error if a non-zero velocity command was given
vel_error = command_velocity - joint_.getVelocity();
// Set the PID error and compute the PID command with nonuniform
// time step size. This also allows the user to pass in a precomputed derivative error.
commanded_effort = pid_controller_.computeCommand(error, vel_error, period);
}
else
{
// Set the PID error and compute the PID command with nonuniform
// time step size.
commanded_effort = pid_controller_.computeCommand(error, period);
}
// Deadzone
if(abs(commanded_effort) < deadzoneThreshold)
{
commanded_effort = 0.0;
}
else if (commanded_effort > 0.0)
{
commanded_effort -= deadzoneThreshold;
}
else
{
commanded_effort += deadzoneThreshold;
}
joint_.setCommand(commanded_effort);
// publish state
if (loop_count_ % 10 == 0)
{
if(controller_state_publisher_ && controller_state_publisher_->trylock())
{
controller_state_publisher_->msg_.header.stamp = time;
controller_state_publisher_->msg_.set_point = command_position;
controller_state_publisher_->msg_.process_value = current_position;
controller_state_publisher_->msg_.process_value_dot = joint_.getVelocity();
controller_state_publisher_->msg_.error = error;
controller_state_publisher_->msg_.time_step = period.toSec();
controller_state_publisher_->msg_.command = commanded_effort;
double dummy;
bool antiwindup;
getGains(controller_state_publisher_->msg_.p,
controller_state_publisher_->msg_.i,
controller_state_publisher_->msg_.d,
controller_state_publisher_->msg_.i_clamp,
dummy,
antiwindup);
controller_state_publisher_->msg_.antiwindup = static_cast<char>(antiwindup);
controller_state_publisher_->unlockAndPublish();
}
}
loop_count_++;
}
void userJointPositionController::setCommandCB(const std_msgs::Float64ConstPtr& msg)
{
setCommand(msg->data);
}
// Note: we may want to remove this function once issue https://github.com/ros/angles/issues/2 is resolved
void userJointPositionController::enforceJointLimits(double &command)
{
// Check that this joint has applicable limits
if (joint_urdf_->type == urdf::Joint::REVOLUTE || joint_urdf_->type == urdf::Joint::PRISMATIC)
{
if( command > joint_urdf_->limits->upper ) // above upper limnit
{
command = joint_urdf_->limits->upper;
}
else if( command < joint_urdf_->limits->lower ) // below lower limit
{
command = joint_urdf_->limits->lower;
}
}
}
} // namespace
PLUGINLIB_EXPORT_CLASS( controller_ns::userJointPositionController, controller_interface::ControllerBase)
I'm on Ubuntu 18.04.3 LTS and ROS Melodic
I'm not sure what's missing to make this work. Let me know if you need additional information, thanks in advance for your help.
Asked by Alex_36 on 2019-10-07 04:24:43 UTC
Comments
Could you please check formatting of your
.yaml
files? It doesn't appear to be valid yaml right now. That is not the cause of your problem -- most likely -- but would make it easier to understand what is going on.Asked by gvdhoorn on 2019-10-07 04:56:55 UTC
My bad, the formatting has been corrected.
Asked by Alex_36 on 2019-10-07 05:30:13 UTC
Just to doublecheck:
The namespace in your source code as well as the type in your plugin.xml do match?
Asked by mgruhler on 2019-10-07 06:09:34 UTC
I believe the namespaces match, I've included the plugin.xml and the controller source.
Asked by Alex_36 on 2019-10-07 07:22:48 UTC
Was this ever resolved?
Asked by jdcarp19 on 2019-10-14 12:05:04 UTC
Not yet, do you have a similar problem?
Asked by Alex_36 on 2019-10-15 04:41:53 UTC
Not sure about this, but I thought the PLUGIN_EXPORT_CLASS line is supposed to be inside your namespace?
Asked by d_joshi on 2020-05-19 20:32:12 UTC