Trouble loading Custom Controller with Suscriber

edit

Hi!

I have been trying to implement my own controller for a 6 wheel robot. I have been using various examples to implement the code, notably the Valkyrie and Packt tutorials, as well as the diff_drive_controller. I am running ROS_Melodic on Ubuntu 18.04 on a Jetson TX2. Using gazebo for the robot simulation.

When adding the suscribe line "sub_command_ = n.subscribe("/ground_truth/state", 1, &Controller_6202::ground_truth_Callback, this);" , the launch file fails to load the controller with the error:

[ERROR] [1607403971.165081452, 0.381000000]: Could not load class 'controller_6202_ns/Controller_6202': Failed to load library /home/polyorbite/catkin_ws/devel/lib//libcontroller_6202_lib.so. Make sure that you are calling the PLUGINLIB_EXPORT_CLASS macro in the library code, and that names are consistent between this macro and your XML. Error string: Could not load library (Poco exception = /home/polyorbite/catkin_ws/devel/lib//libcontroller_6202_lib.so: undefined symbol: _ZN18controller_6202_ns15Controller_620221ground_truth_CallbackERKN5boost10shared_ptrIKN8nav_msgs9Odometry_ISaIvEEEEE) [ERROR] [1607403971.165271469, 0.381000000]: Could not load controller 'controller_6202_name' because controller type 'controller_6202_ns/Controller_6202' does not exist

This is the cpp file implementation for the callback

//Ground_truth_Callback
void ground_truth_Callback(const nav_msgs::Odometry::ConstPtr& msg)
{

    [...]
}   
//Controller initialization
bool Controller_6202::init(hardware_interface::EffortJointInterface* hw, ros::NodeHandle &n)
{
    effortJointHandles.clear();

    //Retrieve the joint object to control
    std::vector<std::string> jointNames;


    if(n.getParam("joints", jointNames))
    {   
        if(hw)
        {
            for(unsigned int i=0; i < jointNames.size(); ++i)
            {
                //Ici, les roues gauches sont aux positions 0 à 3
                effortJointHandles.push_back(hw->getHandle(jointNames[i]));
            }

            buffer_command_effort.resize(effortJointHandles.size(), 0.0);
            buffer_current_positions.resize(effortJointHandles.size(), 0.0);
            buffer_current_velocities.resize(effortJointHandles.size(), 0.0);
            buffer_current_efforts.resize(effortJointHandles.size(), 0.0);
      }
      else
      {
          ROS_ERROR("Effort Joint Interface is empty in hardware interace.");
          return false;
      }
   }
   else
   {
      ROS_ERROR("No joints in given namespace: %s", n.getNamespace().c_str());
      return false;
   }

//Suscribe to ground truth
sub_command_ = n.subscribe("/ground_truth/state", 1, &Controller_6202::ground_truth_Callback, this);

return true;
}

When the suscribe line is removed, there is no error and the controller seems to work fine (without the subscription to the ground truth of course.

sub_command_ and ground_truth_Callback have both been defined in the .h file:

#pragma once

#include <controller_interface/controller.h>
#include <hardware_interface/joint_command_interface.h>
#include <pluginlib/class_list_macros.h>
#include <ros/node_handle.h>

#include <iostream>
#include <nav_msgs/Odometry.h>
#include <tf/tf.h>

namespace controller_6202_ns {

    class Controller_6202: public controller_interface::Controller<hardware_interface::EffortJointInterface>
    {
    public:
        //Controller_6202();
        //virtual ~Controller_6202();
        bool init(hardware_interface::EffortJointInterface* hw, ros::NodeHandle &n);
        void update(const ros::Time& time, const ros::Duration& period);
        void starting(const ros::Time& time);
        void stopping(const ros::Time& time);

        void ground_truth_Callback(const nav_msgs::Odometry::ConstPtr& msg);
        double * getTorque(const ros::Duration& period);

    private:
        std::vector<hardware_interface::JointHandle> effortJointHandles;
        std::vector<double> buffer_command_effort;
        std::vector<double> buffer_current_positions;
        std::vector<double> buffer_current_velocities;
        std::vector<double> buffer_current_efforts;
        //double u;
        //double v;
        //double r;
        ros::Subscriber sub_command_;
    };
}

My package.xml has these included:

    <buildtool_depend>catkin</buildtool_depend>
  <build_depend>controller_interface</build_depend>
  <build_depend>pluginlib</build_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>nav_msgs</build_depend>
  <build_depend>std_msgs</build_depend>

  <build_export_depend>controller_interface</build_export_depend>
  <build_export_depend>pluginlib</build_export_depend>
  <build_export_depend>roscpp</build_export_depend>
  <build_export_depend>nav_msgs</build_export_depend>
  <build_export_depend>std_msgs</build_export_depend>

  <exec_depend>controller_interface</exec_depend>
  <exec_depend>pluginlib</exec_depend>
  <exec_depend>roscpp</exec_depend>

  <exec_depend>std_msgs</exec_depend>
  <exec_depend>nav_msgs</exec_depend>

  <!-- The export tag contains other, unspecified, tags -->
  <export>
        <controller_interface plugin="${prefix}/controller_plugins.xml" />

  </export>

CMakeLists ... (more)