oriented_gradient_node.cpp

Go to the documentation of this file.

// @brief sample node to use oriented gradient
// @author Hiroaki Yaguchi, JSK

#include <vector>
#include <string>

#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 "jsk_perception/oriented_gradient.hpp"

namespace jsk_perception {

class OrientedGradientNode {
 public:
  explicit OrientedGradientNode(const ros::NodeHandle& nh) :
      handle_(nh), image_transport_(nh) {
    image_pub_ = image_transport_.advertise("image", 1);
    image_sub_ = image_transport_.subscribe(
        "/camera/rgb/image_raw", 1,
        &OrientedGradientNode::imageCallback, this);
    cv::namedWindow("OrinetedGradient");
  }

  ~OrientedGradientNode() {
  }

 private:
  ros::NodeHandle handle_;
  image_transport::ImageTransport image_transport_;
  image_transport::Subscriber image_sub_;
  image_transport::Publisher image_pub_;
  void imageCallback(const sensor_msgs::ImageConstPtr& msg) {
    cv_bridge::CvImagePtr cv_ptr;

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

    cv::Mat cv_img = cv_ptr->image;
    cv::Mat cv_og_img;
    cv::Mat cv_out_img;

    // calcOrientedGradient() will write oriented gradient to
    // 2nd arg image as HSV format,
    // H is orientation and V is intensity
    calcOrientedGradient(cv_img, cv_og_img);
    cv::cvtColor(cv_og_img, cv_out_img, CV_HSV2BGR);
    // cv::imshow("OrinetedGradient", cv_out_img);
    // cv::waitKey(1);
    sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(
      msg->header,
      sensor_msgs::image_encodings::BGR8,
      cv_out_img).toImageMsg();
    image_pub_.publish(out_img);
  }
};

}  // namespace


int main(int argc, char** argv) {
  ros::init(argc, argv, "oriented_gradient");
  ros::NodeHandle handle("~");
  jsk_perception::OrientedGradientNode ognode(handle);
  ros::spin();
  return 0;
}