Subscriber to sensor_msgs::Image with class member function callback not triggered

asked 2019-09-29 13:56:29 -0500

I have created a singleton class as a ROS node. I want to subscribe to a sensor_msgs::Image topic (published by another node). I implemented a synchronization mechanism with std_srvs::Trigger as well. The idea is that as soon as the current node is ready to receive a new frame, it triggers the publisher which sends a frame over. This is then repeated until there are no more frames left to process. This mechanism works perfectly fine using rospy. Due to internal reasons I have to implement everything in roscpp as well. I've reached the point that my node sends the publisher a trigger to send over a new frame, which does this. However, the callback for receiving a new image is never called.

Here is the header of the class:


#pragma once

#include <ros/ros.h>
#include <std_srvs/Trigger.h>
#include <sensor_msgs/Image.h>

// Note: FakeVPClass implements a 'Singleton' pattern
class FakeVPClass
{
public:
    static FakeVPClass& get_instance();
    void run_vp();

private:
    void request_new_raw_frame();
    void cb_received_new_raw_frame(const sensor_msgs::Image::ConstPtr& img);

private:
    FakeVPClass();                       // Forbid explicit construction (singleton)
    ~FakeVPClass();                      // Forbid explicit destruction (singleton)
    FakeVPClass(FakeVPClass const &);    // Forbid copy constructor (singleton)
    void operator=(FakeVPClass const &); // Forbid implicit copy (singleton)

private:
    bool m_received_new_raw_frame;
    ros::NodeHandle*  m_node;
    ros::Rate*  m_rate;
    ros::ServiceClient* m_raw_img_srv;
    ros::Subscriber*    m_raw_camera_input_sub;
};

And here is the implementation:


#include "fake_vp_roscpp.h"
#include <stdlib.h>
#include <iostream>

FakeVPClass::FakeVPClass()
{
    std::cout << "FakeVPClass()" << std::endl;
    m_received_new_raw_frame          = false;

    m_node                            = new ros::NodeHandle();
    m_rate                            = new ros::Rate(10);     // 10hz == 100ms

    // Registering to ROS service(s)
    ros::service::waitForService("/request_new_raw_frame");
    m_raw_img_srv = new ros::ServiceClient(m_node->serviceClient<std_srvs::Trigger>("/request_new_raw_frame"));

    // ROS subscriber(s)
    m_raw_camera_input_sub = new ros::Subscriber(m_node->subscribe<sensor_msgs::Image>("raw_camera_input", 1, &FakeVPClass::cb_received_new_raw_frame,    this));

    ROS_INFO("%s ready for operation.", ros::this_node::getName().c_str());
}
FakeVPClass::~FakeVPClass()
{
    std::cout << "~FakeVPClass()" << std::endl;
    delete m_node;
    delete m_rate;
    delete m_raw_img_srv;
    delete m_raw_camera_input_sub;
}

FakeVPClass& FakeVPClass::get_instance()
{
    std::cout << "get_instance()" << std::endl;
    static FakeVPClass m_instance;
    return m_instance;
}

void FakeVPClass::request_new_raw_frame()
{
    std::cout << "request_new_raw_frame()" << std::endl;
    try
    {
        std::cout << "Requesting new frame" << std::endl;
        m_received_new_raw_frame = false;
        std_srvs::Trigger t;
        m_raw_img_srv->call(t);
        std::cout << "...done" << std::endl;
    }
    catch(ros::Exception e)
    {
        ROS_ERROR("Service call unsuccessful: %s ", e.what());
        exit(EXIT_FAILURE);
    }
    catch(...)
    {
        ROS_ERROR("Unknown exception arose while trying to trigger service m_raw_img_srv.");
        exit(EXIT_FAILURE);
    }
}

void FakeVPClass::cb_received_new_raw_frame(const sensor_msgs::Image::ConstPtr& img)
{
    // ...DO SOME STUFF WITH THE IMAGE...
    m_received_new_raw_frame = true;
}

void FakeVPClass::run_vp()
{
    std::cout << "run_vp()" << std::endl;
    while(ros::ok())
    {
        // 1. Request new camera image
        request_new_raw_frame();

        // 2. Wait for new camera image to be sent
        std::cout << "Waiting..." << std::endl;
        while(!m_received_new_raw_frame)
        {
            std::cout << "...sleep..." << std::endl;
            ros::spinOnce();
            m_rate->sleep();
        }
        std::cout << "Done waiting!" << std::endl;

        // DO ... SOME .... WORK
    }
}
//////////////////////////////////////////////////////////////////////////
int main(int argc, char** argv)
{
    // Register our node
    ros::init(argc, argv, "fake_vp_roscpp");

    // Wait until master is operational
    ros::Time::init();
    while (!ros::master::check())
    {
        std::cout << "waiting..." << std::endl;
        ros::Duration(0.5).sleep();
    }

    // Create and execute instance
    FakeVPClass& vp = FakeVPClass::get_instance();
    vp ...
(more)
edit retag flag offensive close merge delete

Comments

Your solution is overly complex for what you're trying to achieve. You could create a custom service type and request and return the image in a single service call. Message delivery is not guaranteed in ROS but service calls are, so it would be a more reliable system.

PeteBlackerThe3rd gravatar imagePeteBlackerThe3rd ( 2019-09-30 05:11:53 -0500 )edit

Interesting idea, thank you. I will do it if I can't make the standard way work. The question remains: why is my callback not invoked?

robeastbme gravatar imagerobeastbme ( 2019-09-30 07:08:38 -0500 )edit