diff --git a/vision_stack/vision_gui/CMakeLists.txt b/vision_stack/vision_gui/CMakeLists.txt index 49dfa3a..207b313 100644 --- a/vision_stack/vision_gui/CMakeLists.txt +++ b/vision_stack/vision_gui/CMakeLists.txt @@ -25,7 +25,7 @@ FIND_PACKAGE(Qt4 REQUIRED) find_package( OpenCV REQUIRED ) INCLUDE(${QT_USE_FILE}) ADD_DEFINITIONS(-DQT_NO_KEYWORDS) -include_directories(include/${PROJECT_NAME}/ ${CMAKE_CURRENT_BINARY_DIR}) +include_directories(include/${PROJECT_NAME}/ ${CMAKE_CURRENT_BINARY_DIR} task_bouy/include/ task_marker/include/) ############################################################################## # Ros Initialisation diff --git a/vision_stack/vision_gui/include/ImageNew/mainwindow.hpp b/vision_stack/vision_gui/include/ImageNew/mainwindow.hpp index 125a4ea..83b26be 100644 --- a/vision_stack/vision_gui/include/ImageNew/mainwindow.hpp +++ b/vision_stack/vision_gui/include/ImageNew/mainwindow.hpp @@ -24,6 +24,11 @@ #include #include #include +#include +#include +#include +#include + class MyLabel : public QLabel { @@ -59,6 +64,8 @@ public Q_SLOTS: void on_startCamera_clicked(); void on_pauseCamera_clicked(); void on_closeCamera_clicked(); + void on_submitRosTopic_clicked(); + // slider callbacks void on_HValue_slider_valueChanged(int value); void on_SValue_slider_valueChanged(int value); @@ -67,6 +74,7 @@ public Q_SLOTS: void on_SThreshValue_slider_valueChanged(int value); void on_VThreshValue_slider_valueChanged(int value); void on_colorImage_mouseDoubleClickEvent(QMouseEvent * ev); + void imageCallBack(const sensor_msgs::ImageConstPtr &_msg); /******** Filters ****************/ @@ -119,12 +127,17 @@ public Q_SLOTS: Ui::MainWindow *ui; bool camera_opend,video_being_processed; cv::Scalar start,end; + // ros::NodeHandle n; + // image_transport::ImageTransport it(n); + // image_transport::Subscriber sub; + // const sensor_msgs::ImageConstPtr &msg; std::string fileName; QTimer *timer; cv::Mat colorImage,displayImage,thImage,binaryImage,camImage; cv::VideoCapture cam; cv::Size image_size; MyLabel *colorLabel; + // sensor_msgs::ImageConstPtr msg; cv::Vec3b vec; bool getNextFrame(cv::Mat &img); void displayImages(); diff --git a/vision_stack/vision_gui/manifest.xml b/vision_stack/vision_gui/manifest.xml index 6c63de7..1250325 100644 --- a/vision_stack/vision_gui/manifest.xml +++ b/vision_stack/vision_gui/manifest.xml @@ -10,8 +10,12 @@ http://ros.org/wiki/ImageNew + + + + diff --git a/vision_stack/vision_gui/src/mainwindow.cpp b/vision_stack/vision_gui/src/mainwindow.cpp index 333288c..0fcb36f 100644 --- a/vision_stack/vision_gui/src/mainwindow.cpp +++ b/vision_stack/vision_gui/src/mainwindow.cpp @@ -5,7 +5,6 @@ #include #include #include - #include #include #include @@ -13,6 +12,10 @@ #include #include #include +#include +#include +#include +#include QString fn_video; @@ -76,6 +79,7 @@ MyWindow::MyWindow(QWidget *parent) : filter_arg2 = B_LESS; filter_arg4 = B_INCLUDE; filter_arg5_area = 500; + } void MyWindow::setThresh() @@ -502,6 +506,48 @@ void MyWindow::on_closeCamera_clicked() ui->closeCamera->setEnabled(false); } +void MyWindow::on_submitRosTopic_clicked() +{ + QString topic = ui->textRosTopic->text(); + try + { + int argc = 0; + ros::init(argc,NULL,"vision_gui_rostopic"); + ros::NodeHandle n; + std::cout <<"Correct ROS Topic input given."<< std::endl; + image_transport::ImageTransport it(n); + image_transport::Subscriber sub; + while(ros::ok()) + { + sub = it.subscribe(topic.toUtf8().constData(), 1, &MyWindow::imageCallBack, this); + std::cout <<"ROS Loop"<< std::endl; + } + ros::shutdown(); + } + catch (ros::InvalidNameException& e) + { + std::cout <<"Incorrect ROS Topic input given : "<image; + source_from_log = true; + video_being_processed = true; + setThresh(); + } + catch (cv_bridge::Exception& e) + { + std::cout <<"ROS Topic input given : Could not convert from" << _msg->encoding.c_str() << std::endl; + } +} + void MyWindow::on_HValue_slider_valueChanged(int value) { std::ostringstream str; diff --git a/vision_stack/vision_gui/src/mainwindow.hpp b/vision_stack/vision_gui/src/mainwindow.hpp index 8d659ab..5f52076 100644 --- a/vision_stack/vision_gui/src/mainwindow.hpp +++ b/vision_stack/vision_gui/src/mainwindow.hpp @@ -24,6 +24,11 @@ #include #include #include +#include +#include +#include +#include + class MyLabel : public QLabel { @@ -59,6 +64,8 @@ public Q_SLOTS: void on_startCamera_clicked(); void on_pauseCamera_clicked(); void on_closeCamera_clicked(); + void on_submitRosTopic_clicked(); + // slider callbacks void on_HValue_slider_valueChanged(int value); void on_SValue_slider_valueChanged(int value); @@ -67,6 +74,7 @@ public Q_SLOTS: void on_SThreshValue_slider_valueChanged(int value); void on_VThreshValue_slider_valueChanged(int value); void on_colorImage_mouseDoubleClickEvent(QMouseEvent * ev); + void imageCallBack(const sensor_msgs::ImageConstPtr &msg); /******** Filters ****************/ @@ -119,6 +127,10 @@ public Q_SLOTS: Ui::MainWindow *ui; bool camera_opend,video_being_processed; cv::Scalar start,end; + // ros::NodeHandle n; + // image_transport::ImageTransport it(n); + // image_transport::Subscriber sub; + // const sensor_msgs::ImageConstPtr &msg; std::string fileName; QTimer *timer; cv::Mat colorImage,displayImage,thImage,binaryImage,camImage; @@ -126,6 +138,7 @@ public Q_SLOTS: cv::Size image_size; MyLabel *colorLabel; cv::Vec3b vec; + // sensor_msgs::ImageConstPtr msg; bool getNextFrame(cv::Mat &img); void displayImages(); void setThresh(); diff --git a/vision_stack/vision_gui/ui/mainwindow.ui b/vision_stack/vision_gui/ui/mainwindow.ui index 04f630c..446494e 100644 --- a/vision_stack/vision_gui/ui/mainwindow.ui +++ b/vision_stack/vision_gui/ui/mainwindow.ui @@ -50,6 +50,11 @@ Open Video + + + Open Bag + + @@ -108,6 +113,20 @@ + + + + /kraken/front_camera/buoy_image + + + + + + + Submit ROS topic + + +