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.