Callback is not called when used with a templated calss method

edit

Hi!

I'm currently trying to build a custom solution for communicating with micro-controllers. I know that rosserial is widely used for that purpose, but I need something that is faster. I mainly need to have the publishers and the subscribers working as a proof of concept.

So far, I have the publishers working, but I can't seem to make the subscribers works.

My project consist of a catkin package with a C++ program that act as a bridge between ROS and the micro-controller. I want the program to look like this :

#include "msgs/byte.h" ///< Custom definition of a std_msgs/Byte message
#include "handle.h"


int main(int argc, char **argv)
{
    rosrawhid::Handle h(argc, argv, "uC");

    h.add_publisher<rosrawhid::Byte, std_msgs::Byte>("pub_byte", 1);
    h.add_subscriber<rosrawhid::Byte, std_msgs::Byte>("sub_byte", 1);

    h.spin();
}

The spin() method look like :

void Handle::spin() {
    while (ros::ok()) {
        ros::spinOnce(); ///< Spin ROS once

        int num = rawhid_recv(0, this->buffer, 64, -1); ///< Read RawHID

        if (num < 0) { ///< If nothing is receive
            this->rawhid_disconnect_counter++;
            if (this->rawhid_disconnect_counter > this->rawhid_max_disconnect) {
                rawhid::reconnect();
                this->rawhid_disconnect_counter = 0;
            }
        } else {
            this->publish(); ///< If something is received, publish it
            this->rawhid_disconnect_counter = 0;
        }
    }
}

And the add_subscriber() method looks like :

template<typename M, typename R>
void Handle::add_subscriber(std::string topic, int queue_size) {
    auto *sub = new Subscriber<M, R>(this->num_subs);
    this->nh->subscribe<R>(topic, queue_size, &Subscriber<M, R>::callback, sub);
    this->subscribers.push_back(sub);
    this->num_subs++;
}

With M and R being my custom message definition and the official ros message class, nh being a pointer to the ros::NodeHandle and subscribers being a vector of pointers to an abstract class AbstractSubscriber. This class and the Subscriber class are defined as such :

/// Abstract class definition ///
class AbstractSubscriber {
public:
    virtual ~AbstractSubscriber() = default;
};

/// Class definition ///
template<typename M, typename R>
class Subscriber : public AbstractSubscriber {
public:
    M *msg;
    int id;
    uint8_t buffer[64]{};

    explicit Subscriber(int identifier);

    void callback(const typename R::ConstPtr& message);
};

/// Class implementation ///
template<typename M, typename R>
Subscriber<M, R>::Subscriber(int identifier)
: msg(new M()), id(identifier) {
    ROS_INFO("Initialized sub %d", this->id);
}

template<typename M, typename R>
void Subscriber<M, R>::callback(const typename R::ConstPtr& message) {
    this->msg->msg = *message;
    this->msg->update_buffer(this->id, this->buffer);
    rawhid_send(0, this->buffer, 64, 0);
    ROS_INFO("Buffer sent!");
}

My problem is that, while everything compile, the callback does not get called. I don't know if this is a problem with roscpp or with C++ itself, but I've been trying to figure out how to solve it for days now.

I'm using ROS neotic on Ubuntu 20.04 on an x86, with kernel 5.8.0.

Any help would be appreciated.

Thanks a lot!

Bruno