gui.cpp

Go to the documentation of this file.

#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <opencv/cv.h>
#include <opencv/highgui.h>
#include <cv_bridge/CvBridge.h>
#include <boost/thread/mutex.hpp>

#include <motld/BoundingBox.h>

ros::Publisher box_pub_;
sensor_msgs::CvBridge bridge_;

boost::mutex state_mutex_;
bool isMouseCallbackSet_ = false;
bool correctBB = false;
static CvFont font_;
static IplImage * color_image_;
static IplImage * curr_image_;
static IplImage * imgc_;
static CvPoint point_, point_next_;
static CvRect bb_;
static int drag_ = 0;

static void addTextAndBB(IplImage *img) {
        cvPutText(img, "Draw a bounding box and press Enter", cvPoint(0, 60),
                  &font_, cvScalar(255, 255, 0));
        bool bb_set = (point_.x != -1 && point_.y != -1);
        if (bb_set) {
                cvRectangle(img, point_, point_next_, CV_RGB(255, 0, 0), 1, 8, 0);
        } else {
                //ROS_ERROR("cannot draw bb");
        }
}

static void mouseHandler(int event, int x, int y, int flags, void* param) {
        /* user press left button */
        if (event == CV_EVENT_LBUTTONDOWN && !drag_) {
                point_ = cvPoint(x, y);
                drag_ = 1;
        }

        /* user drag the mouse */
        if (event == CV_EVENT_MOUSEMOVE && drag_) {
                /* set point_next_ */
                point_next_ = cvPoint(x, y);
        }

        /* user release left button */
        if (event == CV_EVENT_LBUTTONUP && drag_) {
                bb_ = cvRect(point_.x, point_.y, point_next_.x - point_.x, point_next_.y - point_.y);
                drag_ = 0;                
        }
}

static void trackerCallback(const sensor_msgs::ImageConstPtr& msg) {
        // convert image
        boost::unique_lock<boost::mutex>(state_mutex_);
        color_image_ = bridge_.imgMsgToCv(msg, "bgr8");
        curr_image_ = (IplImage *) cvClone(color_image_);
        
        addTextAndBB(curr_image_);
        cvShowImage("bounding_box", curr_image_);
        cvReleaseImage(&curr_image_);

        if (isMouseCallbackSet_ == false) {
                cvSetMouseCallback("bounding_box", mouseHandler, NULL);
                isMouseCallbackSet_ = true;
        }
}

int main(int argc, char **argv) {
        ros::init(argc, argv, "motld_gui");
        ros::NodeHandle nh;
        image_transport::ImageTransport it(nh);
        image_transport::Subscriber sub = it.subscribe("/camera/rgb/image_color", 1, trackerCallback);
        motld::BoundingBox box_msg;
        box_pub_ = nh.advertise<motld::BoundingBox>("/motld_node/tracker_start_box", 1);

        cvNamedWindow("bounding_box");
        cvInitFont(&font_, CV_FONT_HERSHEY_SIMPLEX, 0.5, 0.5, 0, 1, 8);
        //cvStartWindowThread();
        
        bool user_exit = false;
        point_.x = -1;
        point_.y = -1;
        while (ros::ok() && !user_exit) {
                ros::spinOnce();
                char key = cvWaitKey(5);
                if(tolower(key) == 'q') {
                        ROS_INFO("motld gui user exit!");
                        user_exit = true;
                }
                if((key == '\n') && (bb_.x != -1) && (bb_.y != -1)) {
                        correctBB = true;
                        ROS_INFO("motld sending bounding box");
                        box_msg.x = bb_.x;
                        box_msg.y = bb_.y;
                        box_msg.width = bb_.width;
                        box_msg.height = bb_.height;
                        box_pub_.publish(box_msg);
                }
        }
        
        cvSetMouseCallback("bounding_box", NULL, NULL);
        cvDestroyWindow("bounding_box");
        cvReleaseImage(&curr_image_);
}