ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

[SOLVED] - Problem with ROS message filters

asked 2014-06-03 09:04:04 -0600

schizzz8 gravatar image

updated 2014-06-04 04:02:03 -0600

Hi, I have a bag file acquired from a mobile robot equipped with an Asus Xtion, that contains the following topics:

  • /camera/depth/camera_info
  • /camera/depth/image_rect_raw
  • /camera/rgb/camera_info
  • /camera/rgb/image_rect
  • /clock
  • /odom
  • /tf
  • /ticks

I need to write a ROS node that subscribes to ticks, rgb- and depth-image topics and for each timestamp saves in a text file the encoder ticks and the the paths (in the filesystem) to the rgb- and depth-image. So, as a first step, I'm trying to use the TimeSynchronizer class from ROS message filters to see if I can get the images; I know that maybe this can be done with two separates callbacks but I need a synchronized one for later usage so this is what I wrote according to ROS wiki:

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

using namespace sensor_msgs;
using namespace message_filters;

void callback(const ImageConstPtr& msg,const ImageConstPtr& msg2)
{

    cv_bridge::CvImagePtr cv_ptr;
    try
    {
        cv_ptr = cv_bridge::toCvCopy(msg2, sensor_msgs::image_encodings::TYPE_32FC1);
    }
    catch (cv_bridge::Exception& e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }

    if(cv_ptr->image.empty())
        std::cout << "Empty Image!!!\n";
    else
        std::cout << "Image size:" << cv_ptr->image.size() << "\n";

    double minVal, maxVal;
    cv::Mat blur_img;
    minMaxLoc(cv_ptr->image, &minVal, &maxVal); //find minimum and maximum intensities
    cv_ptr->image.convertTo(blur_img, CV_8U, 255.0/(maxVal - minVal), -minVal * 255.0/(maxVal - minVal));
    cv::imshow("Image window", blur_img);

    cv::waitKey(3);

}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "db_creator_node");
    ros::NodeHandle nh;

    message_filters::Subscriber<Image> rgb_sub(nh,"/camera/rgb/image_rect",1);
    message_filters::Subscriber<Image> dpt_sub(nh,"/camera/depth/image_rect_raw",1);
    TimeSynchronizer<Image,Image> sync(rgb_sub,dpt_sub,10);
    sync.registerCallback(boost::bind(&callback, _1, _2));
    cv::namedWindow("Image window");
    cv::startWindowThread();
    ros::spin();
    cv::destroyWindow("Image window");
    return 0;
}

when I run it, the node correctly subscribes to the desired topics, as shown by rqt_graph:

rqt_graph

but the callback is never executed because the statement

 if(cv_ptr->image.empty())
            std::cout << "Empty Image!!!\n";
        else
            std::cout << "Image size:" << cv_ptr->image.size() << "\n";

returns no output.

I'd be very glad if someone could point me out what I'm doing wrong and suggest a possible solution.

Thanks for your attention!!!

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
4

answered 2014-06-03 12:09:16 -0600

ahendrix gravatar image

I would guess that this is failing because your rgb and depth images aren't perfectly synchronized, and you're using a synchronizer that requires them to be perfectly synchronized. Have you tried the ApproximateTime synchronizer policy?

edit flag offensive delete link more

Comments

Yes you're right, I tried the ApproximateTime synchronizer and it worked! Thanks!

schizzz8 gravatar image schizzz8  ( 2014-06-04 03:59:10 -0600 )edit

Question Tools

1 follower

Stats

Asked: 2014-06-03 09:04:04 -0600

Seen: 5,253 times

Last updated: Jun 04 '14