Ask Your Question
0

The node getting killed after few runs when publisher is added

asked 2016-10-04 10:49:45 -0600

skr_robo gravatar image

updated 2016-10-04 11:06:28 -0600

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;
}
edit retag flag offensive close merge delete

Comments

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.

gvdhoorn gravatar imagegvdhoorn ( 2016-10-06 01:52:19 -0600 )edit

Sorry about that. Thanks for telling me.

skr_robo gravatar imageskr_robo ( 2016-10-07 11:58:39 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted
1

answered 2016-10-05 19:49:50 -0600

skr_robo gravatar image

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

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

should be placed outside the while loop.

edit flag offensive delete link more

Comments

1

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.

gvdhoorn gravatar imagegvdhoorn ( 2016-10-06 01:50:05 -0600 )edit
1

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.

gvdhoorn gravatar imagegvdhoorn ( 2016-10-06 01:51:16 -0600 )edit

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?

skr_robo gravatar imageskr_robo ( 2016-10-07 12:08:49 -0600 )edit

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?

skr_robo gravatar imageskr_robo ( 2016-10-07 13:28:30 -0600 )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: 2016-10-04 10:49:45 -0600

Seen: 137 times

Last updated: Oct 05 '16