Robotics StackExchange | Archived questions

TimeSynchronizer in a class using stereo input

Hi, I'm relatively new to ROS. I want to write a class which gets two raw images and creates a callback,where I can afterwards manipulate both obtained images at the same time. I tried TimeSynchronizer but it seems to never reach the Process() function. I want to create later on a disparity map (but not with the stereo_view) so I need synchronized input images. Time stamps are telling me that my input images are synchronized. So how can I get the two corresponding images in a callback function?

#include <ros/ros.h>
#include <std_msgs/String.h>
#include <image_transport/image_transport.h>
#include <image_transport/subscriber_filter.h>
#include <sensor_msgs/image_encodings.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <message_filters/time_synchronizer.h>
#include <message_filters/subscriber.h>

class ImgProcessor
{



ros::NodeHandle nh_;
image_transport::ImageTransport it_;


sensor_msgs::ImageConstPtr img_L;
sensor_msgs::ImageConstPtr img_R;


image_transport::Publisher image_pub_L;
image_transport::Publisher image_pub_R;

std::string IMG_TARGET_L_;
std::string IMG_TARGET_R_;

std::string IMG_DEST_L_;
std::string IMG_DEST_R_;
public:


// Constructor: Initialize the subscribe-act-publish-cycle
ImgProcessor(const std::string IMG_DEST_L, const std::string IMG_TARGET_L,const std::string IMG_DEST_R, const std::string IMG_TARGET_R)
    : it_(nh_)
{
    // Open Window to show output stream
    IMG_TARGET_L_ = IMG_TARGET_L;
    IMG_TARGET_R_ = IMG_TARGET_R;
    IMG_DEST_L_ = IMG_DEST_L;
    IMG_DEST_R_ = IMG_DEST_R;

    // Create output window
    cv::namedWindow(IMG_TARGET_L_);
    cv::namedWindow(IMG_TARGET_R_);

    // Subscribe to input video and publish output video
    message_filters::Subscriber<sensor_msgs::Image> image_sub_L(nh_,IMG_DEST_L_,40);
    message_filters::Subscriber<sensor_msgs::Image> image_sub_R(nh_,IMG_DEST_R_,40);
    message_filters::TimeSynchronizer<sensor_msgs::Image,sensor_msgs::Image> sync(image_sub_L,image_sub_R,40);
    sync.registerCallback(boost::bind(&ImgProcessor::Process,this, _1, _2));

    ROS_INFO("Synched.");

    image_pub_L = it_.advertise(IMG_TARGET_L_,1);
    image_pub_R = it_.advertise(IMG_TARGET_R_,1);

}

// Destructor: Close all opened images
~ImgProcessor()
{
    cv::destroyAllWindows();
}


// Process a message obtained in IMG_DEST
void Process(const sensor_msgs::ImageConstPtr& msg_l,const sensor_msgs::ImageConstPtr& msg_r)
{

    ROS_INFO("Origin Stamp = [%i] ", int(msg_l->header.stamp.toSec()));

    // Create a CV image copy from incoming message
    cv_bridge::CvImagePtr cv_ptr[2];
    try
    {
        cv_ptr[0] = cv_bridge::toCvCopy(msg_l, sensor_msgs::image_encodings::MONO8);
        cv_ptr[1] = cv_bridge::toCvCopy(msg_r, sensor_msgs::image_encodings::MONO8);
    }
    catch(cv_bridge::Exception& e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }


    // Process the incoming image
    //cv::medianBlur(cv_ptr->image,cv_ptr->image,3);
    cv::equalizeHist(cv_ptr[0]->image,cv_ptr[0]->image);
    cv::Canny(cv_ptr[0]->image,cv_ptr[0]->image,50,255);

    cv::equalizeHist(cv_ptr[1]->image,cv_ptr[1]->image);
    cv::Canny(cv_ptr[1]->image,cv_ptr[1]->image,50,255);


    // Update GUI
    cv::imshow(IMG_TARGET_L_, cv_ptr[0]->image);
    cv::waitKey(3);
    cv::imshow(IMG_TARGET_R_, cv_ptr[1]->image);
    cv::waitKey(3);

    img_L = cv_ptr[0]->toImageMsg();
    img_R = cv_ptr[1]->toImageMsg();
    //ROS_INFO("Processed Stamp = [%i] ", int(msg_o->header.stamp.nsecs));
    //Publish transformed video
    image_pub_L.publish(img_L);
    image_pub_R.publish(img_R);
}

};





int main(int argc, char **argv)
{
// Initialize the ROS system
ros::init(argc, argv, "stereo_proc_node");

// Tell the user that the node has started to process images
ROS_INFO("Processing images...");

// Create image processor object
ImgProcessor ip("/stereo/left/image_rect","stereo_proc/left/image","/stereo/right/image_rect","stereo_proc/right/image");


//sync.registerCallback(boost::bind(&callback, _1, _2));
ros::spin();
return 0;

}

Asked by lamparta on 2015-10-14 18:11:07 UTC

Comments

Answers

ImgProcessor(..) : it_(nh_)
{
     ...
    // Subscribe to input video and publish output video
    message_filters::Subscriber<sensor_msgs::Image> image_sub_L(nh_,IMG_DEST_L_,40);
    message_filters::Subscriber<sensor_msgs::Image> image_sub_R(nh_,IMG_DEST_R_,40);
    message_filters::TimeSynchronizer<sensor_msgs::Image,sensor_msgs::Image> sync(image_sub_L,image_sub_R,40);
    sync.registerCallback(boost::bind(&ImgProcessor::Process,this, _1, _2));
    ...
}

This is more basic C++ than ROS, but your TimeSynchronizer instance and both message_filters::Subscriber instances are not member variables, so will go out-of-scope as soon as your constructor runs to completion.

As they then no longer exist, I'd expect your callback to never be called, as you observed.


You may also want to look at topic remapping.

Asked by gvdhoorn on 2015-10-15 07:20:48 UTC

Comments

I'm facing almost a similar problem. In this case what would be the correct way to declare and define the message_filters::Subscriber and TimeSynchronizer inside the class and make the callback work?

Asked by reverse3D on 2018-05-01 04:44:38 UTC

Hi @reverse3D Did you find the correct the way to declare message_filters and TimeSynchronizer inside the class?

Asked by blenderrender on 2018-08-20 03:53:06 UTC

You can check arminkazim answer, it worked for me (basically make use of the class constructor).

Asked by Vlad Hondru on 2020-04-11 10:09:19 UTC

My solution for this problem :

class SubscribeAndPublish { 
public:     
SubscribeAndPublish():sync2(image_sub,image_sub2,500){       
//https://gist.github.com/tdenewiler/e2172f628e49ab633ef2786207793336       
image_sub.subscribe(n, "/camera/color/image_raw", 1);       
image_sub2.subscribe(n, "/camera/aligned_depth_to_color/image_raw", 1);     
sync2.registerCallback(boost::bind(&SubscribeAndPublish::callback,this,
_1, _2));       
odom_pub = n.advertise<nav_msgs::Odometry>("RGBDodom", 50);
}   
void callback(const sensor_msgs::ImageConstPtr& msg,const sensor_msgs::ImageConstPtr& msg2){ }
 private:   
 ros::NodeHandle n;     
 ros::Publisher odom_pub;   
 message_filters::Subscriber<Image> image_sub;  
 message_filters::Subscriber<Image> image_sub2;     
 TimeSynchronizer<Image, Image> sync2; 

};

Asked by arminkazim on 2019-11-05 06:44:00 UTC

Comments