├── .gitignore ├── LICENSE ├── README.md ├── docs ├── images │ ├── color-picker.png │ ├── draw-image-01.png │ ├── draw-image-02.png │ ├── draw-image-03.png │ ├── draw-shape.png │ ├── rqt-turtle.png │ ├── service-caller-spawn.png │ ├── service-caller-teleport-abs.png │ └── service-caller-teleport-rel.png ├── rqt_turtle-draw-image-multi.gif ├── rqt_turtle-draw-image.gif ├── rqt_turtle-draw-shape-cancel.gif ├── rqt_turtle-draw-shape.gif ├── rqt_turtle.gif └── rqt_turtle.mp4 └── rqt_turtle ├── CMakeLists.txt ├── include └── rqt_turtle │ ├── action_worker.h │ ├── draw.h │ ├── image_worker.h │ ├── service_caller.h │ ├── turtle.h │ └── turtle_plugin.h ├── package.xml ├── plugin.xml ├── resources ├── Draw.ui ├── ServiceCaller.ui ├── Task.ui └── turtle_plugin.ui ├── scripts └── rqt_turtle ├── setup.py └── src └── rqt_turtle ├── action_worker.cpp ├── draw.cpp ├── image_worker.cpp ├── service_caller.cpp ├── turtle.cpp └── turtle_plugin.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | devel/ 2 | logs/ 3 | build/ 4 | bin/ 5 | lib/ 6 | msg_gen/ 7 | srv_gen/ 8 | msg/*Action.msg 9 | msg/*ActionFeedback.msg 10 | msg/*ActionGoal.msg 11 | msg/*ActionResult.msg 12 | msg/*Feedback.msg 13 | msg/*Goal.msg 14 | msg/*Result.msg 15 | msg/_*.py 16 | build_isolated/ 17 | devel_isolated/ 18 | 19 | # Generated by dynamic reconfigure 20 | *.cfgc 21 | /cfg/cpp/ 22 | /cfg/*.py 23 | 24 | # Ignore generated docs 25 | *.dox 26 | *.wikidoc 27 | 28 | # eclipse stuff 29 | .project 30 | .cproject 31 | 32 | # qcreator stuff 33 | CMakeLists.txt.user 34 | 35 | srv/_*.py 36 | *.pcd 37 | *.pyc 38 | qtcreator-* 39 | *.user 40 | 41 | /planning/cfg 42 | /planning/docs 43 | /planning/src 44 | 45 | *~ 46 | 47 | # Emacs 48 | .#* 49 | 50 | # Catkin custom files 51 | CATKIN_IGNORE 52 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2020 Franz 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # rqt-turtle 2 | 3 | [`rqt`](http://wiki.ros.org/rqt) plugin for ROS (Noetic) to control turtles in turtlesim. 4 | 5 | ![Short Demo](docs/rqt_turtle.gif) 6 | 7 | To learn how the package was created, please read the [documentation](https://fjp.at/ros/turtle-pong). 8 | 9 | The video below gives more insights on what is currently implemented: 10 | 11 | [![rqt turtle YouTube](http://img.youtube.com/vi/2IQtxEmP2a4/0.jpg)](https://youtu.be/2IQtxEmP2a4) 12 | 13 | 14 | ## Launch 15 | 16 | To run the plugin make sure it is installed in your ros workspace and execute `rqt`. 17 | From the `Plugins` menu select `Robot Tools->TurtleSim`. 18 | 19 | 20 | ## Dependencies 21 | 22 | The following dependencies are only required if you would like to contribute addons to this project. 23 | 24 | - [Qt 5](https://wiki.qt.io/Install_Qt_5_on_Ubuntu): `sudo apt install qt5-default qtcreator` 25 | - OpenCV: Install ubuntu package `sudo apt install libopencv-dev` or install a specific/latest version [from source](https://docs.opencv.org/trunk/d7/d9f/tutorial_linux_install.html) 26 | -------------------------------------------------------------------------------- /docs/images/color-picker.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fjp/rqt-turtle/23d7e9430633df276f21c09f17970aa692bf45d2/docs/images/color-picker.png -------------------------------------------------------------------------------- /docs/images/draw-image-01.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fjp/rqt-turtle/23d7e9430633df276f21c09f17970aa692bf45d2/docs/images/draw-image-01.png -------------------------------------------------------------------------------- /docs/images/draw-image-02.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fjp/rqt-turtle/23d7e9430633df276f21c09f17970aa692bf45d2/docs/images/draw-image-02.png -------------------------------------------------------------------------------- /docs/images/draw-image-03.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fjp/rqt-turtle/23d7e9430633df276f21c09f17970aa692bf45d2/docs/images/draw-image-03.png -------------------------------------------------------------------------------- /docs/images/draw-shape.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fjp/rqt-turtle/23d7e9430633df276f21c09f17970aa692bf45d2/docs/images/draw-shape.png -------------------------------------------------------------------------------- /docs/images/rqt-turtle.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fjp/rqt-turtle/23d7e9430633df276f21c09f17970aa692bf45d2/docs/images/rqt-turtle.png -------------------------------------------------------------------------------- /docs/images/service-caller-spawn.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fjp/rqt-turtle/23d7e9430633df276f21c09f17970aa692bf45d2/docs/images/service-caller-spawn.png -------------------------------------------------------------------------------- /docs/images/service-caller-teleport-abs.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fjp/rqt-turtle/23d7e9430633df276f21c09f17970aa692bf45d2/docs/images/service-caller-teleport-abs.png -------------------------------------------------------------------------------- /docs/images/service-caller-teleport-rel.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fjp/rqt-turtle/23d7e9430633df276f21c09f17970aa692bf45d2/docs/images/service-caller-teleport-rel.png -------------------------------------------------------------------------------- /docs/rqt_turtle-draw-image-multi.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fjp/rqt-turtle/23d7e9430633df276f21c09f17970aa692bf45d2/docs/rqt_turtle-draw-image-multi.gif -------------------------------------------------------------------------------- /docs/rqt_turtle-draw-image.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fjp/rqt-turtle/23d7e9430633df276f21c09f17970aa692bf45d2/docs/rqt_turtle-draw-image.gif -------------------------------------------------------------------------------- /docs/rqt_turtle-draw-shape-cancel.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fjp/rqt-turtle/23d7e9430633df276f21c09f17970aa692bf45d2/docs/rqt_turtle-draw-shape-cancel.gif -------------------------------------------------------------------------------- /docs/rqt_turtle-draw-shape.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fjp/rqt-turtle/23d7e9430633df276f21c09f17970aa692bf45d2/docs/rqt_turtle-draw-shape.gif -------------------------------------------------------------------------------- /docs/rqt_turtle.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fjp/rqt-turtle/23d7e9430633df276f21c09f17970aa692bf45d2/docs/rqt_turtle.gif -------------------------------------------------------------------------------- /docs/rqt_turtle.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fjp/rqt-turtle/23d7e9430633df276f21c09f17970aa692bf45d2/docs/rqt_turtle.mp4 -------------------------------------------------------------------------------- /rqt_turtle/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(rqt_turtle) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | roscpp 12 | rqt_gui 13 | rqt_gui_cpp 14 | actionlib 15 | turtle_actionlib 16 | ) 17 | 18 | ## System dependencies are found with CMake's conventions 19 | # find_package(Boost REQUIRED COMPONENTS system) 20 | find_package(Qt5Widgets REQUIRED) 21 | # https://docs.opencv.org/2.4/doc/tutorials/introduction/linux_gcc_cmake/linux_gcc_cmake.html 22 | find_package(OpenCV REQUIRED) 23 | 24 | 25 | ## Uncomment this if the package has a setup.py. This macro ensures 26 | ## modules and global scripts declared therein get installed 27 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 28 | catkin_python_setup() 29 | 30 | ################################################ 31 | ## Declare ROS dynamic reconfigure parameters ## 32 | ################################################ 33 | 34 | ## To declare and build dynamic reconfigure parameters within this 35 | ## package, follow these steps: 36 | ## * In the file package.xml: 37 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 38 | ## * In this file (CMakeLists.txt): 39 | ## * add "dynamic_reconfigure" to 40 | ## find_package(catkin REQUIRED COMPONENTS ...) 41 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 42 | ## and list every .cfg file to be processed 43 | 44 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 45 | # generate_dynamic_reconfigure_options( 46 | # cfg/DynReconf1.cfg 47 | # cfg/DynReconf2.cfg 48 | # ) 49 | 50 | ################################### 51 | ## catkin specific configuration ## 52 | ################################### 53 | ## The catkin_package macro generates cmake config files for your package 54 | ## Declare things to be passed to dependent projects 55 | ## INCLUDE_DIRS: uncomment this if your package contains header files 56 | ## LIBRARIES: libraries you create in this project that dependent projects also need 57 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 58 | ## DEPENDS: system dependencies of this project that dependent projects also need 59 | #catkin_package( 60 | # INCLUDE_DIRS include 61 | # LIBRARIES rqt_turtle 62 | # CATKIN_DEPENDS roscpp rqt_gui rqt_gui_cpp 63 | # DEPENDS system_lib 64 | #) 65 | # Call the macro without arguments because we don't want to export anything 66 | # Note that this macro is required if catkin_install_python() is used! 67 | # https://stackoverflow.com/questions/56801645/ros1-catkin-make-failed-catkin-install-python-called-without-required-destina 68 | catkin_package() 69 | 70 | ########### 71 | ## Build ## 72 | ########### 73 | 74 | ## Specify additional locations of header files 75 | ## Your package locations should be listed before other locations 76 | include_directories( 77 | # include 78 | ${catkin_INCLUDE_DIRS} 79 | ) 80 | 81 | ## Declare a C++ library 82 | # add_library(${PROJECT_NAME} 83 | # src/${PROJECT_NAME}/rqt_turtle.cpp 84 | # ) 85 | 86 | ## Add cmake target dependencies of the library 87 | ## as an example, code may need to be generated before libraries 88 | ## either from message generation or dynamic reconfigure 89 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 90 | 91 | ## Declare a C++ executable 92 | ## With catkin_make all packages are built within a single CMake context 93 | ## The recommended prefix ensures that target names across packages don't collide 94 | # add_executable(${PROJECT_NAME}_node src/rqt_turtle_node.cpp) 95 | 96 | ## Rename C++ executable without prefix 97 | ## The above recommended prefix causes long target names, the following renames the 98 | ## target back to the shorter version for ease of user use 99 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 100 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 101 | 102 | ## Add cmake target dependencies of the executable 103 | ## same as for the library above 104 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 105 | 106 | ## Specify libraries to link a library or executable target against 107 | # target_link_libraries(${PROJECT_NAME}_node 108 | # ${catkin_LIBRARIES} 109 | # ) 110 | 111 | 112 | set(rqt_turtle_SRCS 113 | src/rqt_turtle/turtle_plugin.cpp 114 | src/rqt_turtle/service_caller.cpp 115 | src/rqt_turtle/draw.cpp 116 | src/rqt_turtle/turtle.cpp 117 | src/rqt_turtle/action_worker.cpp 118 | src/rqt_turtle/image_worker.cpp 119 | ) 120 | 121 | set(rqt_turtle_HDRS 122 | include/rqt_turtle/turtle_plugin.h 123 | include/rqt_turtle/service_caller.h 124 | include/rqt_turtle/draw.h 125 | include/rqt_turtle/turtle.h 126 | include/rqt_turtle/action_worker.h 127 | include/rqt_turtle/image_worker.h 128 | ) 129 | 130 | set(rqt_turtle_UIS 131 | resources/turtle_plugin.ui 132 | resources/ServiceCaller.ui 133 | resources/Draw.ui 134 | resources/Task.ui 135 | ) 136 | 137 | set(rqt_turtle_INCLUDE_DIRECTORIES 138 | include 139 | "${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_INCLUDE_DESTINATION}" 140 | ) 141 | if(NOT EXISTS "${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_INCLUDE_DESTINATION}") 142 | file(MAKE_DIRECTORY "${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_INCLUDE_DESTINATION}") 143 | endif() 144 | 145 | 146 | qt5_wrap_cpp(rqt_turtle_MOCS 147 | ${rqt_turtle_HDRS} 148 | ) 149 | 150 | # ensure generated header files are being created in the devel space 151 | set(_cmake_current_binary_dir "${CMAKE_CURRENT_BINARY_DIR}") 152 | set(CMAKE_CURRENT_BINARY_DIR "${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_INCLUDE_DESTINATION}") 153 | 154 | qt5_wrap_ui(rqt_turtle_UIS_H 155 | ${rqt_turtle_UIS} 156 | ) 157 | 158 | message(${CMAKE_CURRENT_BINARY_DIR}) 159 | 160 | set(CMAKE_CURRENT_BINARY_DIR "${_cmake_current_binary_dir}") 161 | 162 | 163 | include_directories(${rqt_turtle_INCLUDE_DIRECTORIES} ${catkin_INCLUDE_DIRS}) 164 | add_library(${PROJECT_NAME} ${rqt_turtle_SRCS} ${rqt_turtle_MOCS} ${rqt_turtle_UIS_H}) 165 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} Qt5::Widgets ${OpenCV_LIBS}) 166 | 167 | 168 | # TODO use? 169 | find_package(class_loader) 170 | class_loader_hide_library_symbols(${PROJECT_NAME}) 171 | 172 | ############# 173 | ## Install ## 174 | ############# 175 | 176 | # TODO???? 177 | # See http://wiki.ros.org/rqt/Tutorials/Create%20your%20new%20rqt%20plugin 178 | # And https://github.com/ros-visualization/rqt_image_view/blob/master/CMakeLists.txt 179 | # TODO????? 180 | #Add the following lines to call the resource and plugin.xml # TODO required? 181 | #install(DIRECTORY include/${PROJECT_NAME}/ 182 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 183 | #) 184 | #install(DIRECTORY 185 | # resource 186 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 187 | #) 188 | #install(FILES 189 | # plugin.xml 190 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 191 | #) 192 | 193 | # all install targets should use catkin DESTINATION variables 194 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 195 | 196 | ## Mark executable scripts (Python etc.) for installation 197 | ## in contrast to setup.py, you can choose the destination 198 | # For our (optional) script to be installed to the right location, 199 | # if users install your package, this line is required 200 | catkin_install_python(PROGRAMS scripts/rqt_turtle 201 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 202 | ) 203 | 204 | ## Mark executables for installation 205 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 206 | # install(TARGETS ${PROJECT_NAME}_node 207 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 208 | # ) 209 | 210 | ## Mark libraries for installation 211 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 212 | install(TARGETS ${PROJECT_NAME} 213 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 214 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 215 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 216 | ) 217 | 218 | ## Mark cpp header files for installation 219 | # install(DIRECTORY include/${PROJECT_NAME}/ 220 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 221 | # FILES_MATCHING PATTERN "*.h" 222 | # PATTERN ".svn" EXCLUDE 223 | # ) 224 | 225 | ## Mark other files for installation (e.g. launch and bag files, etc.) 226 | # install(FILES 227 | # # myfile1 228 | # # myfile2 229 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 230 | # ) 231 | 232 | ############# 233 | ## Testing ## 234 | ############# 235 | 236 | ## Add gtest based cpp test target and link libraries 237 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_rqt_turtle.cpp) 238 | # if(TARGET ${PROJECT_NAME}-test) 239 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 240 | # endif() 241 | 242 | ## Add folders to be run by python nosetests 243 | # catkin_add_nosetests(test) 244 | -------------------------------------------------------------------------------- /rqt_turtle/include/rqt_turtle/action_worker.h: -------------------------------------------------------------------------------- 1 | #ifndef rqt_turtle__action_worker_H 2 | #define rqt_turtle__action_worker_H 3 | 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | 15 | 16 | namespace rqt_turtle { 17 | 18 | class ActionWorkerKilledException{}; 19 | 20 | 21 | class ActionWorker : public QObject, public QRunnable 22 | { 23 | Q_OBJECT 24 | 25 | actionlib::SimpleActionClient& ac_; 26 | int edges_; 27 | float radius_; 28 | float timeout_; 29 | 30 | bool is_killed_; 31 | 32 | public: 33 | ActionWorker(actionlib::SimpleActionClient& ac, int edges, float radius, float timeout); 34 | 35 | 36 | void run() override; 37 | 38 | signals: 39 | void progress(int value); 40 | 41 | public slots: 42 | void kill(); 43 | }; 44 | 45 | } 46 | 47 | #endif // rqt_turtle__action_worker_H -------------------------------------------------------------------------------- /rqt_turtle/include/rqt_turtle/draw.h: -------------------------------------------------------------------------------- 1 | #ifndef rqt_turtle__draw_H 2 | #define rqt_turtle__draw_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include "opencv2/imgproc.hpp" 10 | #include "opencv2/highgui.hpp" 11 | 12 | #include 13 | #include 14 | 15 | #include 16 | 17 | namespace Ui { 18 | class DrawWidget; 19 | class TaskWidget; 20 | } 21 | 22 | namespace rqt_turtle { 23 | 24 | 25 | class Draw : public QDialog 26 | { 27 | Q_OBJECT 28 | public: 29 | Draw(QWidget* parent, QMap>& turtles); 30 | 31 | private: 32 | Ui::DrawWidget* ui_; 33 | QDialog* draw_dialog_; 34 | 35 | Ui::TaskWidget* ui_task_; 36 | QDialog* task_dialog_; 37 | 38 | QString file_name_; 39 | float turtlesim_size_; 40 | 41 | cv::Mat img_src_; 42 | cv::Mat img_src_gray_; 43 | cv::Mat img_canny_; 44 | int low_threshold_; 45 | 46 | QMap>& turtles_; 47 | QVector turtle_workers_; 48 | std::vector > contours_; 49 | 50 | // create the action client 51 | // true causes the client to spin its own thread 52 | // http://docs.ros.org/noetic/api/actionlib/html/classactionlib_1_1SimpleActionClient.html 53 | actionlib::SimpleActionClient ac_; 54 | bool cancel_goal_; 55 | 56 | QThreadPool threadpool_; 57 | QMap image_worker_progress_; 58 | QSharedPointer timer_; 59 | 60 | cv::Mat resizeImage(const cv::Mat& img); 61 | 62 | void drawShape(); 63 | void previewEdgeImage(); 64 | void drawImage(); 65 | 66 | 67 | void setImage(const QImage &image); 68 | void setEdgeImage(const cv::Mat& image); 69 | 70 | void cannyThreshold(int pos); 71 | 72 | private slots: 73 | void on_btnDraw_clicked(); 74 | void on_btnCancel_clicked(); 75 | 76 | void on_btnOpen_clicked(); 77 | 78 | void on_btnCancelGoal_clicked(); 79 | 80 | void on_sliderLowThreshold_valueChanged(int low_threshold); 81 | 82 | void update_image_progress(QString name, int progress); 83 | int calculate_progress(); 84 | void refresh_progress(); 85 | void cleanup_image_workers(QString); 86 | void cancelDrawImage(); 87 | }; 88 | 89 | } // namespace 90 | 91 | #endif // rqt_turtle__draw_H -------------------------------------------------------------------------------- /rqt_turtle/include/rqt_turtle/image_worker.h: -------------------------------------------------------------------------------- 1 | #ifndef rqt_turtle__image_worker_H 2 | #define rqt_turtle__image_worker_H 3 | 4 | //import sys 5 | //import time 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | #include 13 | 14 | #include 15 | 16 | 17 | namespace rqt_turtle { 18 | 19 | class ImageWorkerKilledException{}; 20 | 21 | 22 | class ImageWorker : public QObject, public QRunnable 23 | { 24 | Q_OBJECT 25 | 26 | bool is_killed_; 27 | 28 | Turtle turtle_; 29 | std::vector > contours_; 30 | int num_contours_; 31 | int num_points_; 32 | int idx_contour_; 33 | int idx_point_; 34 | int percent_; 35 | float turtlesim_size_; 36 | 37 | turtlesim::TeleportAbsolute sTeleportAbs_; 38 | 39 | public: 40 | ImageWorker(Turtle turtle, std::vector > contours, float turtlesim_size = 500.0); 41 | 42 | 43 | void run() override; 44 | 45 | signals: 46 | void progress(QString name, int value); 47 | void finished(QString name); 48 | 49 | public slots: 50 | void kill(); 51 | }; 52 | 53 | } 54 | 55 | #endif // rqt_turtle__image_worker_H -------------------------------------------------------------------------------- /rqt_turtle/include/rqt_turtle/service_caller.h: -------------------------------------------------------------------------------- 1 | #ifndef rqt_turtle__service_caller_H 2 | #define rqt_turtle__service_caller_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | class QListWidgetItem; 10 | 11 | namespace Ui { 12 | class ServiceCallerWidget; 13 | } 14 | 15 | namespace rqt_turtle { 16 | 17 | 18 | class ServiceCaller : public QDialog 19 | { 20 | Q_OBJECT 21 | public: 22 | ServiceCaller(QWidget* parent, std::string service_name); 23 | 24 | QString getTurtleName(); 25 | 26 | QVariantMap getRequest(); 27 | 28 | 29 | 30 | private: 31 | /** 32 | * @brief Execute a command in the terminal and get the string result. 33 | * 34 | * @param cmd The command to execute. 35 | * @return std::string The output of the executed command. 36 | */ 37 | std::string exec_cmd(std::string str_cmd); 38 | 39 | 40 | void createTreeItems(std::string service_name); 41 | 42 | 43 | Ui::ServiceCallerWidget* m_pUi; 44 | QDialog* m_pServiceCallerDialog; 45 | std::string service_name_; 46 | 47 | 48 | private slots: 49 | void on_btnCall_clicked(); 50 | }; 51 | 52 | } // namespace 53 | 54 | #endif // rqt_turtle__service_caller_H -------------------------------------------------------------------------------- /rqt_turtle/include/rqt_turtle/turtle.h: -------------------------------------------------------------------------------- 1 | #ifndef rqt_turtle__turtle_H 2 | #define rqt_turtle__turtle_H 3 | 4 | #include 5 | 6 | #include 7 | 8 | #include 9 | #include 10 | 11 | #define DEFAULT_PEN_R 0xb3 12 | #define DEFAULT_PEN_G 0xb8 13 | #define DEFAULT_PEN_B 0xff 14 | 15 | #define DEFAULT_PEN_WIDTH 3 16 | 17 | class Turtle 18 | { 19 | public: 20 | Turtle(std::string name); 21 | Turtle(std::string name, turtlesim::Pose pose); 22 | Turtle(std::string name, turtlesim::Pose pose, turtlesim::SetPenRequest pen); 23 | Turtle(std::string name, float x, float y, float theta, 24 | uint8_t r = DEFAULT_PEN_R, uint8_t g = DEFAULT_PEN_G, uint8_t b = DEFAULT_PEN_B, uint8_t width = 3, bool off = false); 25 | 26 | QTreeWidgetItem* toTreeItem(QTreeWidget* parent); 27 | 28 | void setPen(bool off); 29 | 30 | std::string name_; 31 | turtlesim::Pose pose_; 32 | turtlesim::SetPenRequest pen_; 33 | 34 | }; 35 | 36 | #endif // rqt_turtle__turtle_H -------------------------------------------------------------------------------- /rqt_turtle/include/rqt_turtle/turtle_plugin.h: -------------------------------------------------------------------------------- 1 | #ifndef rqt_turtle__turtle_plugin_H 2 | #define rqt_turtle__turtle_plugin_H 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | #include 13 | 14 | 15 | class QListWidgetItem; 16 | 17 | namespace Ui { 18 | class TurtlePluginWidget; 19 | } 20 | 21 | namespace rqt_turtle { 22 | 23 | class ServiceCaller; 24 | class Draw; 25 | 26 | 27 | class TurtlePlugin 28 | : public rqt_gui_cpp::Plugin 29 | { 30 | Q_OBJECT 31 | public: 32 | /** 33 | * @brief Construct a new Turtle Plugin object 34 | * 35 | * @details All qt signal to slot connections are done here. 36 | */ 37 | TurtlePlugin(); 38 | 39 | /** 40 | * @brief Setup the plugin and the signal and slot connections. 41 | * 42 | * @param context 43 | */ 44 | virtual void initPlugin(qt_gui_cpp::PluginContext& context); 45 | virtual void shutdownPlugin(); 46 | virtual void saveSettings(qt_gui_cpp::Settings& plugin_settings, qt_gui_cpp::Settings& instance_settings) const; 47 | virtual void restoreSettings(const qt_gui_cpp::Settings& plugin_settings, const qt_gui_cpp::Settings& instance_settings); 48 | 49 | // Comment in to signal that the plugin has a way to configure it 50 | //bool hasConfiguration() const; 51 | //void triggerConfiguration(); 52 | private: 53 | /// Pointer to reference the main ui widgets of the turtle_plugin.ui 54 | Ui::TurtlePluginWidget* ui_; 55 | /// The main widget that will be used to setup the ui_ 56 | QWidget* widget_; 57 | 58 | /// Pointer to the ServiceCaller class dialog 59 | QSharedPointer service_caller_dialog_; 60 | /// Pointer to the ServiceCaller class dialog 61 | QSharedPointer draw_dialog_; 62 | 63 | 64 | /// Storing the currently selected turtles present in the treeTrutles QTreeWdiget. 65 | QVector selected_turtles_; 66 | 67 | /// Vector to keep track of all turtles (keep turtles on the heap using vector of shared pointers) 68 | QMap > turtles_; 69 | 70 | 71 | /** 72 | * @brief Teleport selected turtle(s) 73 | * 74 | * @details teleport is used for both slots, on_btnTeleportAbs_clicked and 75 | * on_btnTeleportRel_clicked. It will create a new ServiceCaller widget, 76 | * which allows to enter the coordinates where the turtle should be teleported. 77 | * Depending on the pressed button ('Teleport Abs' or 'Teleport Rel'), either 78 | * the '/teleport_absoulte' or '/teleport_relative' service will be executed with the 79 | * provided coordinates. 80 | * 81 | * @param teleport_type Can be one of /teleport_absolute or /teleport_relative 82 | */ 83 | QVariantMap teleport(std::string teleport_type); 84 | 85 | void setPen(QSharedPointer turtle); 86 | 87 | void updateTurtleTree(); 88 | 89 | 90 | inline std::string str(QString qstr) { return qstr.toStdString(); }; 91 | 92 | private slots: 93 | /** 94 | * @brief Callback for Reset push button 95 | * 96 | * @details Pressing the Reset button will call the '/reset' service 97 | * and clear the QTreeWidget holding the currently active turtles. 98 | * 99 | */ 100 | void on_btnReset_clicked(); 101 | 102 | /** 103 | * @brief Callback/Slot for Spawn button to create a new turtle. 104 | * 105 | * @details This opens a new ServiceCaller widget with information 106 | * to spawn a new turtle (x, y, theta, name). Press the Call button 107 | * to create the a new turtle with the specified pose. 108 | * 109 | */ 110 | void on_btnSpawn_clicked(); 111 | 112 | /** 113 | * @brief Change the background color of turtlesim 114 | * 115 | * @details Pressing the Color button opens a QColorDialog. 116 | * After selecting the color a /reset is needed to actually update the 117 | * background color of turtlesim. For this the Reset push button can be used. 118 | * 119 | */ 120 | void on_btnColor_clicked(); 121 | void on_btnDraw_clicked(); 122 | 123 | /** 124 | * @brief Callback/Slot to kill/delete the selected turtles. 125 | * 126 | * @details For all turtles currently selected in the QTreeWidget, 127 | * the /kill service is called and their entry is removed 128 | * form the QTreeWidget. 129 | * 130 | */ 131 | void on_btnKill_clicked(); 132 | 133 | /** 134 | * @brief Callback/Slot for Teleport Abs push button. 135 | * 136 | * @details This will open a ServiceCaller widget 137 | * to enter the absolute coordinates where the selected turtles 138 | * should be teleported. The main logic is inside the teleport() method, 139 | * which is reused by on_btnTeleportRel_clicked(). 140 | * 141 | */ 142 | void on_btnTeleportAbs_clicked(); 143 | 144 | /** 145 | * @brief Callback/Slot for Teleport Abs push button 146 | * 147 | * @details This will open a ServiceCaller widget 148 | * to enter the absolute coordinates where the selected turtles 149 | * should be teleported. The main logic is inside the teleport() method, 150 | * which is reused by on_btnTeleportAbs_clicked(). 151 | * 152 | */ 153 | void on_btnTeleportRel_clicked(); 154 | 155 | 156 | void on_btnTogglePen_clicked(); 157 | 158 | /** 159 | * @brief Keeps track of the slected turtles 160 | * 161 | * @details When selecting different turtles in the QTreeWidget, 162 | * the member selected_turtles_ storing the selected turtles is updated. 163 | * 164 | */ 165 | void on_selection_changed(); 166 | }; 167 | 168 | } // namespace 169 | 170 | #endif // rqt_turtle__turtle_plugin_H -------------------------------------------------------------------------------- /rqt_turtle/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rqt_turtle 4 | 0.0.0 5 | rqt plugin for ROS rqt to draw in turtlesim using turtlebot. 6 | 7 | 8 | 9 | 10 | Franz Pucher 11 | 12 | 13 | 14 | 15 | 16 | MIT 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | Franz Pucher 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | catkin 53 | roscpp 54 | rqt_gui 55 | rqt_gui_cpp 56 | actionlib 57 | turtle_actionlib 58 | roscpp 59 | rqt_gui 60 | rqt_gui_cpp 61 | actionlib 62 | turtle_actionlib 63 | roscpp 64 | rqt_gui 65 | rqt_gui_cpp 66 | actionlib 67 | turtle_actionlib 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | -------------------------------------------------------------------------------- /rqt_turtle/plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | Plugin for ROS rqt to draw in turtlesim using turtlebot. 7 | 8 | 9 | 10 | 11 | 12 | folder 13 | Plugins related to robot tools. 14 | 15 | 16 | input-tablet 17 | Plugin for ROS rqt to draw in turtlesim using turtlebot. 18 | 19 | 20 | -------------------------------------------------------------------------------- /rqt_turtle/resources/Draw.ui: -------------------------------------------------------------------------------- 1 | 2 | 3 | DrawWidget 4 | 5 | 6 | 7 | 0 8 | 0 9 | 755 10 | 379 11 | 12 | 13 | 14 | Draw 15 | 16 | 17 | 18 | 19 | 20 | 1 21 | 22 | 23 | 24 | Turtle Action 25 | 26 | 27 | 28 | 29 | 30 | Edges 31 | 32 | 33 | 34 | 35 | 36 | 37 | 5 38 | 39 | 40 | 41 | 42 | 43 | 44 | Radius 45 | 46 | 47 | 48 | 49 | 50 | 51 | 1.5 52 | 53 | 54 | 55 | 56 | 57 | 58 | Time out 59 | 60 | 61 | 62 | 63 | 64 | 65 | 30.0 66 | 67 | 68 | 69 | 70 | 71 | 72 | true 73 | 74 | 75 | true 76 | 77 | 78 | <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> 79 | <html><head><meta name="qrichtext" content="1" /><style type="text/css"> 80 | p, li { white-space: pre-wrap; } 81 | </style></head><body style=" font-family:'Ubuntu'; font-size:11pt; font-weight:400; font-style:normal;"> 82 | <p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Note that turtle_action will only work for one turtle named turtle1. Use topic_tools relay to republish to a different turtle name.</p></body></html> 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | Image 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | Open 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 0 109 | 0 110 | 111 | 112 | 113 | Low Threshold 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 0 124 | 0 125 | 126 | 127 | 128 | Value 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 0 137 | 0 138 | 139 | 140 | 141 | 50 142 | 143 | 144 | Qt::Horizontal 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 1 159 | 1 160 | 161 | 162 | 163 | Qt::Horizontal 164 | 165 | 166 | 167 | 168 | 1 169 | 0 170 | 171 | 172 | 173 | Original Image 174 | 175 | 176 | 177 | 178 | 179 | 1 180 | 0 181 | 182 | 183 | 184 | Edge Image 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | Call selected service 199 | 200 | 201 | Cancel 202 | 203 | 204 | 205 | 16 206 | 16 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | Call selected service 215 | 216 | 217 | Draw 218 | 219 | 220 | 221 | 16 222 | 16 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 234 | -------------------------------------------------------------------------------- /rqt_turtle/resources/ServiceCaller.ui: -------------------------------------------------------------------------------- 1 | 2 | 3 | ServiceCallerWidget 4 | 5 | 6 | 7 | 0 8 | 0 9 | 500 10 | 247 11 | 12 | 13 | 14 | Service Caller 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 0 24 | 0 25 | 26 | 27 | 28 | Service 29 | 30 | 31 | 32 | 33 | 34 | 35 | Call selected service 36 | 37 | 38 | Call 39 | 40 | 41 | 42 | 16 43 | 16 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | Request 56 | 57 | 58 | 59 | 60 | 61 | 62 | Qt::CustomContextMenu 63 | 64 | 65 | Right click on item for more options. 66 | 67 | 68 | true 69 | 70 | 71 | QAbstractItemView::DragOnly 72 | 73 | 74 | QAbstractItemView::ExtendedSelection 75 | 76 | 77 | 78 | Topic 79 | 80 | 81 | 82 | 83 | Type 84 | 85 | 86 | 87 | 88 | Expression 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | -------------------------------------------------------------------------------- /rqt_turtle/resources/Task.ui: -------------------------------------------------------------------------------- 1 | 2 | 3 | TaskWidget 4 | 5 | 6 | 7 | 0 8 | 0 9 | 356 10 | 87 11 | 12 | 13 | 14 | Task 15 | 16 | 17 | 18 | 19 | 20 | 24 21 | 22 | 23 | 24 | 25 | 26 | 27 | TextLabel 28 | 29 | 30 | 31 | 32 | 33 | 34 | Cancel 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /rqt_turtle/resources/turtle_plugin.ui: -------------------------------------------------------------------------------- 1 | 2 | 3 | TurtlePluginWidget 4 | 5 | 6 | 7 | 0 8 | 0 9 | 648 10 | 332 11 | 12 | 13 | 14 | Turtlesim 15 | 16 | 17 | 18 | 19 | 20 | Spawn 21 | 22 | 23 | 24 | 25 | 26 | 27 | Turtles 28 | 29 | 30 | 31 | 32 | 33 | QAbstractItemView::MultiSelection 34 | 35 | 36 | 37 | Name 38 | 39 | 40 | 41 | 42 | x 43 | 44 | 45 | 46 | 47 | y 48 | 49 | 50 | 51 | 52 | theta 53 | 54 | 55 | 56 | 57 | pen 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | Draw 66 | 67 | 68 | 69 | 70 | 71 | 72 | Teleport Abs 73 | 74 | 75 | 76 | 77 | 78 | 79 | Teleport Rel 80 | 81 | 82 | 83 | 84 | 85 | 86 | Kill 87 | 88 | 89 | 90 | 91 | 92 | 93 | Toggle Pen 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | Reset 104 | 105 | 106 | 107 | 108 | 109 | 110 | Color 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | -------------------------------------------------------------------------------- /rqt_turtle/scripts/rqt_turtle: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import sys 4 | 5 | from rqt_gui.main import Main 6 | 7 | 8 | def add_arguments(parser): 9 | group = parser.add_argument_group('Options for rqt_turtle plugin') 10 | group.add_argument('topic', nargs='?', help='The topic name to subscribe to') 11 | 12 | main = Main() 13 | sys.exit(main.main( 14 | sys.argv, standalone='rqt_turtle/TurtlePlugin', 15 | plugin_argument_provider=add_arguments)) -------------------------------------------------------------------------------- /rqt_turtle/setup.py: -------------------------------------------------------------------------------- 1 | ## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD 2 | 3 | from distutils.core import setup 4 | from catkin_pkg.python_setup import generate_distutils_setup 5 | 6 | # fetch values from package.xml 7 | setup_args = generate_distutils_setup( 8 | packages=['rqt_turtle'], 9 | package_dir={'': 'src'}, 10 | scripts=['scripts/rqt_turtle'] 11 | ) 12 | 13 | setup(**setup_args) -------------------------------------------------------------------------------- /rqt_turtle/src/rqt_turtle/action_worker.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace rqt_turtle { 4 | 5 | ActionWorker::ActionWorker(actionlib::SimpleActionClient& ac, int edges, float radius, float timeout) 6 | : ac_(ac) 7 | { 8 | edges_ = edges; 9 | radius_ = radius; 10 | timeout_ = timeout; 11 | is_killed_ = false; 12 | } 13 | 14 | void ActionWorker::run() 15 | { 16 | try 17 | { 18 | /// Send a goal to the action 19 | turtle_actionlib::ShapeGoal shape; 20 | shape.edges = edges_; 21 | shape.radius = radius_; 22 | 23 | /// Cancel action if it takes too long 24 | ros::Time end = ros::Time::now() + ros::Duration(timeout_); 25 | 26 | ac_.sendGoal(shape); 27 | 28 | actionlib::SimpleClientGoalState state = ac_.getState(); 29 | bool loop = true; 30 | ros::Rate rate(1); 31 | while (loop) 32 | { 33 | if (true == is_killed_) /// cancel_goal_ 34 | { 35 | ac_.cancelGoal(); 36 | ROS_INFO("Goal canceled"); 37 | break; 38 | } 39 | state = ac_.getState(); 40 | switch (state.state_) 41 | { 42 | case actionlib::SimpleClientGoalState::PENDING: 43 | ROS_INFO("PENDING - The goal has yet to be processed by the action server."); 44 | break; 45 | case actionlib::SimpleClientGoalState::ACTIVE: 46 | ROS_INFO("ACTIVE - The goal is currently being processed by the action server."); 47 | break; 48 | case actionlib::SimpleClientGoalState::RECALLED: 49 | is_killed_ = true; 50 | ROS_INFO("RECALLED - The goal has not been processed and a cancel request has been received from the action client, \ 51 | but the action server has not confirmed the goal is canceled."); 52 | break; 53 | case actionlib::SimpleClientGoalState::REJECTED: 54 | is_killed_ = true; 55 | ROS_INFO("REJECTED - The goal was rejected by the action server without being processed and without a request from \ 56 | the action client to cancel"); 57 | break; 58 | case actionlib::SimpleClientGoalState::PREEMPTED: 59 | is_killed_ = true; 60 | ROS_INFO("PREEMPTED - The goal is being processed, and a cancel request has been received from the action client, \ 61 | but the action server has not confirmed the goal is canceled."); 62 | break; 63 | case actionlib::SimpleClientGoalState::ABORTED: 64 | is_killed_ = true; 65 | ROS_INFO("ABORTED - The goal was terminated by the action server without an external request from the action client to cancel."); 66 | break; 67 | case actionlib::SimpleClientGoalState::SUCCEEDED: 68 | loop = false; 69 | ROS_INFO("SUCCEEDED - The goal was achieved successfully by the action server."); 70 | break; 71 | case actionlib::SimpleClientGoalState::LOST: 72 | is_killed_ = true; 73 | ROS_INFO("LOST"); 74 | break; 75 | } 76 | if (ros::Time::now() > end) 77 | { 78 | is_killed_ = true; 79 | ROS_INFO("Time out - Action took too long"); 80 | } 81 | rate.sleep(); 82 | } 83 | 84 | 85 | } 86 | catch (...) 87 | { 88 | ROS_INFO("Killed Action Worker"); 89 | } 90 | } 91 | 92 | void ActionWorker::kill() 93 | { 94 | ROS_INFO("Kill received"); 95 | is_killed_ = true; 96 | } 97 | 98 | } -------------------------------------------------------------------------------- /rqt_turtle/src/rqt_turtle/draw.cpp: -------------------------------------------------------------------------------- 1 | #include "rqt_turtle/draw.h" 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | // ROS releated headers 14 | 15 | // Generated ui header 16 | #include "ui_Draw.h" 17 | #include "ui_Task.h" 18 | 19 | 20 | #include 21 | #include 22 | 23 | 24 | 25 | namespace rqt_turtle { 26 | 27 | Draw::Draw(QWidget* parent, QMap>& turtles) 28 | : ui_(new Ui::DrawWidget) 29 | , ui_task_(new Ui::TaskWidget) 30 | , draw_dialog_(this) 31 | , task_dialog_(new QDialog(this)) 32 | , ac_("turtle_shape", true) 33 | , cancel_goal_(false) 34 | , turtles_(turtles) 35 | { 36 | // give QObjects reasonable names 37 | setObjectName("Draw"); 38 | 39 | ROS_INFO("Initialize Draw Dialog"); 40 | ui_->setupUi(draw_dialog_); 41 | ROS_INFO("Initialize Task Dialog"); 42 | ui_task_->setupUi(task_dialog_); 43 | 44 | //connect(ui_->btnDraw, SIGNAL(clicked()), this, SLOT(on_btnDraw_clicked())); 45 | //connect(ui_->btnCancel, SIGNAL(clicked()), this, SLOT(on_btnCancel_clicked())); 46 | //connect(ui_->btnOpen, SIGNAL(clicked()), this, SLOT(on_btnOpen_clicked())); 47 | 48 | turtlesim_size_ = 500.0; 49 | } 50 | 51 | // https://stackoverflow.com/questions/28562401/resize-an-image-to-a-square-but-keep-aspect-ratio-c-opencv 52 | cv::Mat Draw::resizeImage(const cv::Mat& img) 53 | { 54 | int width = img.cols, 55 | height = img.rows; 56 | 57 | cv::Mat square = cv::Mat::zeros(turtlesim_size_, turtlesim_size_, img.type()); 58 | 59 | int max_dim = (width >= height) ? width : height; 60 | float scale = ((float) turtlesim_size_) / max_dim; 61 | cv::Rect roi; 62 | if (width >= height) 63 | { 64 | roi.width = turtlesim_size_; 65 | roi.x = 0; 66 | roi.height = height * scale; 67 | roi.y = (turtlesim_size_ - roi.height) / 2; 68 | } 69 | else 70 | { 71 | roi.y = 0; 72 | roi.height = turtlesim_size_; 73 | roi.width = width * scale; 74 | roi.x = (turtlesim_size_ - roi.width) / 2; 75 | } 76 | 77 | cv::resize(img, square(roi), roi.size()); 78 | 79 | return square; 80 | } 81 | 82 | void Draw::on_btnDraw_clicked() 83 | { 84 | if (ui_->tabs->currentWidget() == ui_->tabTurtleAction) 85 | { 86 | ROS_INFO("Draw Shape"); 87 | drawShape(); 88 | } 89 | if (ui_->tabs->currentWidget() == ui_->tabImage) 90 | { 91 | ROS_INFO("Draw Image"); 92 | drawImage(); 93 | accept(); 94 | } 95 | //accept(); 96 | } 97 | 98 | void Draw::on_btnCancel_clicked() 99 | { 100 | ROS_INFO("Cancel clicked"); 101 | 102 | reject(); 103 | } 104 | 105 | void Draw::on_btnOpen_clicked() 106 | { 107 | ROS_INFO("Open clicked"); 108 | 109 | file_name_ = QFileDialog::getOpenFileName(this, 110 | tr("Open Image"), QDir::homePath(), tr("Image Files (*.png *.jpg *.bmp)")); 111 | 112 | ROS_INFO("File name %s", file_name_.toStdString().c_str()); 113 | 114 | QImageReader reader(file_name_); 115 | reader.setAutoTransform(true); 116 | const QImage image = reader.read(); 117 | if (image.isNull()) { 118 | QMessageBox::information(this, QGuiApplication::applicationDisplayName(), 119 | tr("Cannot load %1: %2") 120 | .arg(QDir::toNativeSeparators(file_name_), reader.errorString())); 121 | return;// false; 122 | } 123 | 124 | setImage(image); 125 | 126 | ROS_INFO("Find Contours"); 127 | 128 | img_src_ = cv::imread(file_name_.toStdString(), 1); 129 | img_src_ = resizeImage(img_src_); 130 | 131 | if (!img_src_.data) 132 | { 133 | ROS_INFO("No image data"); 134 | return; 135 | } 136 | else 137 | { 138 | ROS_INFO("Image rows, cols: %d, %d", img_src_.rows, img_src_.cols); 139 | } 140 | 141 | //img_dst_.create(img_src_.size(), img_src_.type()); 142 | cv::cvtColor(img_src_, img_src_gray_, cv::COLOR_BGR2GRAY); 143 | on_sliderLowThreshold_valueChanged(50); 144 | 145 | connect(ui_->sliderLowThreshold, SIGNAL(valueChanged(int)), this, SLOT(on_sliderLowThreshold_valueChanged(int))); 146 | 147 | //lowThreshold_ = 0; 148 | //const int max_lowThreshold = 100; 149 | //const char* window_name = "Edge Map"; 150 | //cv::namedWindow(window_name, cv::WINDOW_AUTOSIZE ); 151 | //cv::createTrackbar("Min Threshold:", window_name, &this->lowThreshold_, max_lowThreshold, Draw::trackbarCallback, (void*)(this)); 152 | //cannyThreshold(0); 153 | //cv::waitKey(0); 154 | } 155 | 156 | void Draw::setImage(const QImage &image) 157 | { 158 | //image_ = image; 159 | //ui_->lblImage->setPixmap(QPixmap::fromImage(image).scaledToWidth(ui_->lblImage->width())); 160 | ui_->lblImage->setPixmap(QPixmap::fromImage(image) 161 | .scaled(QSize(turtlesim_size_, turtlesim_size_), Qt::KeepAspectRatio)); 162 | //ui_->lblImage->setScaledContents(true); 163 | //float scaleFactor = 0.5; 164 | 165 | //ui_->lblImage->resize(scaleFactor * ui_->lblImage->pixmap()->size()); 166 | 167 | //if (!fitToWindowAct->isChecked()) 168 | //ui_->lblImage->adjustSize(); 169 | } 170 | 171 | void Draw::setEdgeImage(const cv::Mat& image) 172 | { 173 | // Convert cv::Mat to QImage 174 | // https://stackoverflow.com/questions/5026965/how-to-convert-an-opencv-cvmat-to-qimage 175 | QImage img_detected_edges = QImage((uchar*) image.data, image.cols, image.rows, image.step, QImage::Format_RGB888); 176 | ui_->lblEdgeImage->setPixmap(QPixmap::fromImage(img_detected_edges) 177 | .scaled(QSize(turtlesim_size_, turtlesim_size_), Qt::KeepAspectRatio)); 178 | } 179 | 180 | void Draw::on_sliderLowThreshold_valueChanged(int value) 181 | { 182 | /// Detect edges using Canny 183 | cannyThreshold(value); 184 | previewEdgeImage(); 185 | } 186 | 187 | void Draw::previewEdgeImage() 188 | { 189 | // https://docs.opencv.org/2.4/doc/tutorials/imgproc/shapedescriptors/find_contours/find_contours.html 190 | std::vector hierarchy; 191 | cv::RNG rng(12345); 192 | 193 | /// Find contours 194 | cv::findContours(img_canny_, contours_, hierarchy, cv::RETR_TREE, cv::CHAIN_APPROX_SIMPLE, cv::Point(0, 0)); 195 | 196 | /// Draw contours 197 | cv::Mat drawing = cv::Mat::zeros(img_canny_.size(), CV_8UC3); 198 | ROS_INFO("Low threshold %d yields %d contours", low_threshold_, (int)contours_.size()); 199 | for(int i = 0; i < contours_.size(); i++) 200 | { 201 | cv::Scalar color = cv::Scalar(rng.uniform(0, 255), rng.uniform(0,255), rng.uniform(0,255)); 202 | cv::drawContours(drawing, contours_, i, color, 2, 8, hierarchy, 0, cv::Point()); 203 | } 204 | 205 | setEdgeImage(drawing); 206 | } 207 | 208 | void Draw::drawImage() 209 | { 210 | image_worker_progress_.clear(); 211 | QVector runners; 212 | turtlesim::Spawn spawn; 213 | QStringList names; 214 | for (int i = 0; i < contours_.size(); ++i) 215 | { 216 | QString name = QString("t") + QString::number(i); 217 | names.push_back(name); 218 | Turtle turtle(name.toStdString(), 0.0, 0.0, 0.0); 219 | spawn.request.name = turtle.name_.c_str(); 220 | spawn.request.x = turtle.pose_.x; 221 | spawn.request.y = turtle.pose_.y; 222 | spawn.request.theta = turtle.pose_.theta; 223 | ros::service::call("/spawn", spawn); 224 | std::vector > contours(contours_.begin() + i, contours_.begin() + i + 1); 225 | ROS_INFO("%d contour with %d points", (int)contours.size(), (int)contours[0].size()); 226 | ImageWorker* runner = new ImageWorker(turtle, contours, 500); 227 | runners.push_back(runner); 228 | // TODO implement progress correctly 229 | connect(runner, SIGNAL(progress(QString, int)), this, SLOT(update_image_progress(QString, int))); 230 | connect(runner, SIGNAL(finished(QString)), this, SLOT(cleanup_image_workers(QString))); 231 | connect(ui_task_->btnCancel, SIGNAL(clicked()), runner, SLOT(kill())); 232 | } 233 | 234 | connect(ui_task_->btnCancel, SIGNAL(clicked()), task_dialog_, SLOT(reject())); 235 | connect(ui_task_->btnCancel, SIGNAL(clicked()), this, SLOT(cancelDrawImage())); 236 | 237 | ROS_INFO("Start runners"); 238 | for (auto runner : runners) 239 | { 240 | threadpool_.start(runner); 241 | } 242 | 243 | timer_ = QSharedPointer(new QTimer(this)); 244 | timer_->setInterval(100); 245 | connect(timer_.data(), SIGNAL(timeout()), this, SLOT(refresh_progress())); 246 | timer_->start(); 247 | 248 | task_dialog_->exec(); 249 | } 250 | 251 | 252 | void Draw::update_image_progress(QString name, int progress) 253 | { 254 | ROS_INFO("Update image progress of %s: %d", name.toStdString().c_str(), progress); 255 | image_worker_progress_[name] = progress; 256 | } 257 | 258 | int Draw::calculate_progress() 259 | { 260 | if (image_worker_progress_.isEmpty()) 261 | { 262 | return 0; 263 | } 264 | 265 | auto values = image_worker_progress_.values(); 266 | int sum = 0; 267 | for (auto value : values) 268 | { 269 | sum += value; 270 | } 271 | int progress = (float)sum / (float)image_worker_progress_.size(); 272 | return progress; 273 | } 274 | 275 | void Draw::refresh_progress() 276 | { 277 | //# Calculate total progress. 278 | int progress = calculate_progress(); 279 | ROS_INFO("Refresh progress: %d", progress); 280 | ui_task_->progressBar->setValue(progress); 281 | ui_task_->label->setText(QString("%1 workers").arg(image_worker_progress_.size())); 282 | } 283 | 284 | void Draw::cleanup_image_workers(QString name) 285 | { 286 | auto values = image_worker_progress_.values(); 287 | if (std::all_of(values.constBegin(), values.constEnd(), [](int value){ return 100 == value; })) 288 | { 289 | // Update the progress bar if we've removed a value. 290 | //refresh_progress(); 291 | task_dialog_->accept(); 292 | cancelDrawImage(); 293 | image_worker_progress_.clear(); // Empty the map. 294 | } 295 | } 296 | 297 | void Draw::cancelDrawImage() 298 | { 299 | timer_->stop(); 300 | auto names = image_worker_progress_.keys(); 301 | for (auto name : names) 302 | { 303 | turtlesim::Kill kill; 304 | kill.request.name = name.toStdString(); 305 | ros::service::call("kill", kill); 306 | } 307 | } 308 | 309 | void Draw::cannyThreshold(int low_threshold) 310 | { 311 | low_threshold_ = low_threshold; 312 | const int kernel_size = 3; 313 | const int max_lowThreshold = 100; 314 | const int ratio = 3; 315 | 316 | cv::blur(img_src_gray_, img_canny_, cv::Size(3,3)); 317 | cv::Canny(img_canny_, img_canny_, low_threshold_, low_threshold_*ratio, kernel_size); 318 | } 319 | 320 | void Draw::drawShape() 321 | { 322 | ROS_INFO("Waiting for action server to start."); 323 | 324 | if (!ac_.isServerConnected()) 325 | { 326 | QMessageBox msgBox; 327 | msgBox.setText("Action server not connected"); 328 | msgBox.setInformativeText("Please run 'rosrun turtle_actionlib shape_server' and press Ok or cancel \ 329 | to avoid blocking rqt_turtle gui while wating for shape_server."); 330 | msgBox.setStandardButtons(QMessageBox::Ok | QMessageBox::Cancel); 331 | int ret = msgBox.exec(); 332 | if (ret == QMessageBox::Cancel) 333 | { 334 | return; 335 | } 336 | } 337 | 338 | /// Wait for the action server to start 339 | ac_.waitForServer(); //will wait for infinite time 340 | ROS_INFO("Action server started, sending goal."); 341 | 342 | /// Create ActionWorker which will send a goal to the action server 343 | int edges = ui_->lineEditEdges->text().toInt(); 344 | float radius = ui_->lineEditRadius->text().toFloat(); 345 | float timeout = ui_->lineEditTimeout->text().toFloat(); 346 | ActionWorker* action_worker = new ActionWorker(ac_, edges, radius, timeout); 347 | 348 | ui_->btnDraw->setText(QString("Cancel Goal")); 349 | disconnect(ui_->btnDraw, SIGNAL(clicked()), this, SLOT(on_btnDraw_clicked())); 350 | connect(ui_->btnDraw, SIGNAL(clicked()), action_worker, SLOT(kill())); 351 | connect(ui_->btnDraw, SIGNAL(clicked()), this, SLOT(on_btnCancelGoal_clicked())); 352 | 353 | threadpool_.start(action_worker); 354 | } 355 | 356 | void Draw::on_btnCancelGoal_clicked() 357 | { 358 | ui_->btnDraw->setText("Draw"); 359 | disconnect(ui_->btnDraw, SIGNAL(clicked()), this, SLOT(on_btnCancelGoal_clicked())); 360 | connect(ui_->btnDraw, SIGNAL(clicked()), this, SLOT(on_btnDraw_clicked())); 361 | } 362 | 363 | 364 | } // namespace -------------------------------------------------------------------------------- /rqt_turtle/src/rqt_turtle/image_worker.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace rqt_turtle { 4 | 5 | ImageWorker::ImageWorker(Turtle turtle, std::vector > contours, float turtlesim_size) 6 | : turtle_(turtle) 7 | { 8 | is_killed_ = false; 9 | 10 | contours_ = contours; 11 | num_contours_ = contours.size(); 12 | num_points_ = 0; 13 | for (auto contour : contours_) 14 | { 15 | num_points_ += contour.size(); 16 | } 17 | idx_contour_ = 0; 18 | idx_point_ = 0; 19 | percent_ = 0; 20 | 21 | turtlesim_size_ = turtlesim_size; 22 | } 23 | 24 | void ImageWorker::run() 25 | { 26 | try 27 | { 28 | idx_point_ = 0; 29 | ROS_INFO("%s draws %d contours", turtle_.name_.c_str(), num_contours_); 30 | for (auto contour : contours_) 31 | { 32 | ROS_INFO("%s draws contour %d of %d", turtle_.name_.c_str(), idx_contour_, num_contours_); 33 | turtle_.setPen(true); 34 | for (auto point : contour) 35 | { 36 | percent_ = (int)(100.0 * (float)(idx_point_+1) / (float)num_points_); 37 | emit progress(QString::fromStdString(turtle_.name_), percent_); 38 | 39 | /// Normalize to turtle coordinates and flip on horizontal axis 40 | sTeleportAbs_.request.x = point.x / turtlesim_size_ * 11.0; 41 | sTeleportAbs_.request.y = (point.y / turtlesim_size_ * 11.0) - 11.0 / 2.0; 42 | sTeleportAbs_.request.y = sTeleportAbs_.request.y * -1.0; 43 | sTeleportAbs_.request.y = sTeleportAbs_.request.y + 11.0 / 2.0; 44 | sTeleportAbs_.request.theta = 0.0; // todo use two points to calculate angle 45 | 46 | if (is_killed_) 47 | { 48 | throw ImageWorkerKilledException(); 49 | } 50 | 51 | ros::service::call("/" + turtle_.name_ + "/teleport_absolute", sTeleportAbs_); 52 | auto response = sTeleportAbs_.response; 53 | turtle_.setPen(false); 54 | 55 | idx_point_++; 56 | } 57 | } 58 | } 59 | catch (...) 60 | { 61 | ROS_INFO("Killed ImageWorker %s", turtle_.name_.c_str()); 62 | } 63 | 64 | emit finished(QString::fromStdString(turtle_.name_)); 65 | } 66 | 67 | void ImageWorker::kill() 68 | { 69 | ROS_INFO("Kill received"); 70 | is_killed_ = true; 71 | } 72 | 73 | } -------------------------------------------------------------------------------- /rqt_turtle/src/rqt_turtle/service_caller.cpp: -------------------------------------------------------------------------------- 1 | #include "rqt_turtle/service_caller.h" 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | #include "ui_ServiceCaller.h" 11 | 12 | 13 | 14 | namespace rqt_turtle { 15 | 16 | ServiceCaller::ServiceCaller(QWidget* parent, std::string service_name) 17 | : m_pUi(new Ui::ServiceCallerWidget) 18 | , m_pServiceCallerDialog(this) 19 | , service_name_(service_name) 20 | { 21 | // give QObjects reasonable names 22 | setObjectName("ServiceCaller"); 23 | 24 | ROS_INFO("Initialize ServiceCaller for %s", service_name.c_str()); 25 | 26 | m_pUi->setupUi(m_pServiceCallerDialog); 27 | 28 | connect(m_pUi->btnCall, SIGNAL(clicked()), this, SLOT(on_btnCall_clicked())); 29 | 30 | createTreeItems(service_name); 31 | } 32 | 33 | std::string ServiceCaller::exec_cmd(std::string str_cmd) 34 | { 35 | const char* cmd = str_cmd.c_str(); 36 | std::array buffer; 37 | std::string result; 38 | std::unique_ptr pipe(popen(cmd, "r"), pclose); 39 | if (!pipe) { 40 | throw std::runtime_error("popen() failed!"); 41 | } 42 | while (fgets(buffer.data(), buffer.size(), pipe.get()) != nullptr) { 43 | result += buffer.data(); 44 | } 45 | return result; 46 | } 47 | 48 | void ServiceCaller::createTreeItems(std::string service_name) 49 | { 50 | ROS_INFO("Create Tree Items for service %s", service_name.c_str()); 51 | 52 | // Get service type and args 53 | std::string cmd_type = "rosservice type " + service_name; 54 | ROS_INFO("cmd_type: %s", cmd_type.c_str()); 55 | std::string service_type = exec_cmd(cmd_type); 56 | std::string cmd_args = "rosservice args " + service_name; 57 | std::string service_args = exec_cmd(cmd_args); 58 | QString qstr_service_args_line = QString::fromStdString(service_args); 59 | ROS_INFO("Service args: %s", qstr_service_args_line.toStdString().c_str()); 60 | QStringList qstr_args = qstr_service_args_line.split(QRegExp("\\s+"), QString::SkipEmptyParts); 61 | 62 | // https://doc.qt.io/qt-5/qtreewidget.html#details 63 | QList items; 64 | for (int i = 0; i < qstr_args.size(); ++i) 65 | { 66 | QStringList i_args; 67 | i_args.append(qstr_args[i]); 68 | // TODO type 69 | i_args.append(QString::fromStdString("float32")); 70 | // TODO init value depending on type 71 | i_args.append(QString::fromStdString("0.0")); 72 | QTreeWidgetItem* item = new QTreeWidgetItem(static_cast(nullptr), i_args); 73 | item->setFlags(Qt::ItemIsEditable|Qt::ItemIsEnabled); 74 | items.append(item); 75 | } 76 | m_pUi->request_tree_widget->insertTopLevelItems(0, items); 77 | } 78 | 79 | void ServiceCaller::on_btnCall_clicked() 80 | { 81 | ROS_INFO("Call clicked"); 82 | 83 | accept(); 84 | } 85 | 86 | QVariantMap ServiceCaller::getRequest() 87 | { 88 | QVariantMap map; 89 | 90 | // https://doc.qt.io/archives/qt-4.8/qtreewidgetitemiterator.html#details 91 | QTreeWidgetItemIterator it(m_pUi->request_tree_widget); 92 | while (*it) 93 | { 94 | QString variable = (*it)->text(0); 95 | map[variable] = (*it)->text(2); 96 | ++it; 97 | } 98 | return map; 99 | } 100 | 101 | } // namespace -------------------------------------------------------------------------------- /rqt_turtle/src/rqt_turtle/turtle.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | 6 | Turtle::Turtle(std::string name) 7 | { 8 | name_ = name; 9 | 10 | pose_.x = 0.0; 11 | pose_.y = 0.0; 12 | pose_.theta = 0.0; 13 | 14 | pen_.r = DEFAULT_PEN_R; 15 | pen_.g = DEFAULT_PEN_G; 16 | pen_.b = DEFAULT_PEN_B; 17 | pen_.width = DEFAULT_PEN_WIDTH; 18 | pen_.off = false; 19 | } 20 | 21 | 22 | Turtle::Turtle(std::string name, float x, float y, float theta, 23 | uint8_t r, uint8_t g, uint8_t b, uint8_t width, bool off) 24 | { 25 | ROS_INFO("Created Turlte %s", name.c_str()); 26 | name_ = name; 27 | 28 | pose_.x = x; 29 | pose_.y = y; 30 | pose_.theta = theta; 31 | 32 | pen_.r = r; 33 | pen_.g = g; 34 | pen_.b = b; 35 | pen_.width = width; 36 | pen_.off = off; 37 | } 38 | 39 | 40 | Turtle::Turtle(std::string name, turtlesim::Pose pose) 41 | { 42 | name_ = name; 43 | 44 | pose_ = pose; 45 | 46 | pen_.r = DEFAULT_PEN_R; 47 | pen_.g = DEFAULT_PEN_G; 48 | pen_.b = DEFAULT_PEN_B; 49 | pen_.width = DEFAULT_PEN_WIDTH; 50 | pen_.off = false; 51 | } 52 | 53 | 54 | Turtle::Turtle(std::string name, turtlesim::Pose pose, turtlesim::SetPenRequest pen) 55 | { 56 | name_ = name; 57 | pose_ = pose; 58 | pen_ = pen; 59 | } 60 | 61 | 62 | QTreeWidgetItem* Turtle::toTreeItem(QTreeWidget* parent) 63 | { 64 | QTreeWidgetItem* item = new QTreeWidgetItem(parent); 65 | item->setText(0, QString::fromStdString(name_)); 66 | item->setText(1, QString::number(pose_.x)); 67 | item->setText(2, QString::number(pose_.y)); 68 | item->setText(3, QString::number(pose_.theta)); 69 | item->setText(4, pen_.off ? QString("off") : QString("on")); 70 | return item; 71 | } 72 | 73 | 74 | void Turtle::setPen(bool off) 75 | { 76 | pen_.off = off; 77 | 78 | turtlesim::SetPen set_pen; 79 | set_pen.request.r = pen_.r; 80 | set_pen.request.g = pen_.g; 81 | set_pen.request.b = pen_.b; 82 | set_pen.request.width = pen_.width; 83 | set_pen.request.off = pen_.off; 84 | 85 | std::string service_name = "/" + name_ + "/set_pen"; 86 | ros::service::call(service_name, set_pen); 87 | } -------------------------------------------------------------------------------- /rqt_turtle/src/rqt_turtle/turtle_plugin.cpp: -------------------------------------------------------------------------------- 1 | #include "rqt_turtle/turtle_plugin.h" 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | 19 | #include "ui_turtle_plugin.h" 20 | 21 | #include "rqt_turtle/service_caller.h" 22 | #include "rqt_turtle/draw.h" 23 | 24 | 25 | namespace rqt_turtle { 26 | 27 | TurtlePlugin::TurtlePlugin() 28 | : rqt_gui_cpp::Plugin() 29 | , ui_(new Ui::TurtlePluginWidget) 30 | , widget_(0) 31 | { 32 | // Constructor is called first before initPlugin function, needless to say. 33 | 34 | // give QObjects reasonable names 35 | setObjectName("TurtlePlugin"); 36 | } 37 | 38 | void TurtlePlugin::initPlugin(qt_gui_cpp::PluginContext& context) 39 | { 40 | ROS_INFO("Init rqt_turtle plugin"); 41 | // access standalone command line arguments 42 | QStringList argv = context.argv(); 43 | // create QWidget 44 | widget_ = new QWidget(); 45 | // extend the widget with all attributes and children from UI file 46 | ui_->setupUi(widget_); 47 | // add widget to the user interface 48 | context.addWidget(widget_); 49 | 50 | connect(ui_->btnReset, SIGNAL(clicked()), this, SLOT(on_btnReset_clicked())); 51 | connect(ui_->btnSpawn, SIGNAL(clicked()), this, SLOT(on_btnSpawn_clicked())); 52 | connect(ui_->btnKill, SIGNAL(clicked()), this, SLOT(on_btnKill_clicked())); 53 | connect(ui_->btnColor, SIGNAL(clicked()), this, SLOT(on_btnColor_clicked())); 54 | connect(ui_->btnDraw, SIGNAL(clicked()), this, SLOT(on_btnDraw_clicked())); 55 | connect(ui_->btnTeleportAbs, SIGNAL(clicked()), this, SLOT(on_btnTeleportAbs_clicked())); 56 | connect(ui_->btnTeleportRel, SIGNAL(clicked()), this, SLOT(on_btnTeleportRel_clicked())); 57 | connect(ui_->btnTogglePen, SIGNAL(clicked()), this, SLOT(on_btnTogglePen_clicked())); 58 | 59 | connect(ui_->treeTurtles, SIGNAL(itemSelectionChanged()), 60 | this, SLOT(on_selection_changed())); 61 | 62 | updateTurtleTree(); 63 | } 64 | 65 | void TurtlePlugin::updateTurtleTree() 66 | { 67 | // https://stackoverflow.com/questions/26785675/ros-get-current-available-topic-in-code-not-command 68 | // Use XML-RPC ROS Master API to get the topic names 69 | // Then filter for topics containing pose (which belongs to a turtle) 70 | ros::master::V_TopicInfo master_topics; 71 | ros::master::getTopics(master_topics); 72 | 73 | ros::NodeHandle nh = getNodeHandle(); 74 | for (ros::master::V_TopicInfo::iterator it = master_topics.begin(); it != master_topics.end(); it++) 75 | { 76 | const ros::master::TopicInfo& info = *it; 77 | ROS_INFO_STREAM("topic_" << it - master_topics.begin() << ": " << info.name); 78 | QString topic_name = QString::fromStdString(info.name); 79 | if (topic_name.contains(QString("/pose"))) 80 | { 81 | QStringList topic_name_parts = topic_name.split(QRegExp("\\/"), QString::SkipEmptyParts); 82 | std::string turtle_name = topic_name_parts[0].toStdString(); 83 | ROS_INFO("topic_name_part 0: %s", turtle_name.c_str()); 84 | 85 | // Wait for a single pose message to arrive on the turtlesim::Pose topic 86 | turtlesim::PoseConstPtr pose = ros::topic::waitForMessage(topic_name.toStdString()); 87 | ROS_INFO("Pose received: x: %f, y: %f, theta: %f", pose->x, pose->y, pose->theta); 88 | 89 | // Create new turtle in turtle map 90 | // Note: assume that the pen is toggled on 91 | QSharedPointer turtle = QSharedPointer(new Turtle(turtle_name, *pose)); 92 | turtles_[QString::fromStdString(turtle_name)] = turtle; 93 | } 94 | } 95 | 96 | // Insert the turtles into the QTreeWidget 97 | for (auto turtle : turtles_) 98 | { 99 | ui_->treeTurtles->insertTopLevelItem(0, turtle->toTreeItem(ui_->treeTurtles)); 100 | } 101 | } 102 | 103 | void TurtlePlugin::shutdownPlugin() 104 | { 105 | // TODO unregister all publishers here 106 | } 107 | 108 | void TurtlePlugin::saveSettings(qt_gui_cpp::Settings& plugin_settings, qt_gui_cpp::Settings& instance_settings) const 109 | { 110 | // TODO save intrinsic configuration, usually using: 111 | // instance_settings.setValue(k, v) 112 | } 113 | 114 | void TurtlePlugin::restoreSettings(const qt_gui_cpp::Settings& plugin_settings, const qt_gui_cpp::Settings& instance_settings) 115 | { 116 | // TODO restore intrinsic configuration, usually using: 117 | // v = instance_settings.value(k) 118 | } 119 | 120 | /*bool hasConfiguration() const 121 | { 122 | return true; 123 | } 124 | 125 | void triggerConfiguration() 126 | { 127 | // Usually used to open a dialog to offer the user a set of configuration 128 | }*/ 129 | 130 | void TurtlePlugin::on_btnReset_clicked() 131 | { 132 | ROS_INFO("Reset turtlesim."); 133 | std_srvs::Empty empty; 134 | ros::service::call("reset", empty); 135 | 136 | // Clear the QTreeWidget 137 | ui_->treeTurtles->clear(); 138 | turtles_.clear(); 139 | 140 | updateTurtleTree(); 141 | } 142 | 143 | void TurtlePlugin::on_btnSpawn_clicked() 144 | { 145 | ROS_DEBUG("Spawn clicked"); 146 | std::string service_name = "/spawn"; 147 | service_caller_dialog_ = QSharedPointer(new ServiceCaller(widget_, service_name)); 148 | 149 | QString qstr_turtle_name; 150 | QVariantMap request; 151 | bool ok = service_caller_dialog_->exec() == QDialog::Accepted; 152 | if (ok) 153 | { 154 | ROS_INFO("accepted"); 155 | request = service_caller_dialog_->getRequest(); 156 | qstr_turtle_name = request["name"].toString(); 157 | } 158 | 159 | if (!ok || qstr_turtle_name.isEmpty()) 160 | { 161 | ROS_INFO("Closed Service Caller Dialog or Turtle Name empty."); 162 | return; 163 | } 164 | auto existing_turtles = ui_->treeTurtles->findItems(qstr_turtle_name, Qt::MatchExactly); 165 | const char * str_turtle_name = qstr_turtle_name.toStdString().c_str(); 166 | if (existing_turtles.size() > 0) 167 | { 168 | ROS_INFO("Turtle with the name \"%s\" already exists.", str_turtle_name); 169 | return; 170 | } 171 | 172 | ROS_INFO("Spawn turtle: %s.", str_turtle_name); 173 | turtlesim::Spawn spawn; 174 | spawn.request.x = request["x"].toString().toFloat(); 175 | spawn.request.y = request["y"].toString().toFloat(); 176 | spawn.request.theta = request["theta"].toFloat(); 177 | spawn.request.name = request["name"].toString().toStdString().c_str(); 178 | ros::service::call("spawn", spawn); 179 | 180 | QTreeWidgetItem *item = new QTreeWidgetItem(ui_->treeTurtles); 181 | item->setText(0, qstr_turtle_name); // Column 0 name 182 | item->setText(1, request["x"].toString()); // Column 1 x 183 | item->setText(2, request["y"].toString()); // Column 2 y 184 | item->setText(3, request["theta"].toString()); // Column 3 theta 185 | item->setText(4, QString("on")); // Column 4 pen on/off (pen is always on by default) 186 | ui_->treeTurtles->insertTopLevelItem(0, item); 187 | 188 | // Create new turtle in turtle map 189 | // Note: assume that the pen is toggled on 190 | QSharedPointer turtle = QSharedPointer(new Turtle(qstr_turtle_name.toStdString(), 191 | spawn.request.x, spawn.request.y, spawn.request.theta)); 192 | turtles_[qstr_turtle_name] = turtle; 193 | } 194 | 195 | void TurtlePlugin::on_btnColor_clicked() 196 | { 197 | int r, g, b; 198 | ros::param::get("/turtlesim/background_b", b); 199 | ros::param::get("/turtlesim/background_g", g); 200 | ros::param::get("/turtlesim/background_r", r); 201 | ROS_INFO("Current color (r,g,b) = (%i,%i,%i)", r, g, b); 202 | QColor qColor = QColorDialog::getColor(); 203 | ROS_INFO("Color %s", qColor.name().toStdString().c_str()); 204 | qColor.getRgb(&r, &g, &b); 205 | ROS_INFO("Setting color to (r,g,b) = (%i,%i,%i)", r, g, b); 206 | ros::param::set("/turtlesim/background_b", b); 207 | ros::param::set("/turtlesim/background_g", g); 208 | ros::param::set("/turtlesim/background_r", r); 209 | 210 | // Note: this will not set the color (only after reset is called) 211 | std_srvs::Empty reset; 212 | ros::service::call("/clear", reset); 213 | } 214 | 215 | void TurtlePlugin::on_btnDraw_clicked() 216 | { 217 | draw_dialog_ = QSharedPointer(new Draw(widget_, turtles_)); 218 | 219 | draw_dialog_->open(); 220 | //bool ok = draw_dialog_->exec() == QDialog::Accepted; 221 | bool ok = true; 222 | 223 | // Remove ? 224 | /* 225 | auto list = m_pUi->treeTurtles->selectedItems(); 226 | ROS_INFO("%d", list.size()); 227 | if (list.size() > 0) 228 | { 229 | QString turtleName = list[0]->text(0); 230 | std::vector > contours = draw_dialog_->contours(); 231 | ROS_INFO("Found %d contours", (int)contours.size()); 232 | DrawImage(contours); 233 | } 234 | */ 235 | } 236 | 237 | void TurtlePlugin::on_btnKill_clicked() 238 | { 239 | if (selected_turtles_.empty()) 240 | { 241 | ROS_INFO("No turtles selected"); 242 | return; 243 | } 244 | 245 | for (auto selected_turtle : selected_turtles_) 246 | { 247 | ROS_INFO("Killing turtle %s", str(selected_turtle).c_str()); 248 | turtlesim::Kill kill; 249 | kill.request.name = str(selected_turtle); 250 | ros::service::call("kill", kill); 251 | 252 | // remove turtle from tree widget 253 | QList list = ui_->treeTurtles->findItems(selected_turtle, Qt::MatchExactly); 254 | for (auto item : list) 255 | { 256 | // Remove turtle from turtles_ QMap 257 | turtles_.remove(item->text(0)); 258 | // Remove the turtle from the QTreeWidget treeTurle 259 | delete item; 260 | } 261 | } 262 | } 263 | 264 | void TurtlePlugin::on_btnTeleportAbs_clicked() 265 | { 266 | if (selected_turtles_.empty()) 267 | { 268 | ROS_INFO("No turtles selected"); 269 | return; 270 | } 271 | 272 | // Create ServiceCaller for first selected turtle 273 | auto it_selected_turtle = selected_turtles_.begin(); 274 | std::string turtle_name = str(*it_selected_turtle); 275 | std::string strServiceName = "/" + turtle_name + "/teleport_absolute"; 276 | QVariantMap request = teleport(strServiceName); 277 | 278 | if (request.empty()) 279 | { 280 | return; 281 | } 282 | 283 | for (auto selected_turtle : selected_turtles_) 284 | { 285 | turtle_name = str(selected_turtle); 286 | strServiceName = "/" + turtle_name + "/teleport_absolute"; 287 | 288 | turtlesim::TeleportAbsolute sTeleportAbsolute; 289 | sTeleportAbsolute.request.x = request["x"].toString().toFloat(); 290 | sTeleportAbsolute.request.y = request["y"].toString().toFloat(); 291 | sTeleportAbsolute.request.theta = request["theta"].toString().toFloat(); 292 | ROS_INFO("Teleport %s to x: %f, y: %f, theta: %f", 293 | turtle_name.c_str(), 294 | sTeleportAbsolute.request.x, 295 | sTeleportAbsolute.request.y, 296 | sTeleportAbsolute.request.theta); 297 | ros::service::call(strServiceName, sTeleportAbsolute); 298 | auto response = sTeleportAbsolute.response; 299 | } 300 | } 301 | 302 | void TurtlePlugin::on_btnTeleportRel_clicked() 303 | { 304 | if (selected_turtles_.empty()) 305 | { 306 | ROS_INFO("No turtles selected"); 307 | return; 308 | } 309 | 310 | // Create ServiceCaller for first selected turtle 311 | auto it_selected_turtle = selected_turtles_.begin(); 312 | std::string turtle_name = str(*it_selected_turtle); 313 | std::string strServiceName = "/" + turtle_name + "/teleport_relative"; 314 | QVariantMap request = teleport(strServiceName); 315 | 316 | if (request.empty()) 317 | { 318 | return; 319 | } 320 | 321 | for (auto selected_turtle : selected_turtles_) 322 | { 323 | turtle_name = str(selected_turtle); 324 | strServiceName = "/" + turtle_name + "/teleport_relative"; 325 | 326 | turtlesim::TeleportRelative sTeleportRelative; 327 | sTeleportRelative.request.linear = request["linear"].toString().toFloat(); 328 | sTeleportRelative.request.angular = request["angular"].toString().toFloat(); 329 | ROS_INFO("Teleport %s to linear: %f, angular: %f", 330 | turtle_name.c_str(), 331 | sTeleportRelative.request.linear, 332 | sTeleportRelative.request.angular); 333 | ros::service::call(strServiceName, sTeleportRelative); 334 | auto response = sTeleportRelative.response; 335 | } 336 | } 337 | 338 | 339 | QVariantMap TurtlePlugin::teleport(std::string strServiceName) 340 | { 341 | service_caller_dialog_ = QSharedPointer(new ServiceCaller(widget_, strServiceName)); 342 | 343 | QString qstrTurtleName; 344 | QVariantMap request; 345 | bool ok = service_caller_dialog_->exec() == QDialog::Accepted; 346 | if (ok) 347 | { 348 | ROS_DEBUG("accepted"); 349 | request = service_caller_dialog_->getRequest(); 350 | qstrTurtleName = request["name"].toString(); 351 | } 352 | else 353 | { 354 | ROS_DEBUG("ServiceCaller Dialog closed"); 355 | return QVariantMap(); 356 | } 357 | 358 | return request; 359 | } 360 | 361 | 362 | void TurtlePlugin::on_btnTogglePen_clicked() 363 | { 364 | 365 | if (selected_turtles_.empty()) 366 | { 367 | ROS_INFO("No turtles selected"); 368 | return; 369 | } 370 | 371 | for (auto selected_turtle : selected_turtles_) 372 | { 373 | QSharedPointer turtle = turtles_[selected_turtle]; 374 | if (turtle->pen_.off) 375 | { 376 | turtle->setPen(false); 377 | } 378 | else 379 | { 380 | turtle->setPen(true); 381 | } 382 | 383 | QString qstr_pen_state = turtle->pen_.off ? QString("Off") : QString("On"); 384 | ROS_INFO("Set pen for turtle %s: %s", turtle->name_.c_str(), qstr_pen_state.toStdString().c_str()); 385 | 386 | QList list = ui_->treeTurtles->findItems(selected_turtle, Qt::MatchExactly); 387 | /// Note this will always be a list of one element 388 | for (auto item : list) 389 | { 390 | // Update pen in treeTurtles QTeeWidget 391 | item->setText(4, qstr_pen_state); 392 | } 393 | } 394 | } 395 | 396 | void TurtlePlugin::setPen(QSharedPointer turtle) 397 | { 398 | turtlesim::SetPen set_pen; 399 | set_pen.request.r = turtle->pen_.r; 400 | set_pen.request.g = turtle->pen_.g; 401 | set_pen.request.b = turtle->pen_.b; 402 | set_pen.request.width = turtle->pen_.width; 403 | set_pen.request.off = turtle->pen_.off; 404 | 405 | std::string service_name = "/" + turtle->name_ + "/set_pen"; 406 | ros::service::call(service_name, set_pen); 407 | } 408 | 409 | 410 | void TurtlePlugin::on_selection_changed() 411 | { 412 | //auto current = m_pUi->treeTurtles->currentItem(); // TODO use member list if multiple turtles are selected 413 | // Get list of selected turtles 414 | auto selected_items = ui_->treeTurtles->selectedItems(); 415 | selected_turtles_.clear(); 416 | if (selected_items.empty()) 417 | { 418 | return; 419 | } 420 | //m_strSelectedTurtle = current->text(0).toStdString(); 421 | QString turtle_name; 422 | std::stringstream ss; 423 | for (auto item : selected_items) 424 | { 425 | turtle_name = item->text(0); 426 | selected_turtles_.push_back(turtle_name); 427 | ss << str(turtle_name) << " "; 428 | 429 | } 430 | ROS_INFO("Selected %s", ss.str().c_str()); 431 | 432 | } 433 | 434 | 435 | } // namespace 436 | 437 | // Deprecated 438 | // See: http://wiki.ros.org/pluginlib#pluginlib.2Fpluginlib_groovy.Simplified_Export_Macro 439 | //PLUGINLIB_DECLARE_CLASS(rqt_turtle, TurtlePlugin, rqt_turtle::TurtlePlugin, rqt_gui_cpp::Plugin) 440 | PLUGINLIB_EXPORT_CLASS(rqt_turtle::TurtlePlugin, rqt_gui_cpp::Plugin) --------------------------------------------------------------------------------