clickable_pc.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 
00003 #include <geometry_msgs/PointStamped.h>
00004 #include <opencv/cv.h>
00005 #include <opencv2/imgproc/imgproc.hpp>
00006 #include <opencv2/highgui/highgui.hpp>
00007 #include <sensor_msgs/image_encodings.h>
00008 
00009 #include <hrl_head_registration/head_registration.h>
00010 
00011 class ClickableDisplay {
00012     public:
00013         ros::NodeHandle nh, nh_priv;
00014         ros::Publisher l_click_pub, r_click_pub;
00015         std::string img_frame;
00016         bool have_img;
00017 
00018         ClickableDisplay();
00019         ~ClickableDisplay() {}
00020         void onInit();
00021         //void imgCallback(const sensor_msgs::ImageConstPtr& img_msg);
00022         void showImg(cv::Mat& img);
00023         static void mouseClickCallback(int event, int x, int y, int flags, void* data);
00024 
00025 };
00026 
00027 ClickableDisplay::ClickableDisplay() : nh_priv("~"), 
00028                                        have_img(false) {
00029 }
00030 
00031 void ClickableDisplay::onInit() {
00032     cv::namedWindow("Clickable World", 1);
00033     cv::setMouseCallback("Clickable World", &ClickableDisplay::mouseClickCallback, this);
00034     ROS_INFO("[clickable_display] ClickableDisplay loaded.");
00035     ros::Duration(1).sleep();
00036 }
00037 
00038 //void ClickableDisplay::imgCallback(const sensor_msgs::ImageConstPtr& img_msg) {
00039 //}
00040 
00041 void ClickableDisplay::showImg(cv::Mat& img) {
00042     ros::Rate r(30);
00043     while(ros::ok()) {
00044         ros::spinOnce();
00045         cv::imshow("Clickable World", img);
00046         cv::waitKey(3);
00047         r.sleep();
00048     }
00049 }
00050 
00051 char* file_out;
00052 
00053 void ClickableDisplay::mouseClickCallback(int event, int x, int y, int flags, void* param) {
00054     ClickableDisplay* this_ = reinterpret_cast<ClickableDisplay*>(param);
00055     if(event == CV_EVENT_LBUTTONDOWN) {
00056         FILE* file = fopen(file_out, "w");
00057         fprintf(file, "%d,%d\n", x, y);
00058         fclose(file);
00059         ros::shutdown();
00060     }
00061 }
00062 
00063 int main(int argc, char **argv)
00064 {
00065     ros::init(argc, argv, "clickable_display");
00066     ClickableDisplay cd;
00067     cv::Mat image(480, 640, CV_8UC3);
00068 
00069     PCRGB::Ptr input_pc;
00070     readPCBag(argv[1], input_pc);
00071     file_out = argv[2];
00072     uint8_t r, g, b;
00073     for(uint32_t i=0;i<input_pc->size();i++) {
00074         if(PT_IS_NOT_NAN(input_pc, i)) {
00075             extractRGB(input_pc->points[i].rgb, r, g, b);
00076             image.at<cv::Vec3b>(i/640, i%640)[2] = r;
00077             image.at<cv::Vec3b>(i/640, i%640)[1] = g;
00078             image.at<cv::Vec3b>(i/640, i%640)[0] = b;
00079         }
00080     }
00081 
00082     cd.onInit();
00083     cd.showImg(image);
00084     ros::spin();
00085     return 0;
00086 }


hrl_head_registration
Author(s): Kelsey Hawkins
autogenerated on Wed Nov 27 2013 11:45:27