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

Ros and OpenCV waitkey for imshow?

asked 2016-03-28 00:33:08 -0500

215 gravatar image

updated 2016-03-28 00:34:30 -0500

I am at the moment trying to come up with an subscriber, which only purpose should be extract images as quickly as possible (avoid process in callback function), and process them such that it doesn't interfere with the speed of the callback (avoiding delaying information). I seem to have managed to do so, but now i am wondering whether my waitKey() might be stalling my callback? if so how do i avoid it stalling my callback?

The subscriber looks like this:

#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <iostream>

namespace enc = sensor_msgs::image_encodings;

class Node
{
protected:
    //   image_transport::ImageTransport it_;
    image_transport::Subscriber sub_l;
    image_transport::Subscriber sub_r;
    ros::NodeHandle nh_l_;
    ros::NodeHandle nh_r_;
    ros::NodeHandle nhPrivate_;

    sensor_msgs::ImageConstPtr imageIn_L;
    sensor_msgs::ImageConstPtr imageIn_temp_L; //used for storing


    sensor_msgs::ImageConstPtr imageIn_r;
    sensor_msgs::ImageConstPtr imageIn_temp_r; //used for storing


    cv_bridge::CvImagePtr image_in_l;   //pointer to input image
    cv_bridge::CvImagePtr image_in_r;   //pointer to input image

    cv_bridge::CvImagePtr image_out;  //for ros msg conversion



public:
    cv::Mat cvImage_in_l;       //for processing, input at each step
    cv::Mat cvImage_in_r;       //for processing, input at each step


    Node(ros::NodeHandle& nh):nh_l_(nh), nh_r_(nh)
    {
        image_transport::ImageTransport it_l(nh_l_);
        image_transport::ImageTransport it_r(nh_r_);

        sub_l = it_l.subscribe("/stereo_camera/left/image_raw", 1, &Node::imageCb_l, this);
        sub_r = it_r.subscribe("/stereo_camera/right/image_raw", 1, &Node::imageCb_r, this);

    }

    void imageCb_l(const sensor_msgs::ImageConstPtr& image)
    {
        ROS_ERROR("callback - l");
        if ( &image == NULL)
        return;

        try
        {
            imageIn_L = image;
        }
        catch (cv_bridge::Exception& e)
        {
            ROS_ERROR("Conversion error: %s", e.what());
            return;
        }
    }


    void imageCb_r(const sensor_msgs::ImageConstPtr& image)
    {
        ROS_ERROR("callback - r");
        if ( &image == NULL)
        return;

        try
        {
            imageIn_r = image;
        }
        catch (cv_bridge::Exception& e)
        {
            ROS_ERROR("Conversion error: %s", e.what());
            return;
        }
    }

    void process()
    {    
        if ( imageIn_L == NULL || imageIn_L == imageIn_temp_L)
         return;

        imageIn_temp_L = imageIn_L;

        if ( imageIn_r == NULL || imageIn_r == imageIn_temp_r)
        return;
        imageIn_temp_r = imageIn_r;

        //Actual process

        image_in_l = cv_bridge::toCvCopy(imageIn_L, enc::BGR8);
        image_in_r = cv_bridge::toCvCopy(imageIn_r, enc::BGR8);

        cvImage_in_l = image_in_l->image;
        cvImage_in_r = image_in_r->image;

        cv::cvtColor(cvImage_in_l,cvImage_in_l,CV_BGR2HSV);
        cv::cvtColor(cvImage_in_r,cvImage_in_r,CV_BGR2HSV);


        cv::inRange(cvImage_in_l, cv::Scalar(0,184,0), cv::Scalar(14,236,255), cvImage_in_l);
        cv::inRange(cvImage_in_r, cv::Scalar(0,184,0), cv::Scalar(14,236,255), cvImage_in_r);


        cv::imshow("imageL", cvImage_in_l);
        cv::imshow("imageR", cvImage_in_r);

        cv::waitKey(1);


    }


    void spin()
    {
        ros::Rate rate(30);
        while (ros::ok())
        {
              ros::spinOnce();
              process();
              rate.sleep();
        }
    }

};




int main(int argc, char** argv)
{
    ros::init(argc, argv, "image_processor");
    ros::NodeHandle nh;
    Node node(nh);
    node.spin();

}
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2016-03-28 09:22:08 -0500

NEngelhard gravatar image

I don't think that this is the best approach for your problem and you could remove much of the code.

void imageCb_r(const sensor_msgs::ImageConstPtr& image)
{
    ROS_ERROR("callback - r");
    /// This is not needed. The callback is triggered only if there is some data. 
    if ( &image == NULL) 
    return;

    try
    { 
        /// This is simple assignment, no conversion so there is nothing to throw.
        imageIn_r = image;   
    }
    catch (cv_bridge::Exception& e)
    {
        ROS_ERROR("Conversion error: %s", e.what());
        return;
    }
}

The names sound like a stereo camera, you however do not care about the timestamps so that not the right images are used to create a stereo pair. You could use a TimeSynchronizer for that: http://wiki.ros.org/message_filters

The cv::WaitKey(1) should not make a difference at it's length is negligible compared to the whole process()-call.

A much bigger influence has the duration of your Rate. You just check for new images every 33ms which could delay your processing much more than the 1ms Waitkey (although I'm not sure how long the execution of the waitkey really takes).

edit flag offensive delete link more

Question Tools

1 follower

Stats

Asked: 2016-03-28 00:33:08 -0500

Seen: 3,225 times

Last updated: Mar 28 '16