Subscriber to sensor_msgs::Image with class member function callback not triggered
I have created a singleton class as a ROS node. I want to subscribe to a sensormsgs::Image topic (published by another node). I implemented a synchronization mechanism with stdsrvs::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.run_vp();
return 0;
}
And now the output:
waiting...
get_instance()
FakeVPClass()
[ INFO] [1569775480.756903309]: /fake_vp_roscpp ready for operation.
run_vp()
request_new_raw_frame()
Requesting new frame
...done
Waiting...
...sleep...
...sleep...
...sleep...
Looking at the rostopics everything seems just fine:
$ rostopic list /raw_camera_input --verbose
Published topics:
* /raw_camera_input [sensor_msgs/Image] 1 publisher
Subscribed topics:
* /raw_camera_input [sensor_msgs/Image] 1 subscriber
I'm very sure that the publisher sends a new frame (I traced the sending procedure), yet the callback function cbreceivednewrawframe(...) is never triggered. I've also tried the same idea using image_transport, but the no change. No callback occurs. I'm doing something wrong, but I can't figure out what. I'd really really appreciate some help.....
(Note: I'm using Ubuntu 14.04 and ROS Indigo version 1.11.21)
Asked by robeastbme on 2019-09-29 13:56:29 UTC
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.
Asked by PeteBlackerThe3rd on 2019-09-30 05:11:53 UTC
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?
Asked by robeastbme on 2019-09-30 07:08:38 UTC