Foxy opencv exp.cpp

//*************************************************************************
//  opencv_exp.cpp
// 
//  Copyright (C) 2021, Ed C. Epp
//
//  Image subscriber demo with transform to OpenCV and mouse regon
//  selection.
//

#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/image.hpp"

#include <cv_bridge/cv_bridge.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

using std::placeholders::_1;

//****************************************************************************
//          on_mouse function
//****************************************************************************

CvRect  our_selection;
int     our_object_selected = 0;
int     our_new_object_selected = 0;
int     our_dragging = 0;
CvPoint our_drag_location;

void on_mouse (int event, int x, int y, int flags, void* param)
{
    //std::cout << "on_mouse\n";
    (void)param;   //suppress parmm not used warning
    
    our_dragging = 0;
    switch (event)
    {
    case cv::EVENT_LBUTTONDOWN:
            our_selection = cvRect(x,y,0,0);
            break;
    case cv::EVENT_MOUSEMOVE:
      if (flags == cv::EVENT_FLAG_LBUTTON)
            {
                our_dragging = 1;
                our_drag_location = cvPoint(x,y);
            }
            break;
    case cv::EVENT_LBUTTONUP:
            our_selection.width  = abs (x - our_selection.x);
            our_selection.height = abs (y - our_selection.y);
            our_selection.x      = MIN (x, our_selection.x);
            our_selection.y      = MIN (y, our_selection.y);
            our_object_selected = our_new_object_selected = 1;
	    //std::cout <<
	    //		"Mouse up: x: %d  y: %d  width: %d  height: %d", 
	    //   our_selection.x << " " << our_selection.y << " " <<
	    //  our_selection.width << " " << our_selection.height << "\n";
            break;
    }
}

// =============================================================================
// OpenCVExp class definition

class OpenCVExp : public rclcpp::Node
{
public:
  // ---------------------------------------------------------------------------
  // constructor - create the subscriber
  OpenCVExp(): Node("Opencv_Exp")
  {
    // Set the quality of service
    rclcpp::QoS video_qos(10);
    video_qos.keep_last(10);
    video_qos.best_effort();
    video_qos.durability_volatile();

    // Subscribe to image topics
    my_subscription = this->create_subscription<sensor_msgs::msg::Image>(
       "image", video_qos, std::bind(&OpenCVExp::image_callback, this, _1));
    
    // Open HighGUI Windows
    cv::namedWindow ("CamShiftTracking", 1);
    cv::setMouseCallback( "CamShiftTracking", on_mouse, 0 );
    cv::moveWindow("CamShiftTracking", 100, 100);
}

private:
  //----------------------------------------------------------------------------
  // image callback function
  void image_callback(const sensor_msgs::msg::Image::SharedPtr msg) 
  {
    //RCLCPP_INFO(this->get_logger(), "I See: " +  msg->encoding);

    // Copy in the webcam image
    try
    {
      my_webcam_img = cv_bridge::toCvCopy(msg, msg->encoding); 
    }
    catch (cv_bridge::Exception& error)
    {
        RCLCPP_ERROR (this->get_logger(), "cv_bridge Input Error: %s",
		      error.what());
    }

    // Convert the image to cv::Mat
    my_image = cv::Mat (my_webcam_img->image).clone ();

    // Convert Input image from BGR to HSV 
    cv::cvtColor (my_image, my_hsv, cv::COLOR_BGR2HSV);

    // Display a negative region when mouse dragging
    if( our_dragging )
    {
        int x      = MIN (our_drag_location.x, our_selection.x);
        int y      = MIN (our_drag_location.y, our_selection.y);
        int width  = abs (our_drag_location.x - our_selection.x);
        int height = abs (our_drag_location.y - our_selection.y);

	cv::Mat roi(my_image, cvRect(x, y, width, height));
        bitwise_not(roi, roi);
            //RCLCPP_INFO (this->get_logger(),
	    //		 "Our Dragging: x: %d  y: %d  width: %d  height: %d", 
	    //           x, y, width, height);
    }

    // Display input image
    cv::imshow ("CamShiftTracking", my_image);

    cv::waitKey(3);
  }

  // ---------------------------------------------------------------------------
  // Subscriber instance variable
  rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr my_subscription;

  // Image related
  cv::Mat                my_image;
  cv::Mat                my_hsv;
  cv_bridge::CvImagePtr  my_webcam_img;
};


// =============================================================================
// main
int main(int argc, char * argv[])
{
  // Act on the parameters and other stuff
  rclcpp::init(argc, argv);

  // Create the ROS node and start it up
  rclcpp::spin(std::make_shared<OpenCVExp>());

  rclcpp::shutdown();
  return 0;
}