Ask Your Question
1

C++11 closure as ROS callback?

asked 2015-08-11 10:26:16 -0600

drewm1980 gravatar image

Is it possible to register a C++11 closure as a ROS callback? I'm using ROS indigo.

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2015-08-11 11:00:13 -0600

drewm1980 gravatar image

updated 2015-08-11 11:01:02 -0600

I tried it out, and closures work! Here's an example, though it would need some messages to actually run.

#include <ros/ros.h>

#include <opencv2/core/core.hpp>
#include <opencv2/core/core_c.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

#include <image_transport/image_transport.h>

#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>

#include <stdlib.h>
#include <string>
#include <iostream>

using namespace std;

int main(int argc, char** argv)
{
  ros::init(argc, argv, "annotater");
  ros::NodeHandle ros_node_handle_;
  image_transport::ImageTransport ros_image_transport_(ros_node_handle_);
  image_transport::Subscriber ros_image_sub_;
  image_transport::Publisher ros_image_pub_;

  ros_image_pub_ = ros_image_transport_.advertise("output_image", 1);

  auto ImageCallback = [&] (const sensor_msgs::ImageConstPtr& msg)
  {
    cv_bridge::CvImagePtr rosimg;
    cv::Mat cvimg_pub;
    static int count=0;

    if (msg->encoding == "16UC1")
    {
      count += 1;
      cout << "Publishing a message " << count << endl;
      rosimg = cv_bridge::toCvCopy(msg, "16UC1");
      auto cvimg = rosimg->image;
      cv::cvtColor(cvimg, cvimg_pub, CV_GRAY2RGB);

      // Render some stuff on the image before publishing it
      auto drawOrigin = cv::Point(50,50);
      double fontScale = 2;
      int thickness = 3;
      auto font = CV_FONT_HERSHEY_SIMPLEX;
      cv::putText(cvimg_pub, "some text", drawOrigin, font, fontScale, cv::Scalar::all(0xFFFF), thickness, 8);

      // Throw up a plot for debugging
      cv::namedWindow("DebugImage");
      cv::imshow("DebugImage", cvimg_pub);
      char key = cv::waitKey(1); 
      if(key=='q') exit(0);
      cv::updateWindow("DebugImage");

      // Publish
      auto rosimg_pub = cv_bridge::CvImage(std_msgs::Header(),"bgr8",cvimg_pub);
      ros_image_pub_.publish(rosimg_pub.toImageMsg());

    } 
    else 
    { 
      cout << "Unhandled image encoding on input: " << msg->encoding << endl; 
    }
  };

  ros_image_sub_ = ros_image_transport_.subscribe("input_image", 1, ImageCallback);

  ros::spin();
  return 0;
}
edit flag offensive delete link more

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-08-11 10:26:16 -0600

Seen: 100 times

Last updated: Aug 11 '15