0
$\begingroup$

Rosanswers logo

I have a node that converts a ROS image to OpenCV image based on this. I am using this to obatin a depth images and depth values of pixels. I don't have any issues with it as long as I print out the depth values.

Then I created a different package, retaining the above code, but with an added publisher. I have added the complete code at the end. This node uses a custom message type: depth_centre.msg. But this node is automatically killed after a few times. (I tried counting the number of loops and it is gets killed around 570 to 600). Why is this happening? Do I need to do something with the loop rate or size of message queue?

Code:

#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 <rscv_pub_centre/depth_centre.h>

static const std::string OPENCV_WINDOW = "Image window";
static const std::string OPENCV_WINDOW_2 = "SubImage window";
int count = 0;
class ImageConverter
{
  ros::NodeHandle nh_;
  ros::NodeHandle pub_;
  image_transport::ImageTransport it_;
  image_transport::Subscriber image_sub_;
  image_transport::Publisher image_pub_;
  
public:
  ImageConverter()
    : it_(nh_)
  {
    // Subscrive to input video feed and publish output video feed
    image_sub_ = it_.subscribe("/camera/depth/image_rect_color", 1, 
      &ImageConverter::imageCb, this);
    image_pub_ = it_.advertise("/image_converter/output_video", 1);

    //cv::namedWindow(OPENCV_WINDOW);
    //cv::namedWindow(OPENCV_WINDOW_2);
  }

  ~ImageConverter()
  {
    //cv::destroyWindow(OPENCV_WINDOW);
    //cv::destroyWindow(OPENCV_WINDOW_2);
  }

  void imageCb(const sensor_msgs::ImageConstPtr& msg)
  {
    
    cv_bridge::CvImagePtr cv_ptr;
    try
    {
      cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::TYPE_32FC1);
    }
    catch (cv_bridge::Exception& e)
    {
      ROS_ERROR("cv_bridge exception: %s", e.what());
      return;
    }
            
    cv::Mat mainImage=cv_ptr->image;
    cv::Mat subImage(mainImage, cv::Rect(100,100,1000,600));

    // Update GUI Window
    //cv::imshow(OPENCV_WINDOW, cv_ptr->image);
    //cv::imshow(OPENCV_WINDOW_2, subImage);
    //cv::waitKey(3);
    
    // Output modified video stream
    image_pub_.publish(cv_ptr->toImageMsg());

    //Publish Values in meter
    ros::Publisher depth_pub = pub_.advertise<rscv_pub_centre::depth_centre>("depth_m_centre", 1);
    ros::Rate loop_rate(10);

    while (ros::ok())
    {
        rscv_pub_centre::depth_centre msg;
        int rw = cv_ptr->image.rows;
        int cl = cv_ptr->image.cols;
        for (int i=(rw/2)-2; i<(rw/2); i++)
        {
            for (int j=(cl/2)-2; j<(cl/2); j++)
            {
                      cv::Scalar intensity = cv_ptr->image.at<float>(i, j); 
                      float depth = intensity.val[0];
                      msg.row=i;
                      msg.col=j;
                      msg.depth_in_meter=depth;
                
                      ROS_INFO("DEPTH INFO(%d,%d): [%f]",msg.row,msg.col,msg.depth_in_meter);
                      depth_pub.publish(msg); 
            
             }
        }
        count++;
        ROS_INFO("\nCount: %d", count);
        ros::spinOnce();
        loop_rate.sleep();
     }
  }
};

int main(int argc, char** argv)
{
  ros::init(argc, argv, "rscv_pub_centre_node");
  ImageConverter ic;
  ros::spin();
  return 0;
}

Originally posted by skr_robo on ROS Answers with karma: 178 on 2016-10-04

Post score: 0


Original comments

Comment by gvdhoorn on 2016-10-06:
Please don't close questions when you feel they have been answered. Just tick the checkmark to the left of the best answer. That is enough to clearly mark any question as answered.

Comment by skr_robo on 2016-10-07:
Sorry about that. Thanks for telling me.

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

I figured out the issue and it is rather silly. The lines:

ros::spinOnce();
loop_rate.sleep();

should be placed outside the while loop.


Originally posted by skr_robo with karma: 178 on 2016-10-05

This answer was ACCEPTED on the original site

Post score: 1


Original comments

Comment by gvdhoorn on 2016-10-06:
I think the bigger problem here is that you have an infinite while-loop in your imageCb(..). That won't work: as soon as the callback is executed once, it will get trapped in the while-loop, never returning. Instantiating publishers in your callback is also not a good idea.

Comment by gvdhoorn on 2016-10-06:
Please instantiate all publishers and subscribers first, register callbacks, then call either ros::spin(), or write a while loop that calls ros::spinOnce() and sleeps a little (using a ros::Rate, but that is not required). never (well, never ..) create pubs or subs in callbacks.

Comment by skr_robo on 2016-10-07:
Thanks for the input, but I am not sure if I understood it right. I believe I should just let the conversion happen and then allow program control to return to main and from there begin my publisher. Is that right?

Comment by skr_robo on 2016-10-07:
Do you think there is a way I can publish each pixel value after obtaining OpenCV image by simply restructuring the node or do I need to completely change my approach?

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.