Ask Your Question
0

TimeSynchronizer in a class using stereo input

asked 2015-10-14 18:11:07 -0500

lamparta gravatar image

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;

}

edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
1

answered 2019-11-05 05:44:00 -0500

arminkazim gravatar image

updated 2019-11-05 05:45:54 -0500

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;

};

edit flag offensive delete link more
0

answered 2015-10-15 07:20:48 -0500

gvdhoorn gravatar image

updated 2015-10-15 07:24:58 -0500

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.

edit flag offensive delete link more

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?

reverse3D gravatar image reverse3D  ( 2018-05-01 04:44:38 -0500 )edit

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

blenderrender gravatar image blenderrender  ( 2018-08-20 03:53:06 -0500 )edit

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

Vlad Hondru gravatar image Vlad Hondru  ( 2020-04-11 10:09:19 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2015-10-14 18:11:07 -0500

Seen: 509 times

Last updated: Nov 05 '19