diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..dbe9c82 --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +.vscode/ \ No newline at end of file diff --git a/README.md b/README.md index e69de29..693cfbc 100644 --- a/README.md +++ b/README.md @@ -0,0 +1,8 @@ +- Python 3.8.10 +- Tkinter 8.6 +- Pillow 9.2.0 +- ruamel.yaml 0.17.21 + +waypoint_manager 参考 +- https://imagingsolution.net/category/program/python/tkinter/ +- https://daeudaeu.com/tkinter-mousewheel/ diff --git a/orne_rviz_plugins/CMakeLists.txt b/orne_rviz_plugins/CMakeLists.txt new file mode 100644 index 0000000..54a4b6d --- /dev/null +++ b/orne_rviz_plugins/CMakeLists.txt @@ -0,0 +1,52 @@ +cmake_minimum_required(VERSION 2.8.3) +project(orne_rviz_plugins) +find_package(catkin REQUIRED COMPONENTS rviz std_srvs) +catkin_package() +include_directories(${catkin_INCLUDE_DIRS}) +link_directories(${catkin_LIBRARY_DIRS}) + +## This setting causes Qt's "MOC" generation to happen automatically. +set(CMAKE_AUTOMOC ON) + +## This plugin includes Qt widgets, so we must include Qt. +## We'll use the version that rviz used so they are compatible. +if(rviz_QT_VERSION VERSION_LESS "5") + message(STATUS "Using Qt4 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") + find_package(Qt4 ${rviz_QT_VERSION} EXACT REQUIRED QtCore QtGui) + ## pull in all required include dirs, define QT_LIBRARIES, etc. + include(${QT_USE_FILE}) +else() + message(STATUS "Using Qt5 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") + find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets) + ## make target_link_libraries(${QT_LIBRARIES}) pull in all required dependencies + set(QT_LIBRARIES Qt5::Widgets) +endif() + +add_definitions(-DQT_NO_KEYWORDS) + +set(SOURCE_FILES + src/state_trigger_panel.cpp + ${MOC_FILES} +) + +add_library(${PROJECT_NAME} ${SOURCE_FILES}) + +target_link_libraries(${PROJECT_NAME} ${QT_LIBRARIES} ${catkin_LIBRARIES}) + +install(TARGETS + ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(FILES + plugin_description.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + +install(DIRECTORY media/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/media) + +install(DIRECTORY icons/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/icons) + diff --git a/orne_rviz_plugins/icons/classes/Imu.png b/orne_rviz_plugins/icons/classes/Imu.png new file mode 100644 index 0000000..7d90110 --- /dev/null +++ b/orne_rviz_plugins/icons/classes/Imu.png Binary files differ diff --git a/orne_rviz_plugins/icons/classes/PlantFlag.png b/orne_rviz_plugins/icons/classes/PlantFlag.png new file mode 100644 index 0000000..d11c0ee --- /dev/null +++ b/orne_rviz_plugins/icons/classes/PlantFlag.png Binary files differ diff --git a/orne_rviz_plugins/icons/classes/Teleop.png b/orne_rviz_plugins/icons/classes/Teleop.png new file mode 100644 index 0000000..d96e6c1 --- /dev/null +++ b/orne_rviz_plugins/icons/classes/Teleop.png Binary files differ diff --git a/orne_rviz_plugins/media/flag.dae b/orne_rviz_plugins/media/flag.dae new file mode 100644 index 0000000..e56a734 --- /dev/null +++ b/orne_rviz_plugins/media/flag.dae @@ -0,0 +1,323 @@ + + + + + Illusoft Collada 1.4.0 plugin for Blender - http://colladablender.illusoft.com + Blender v:249 - Illusoft Collada Exporter v:0.3.162 + + + file:///u/hersh/flag.blend + + 2011-03-16T15:32:31.374173 + 2011-03-16T15:32:31.374204 + Z_UP + + + + + + + + 0.00000 0.00000 0.00000 1 + + + 0.04706 0.37059 0.44706 1 + + + 0.09412 0.74118 0.89412 1 + + + 0.50000 0.50000 0.50000 1 + + + 12.5 + + + 1.00000 1.00000 1.00000 1 + + + 0.0 + + + 1 1 1 1 + + + 0.5 + + + + + + + + + + + 0.00000 0.00000 0.00000 1 + + + 0.50000 0.00000 0.00000 1 + + + 1.00000 0.00000 0.00000 1 + + + 0.50000 0.50000 0.50000 1 + + + 12.5 + + + 1.00000 1.00000 1.00000 1 + + + 0.0 + + + 1 1 1 1 + + + 0.0 + + + + + + + + + + + 0.00000 0.00000 0.00000 1 + + + 0.50000 0.50000 0.50000 1 + + + 1.00000 1.00000 1.00000 1 + + + 0.50000 0.50000 0.50000 1 + + + 12.5 + + + 1.00000 1.00000 1.00000 1 + + + 0.0 + + + 1 1 1 1 + + + 0.0 + + + + + + + + + + + 1.00000 1.00000 1.00000 + + + + + + + + + + + + + + + + + + + + 0.70711 0.70711 -0.01000 0.83147 0.55557 -0.01000 0.92388 0.38268 -0.01000 0.98079 0.19509 -0.01000 1.00000 0.00000 -0.01000 0.98079 -0.19509 -0.01000 0.92388 -0.38268 -0.01000 0.83147 -0.55557 -0.01000 0.70711 -0.70711 -0.01000 0.55557 -0.83147 -0.01000 0.38268 -0.92388 -0.01000 0.19509 -0.98079 -0.01000 -0.00000 -1.00000 -0.01000 -0.19509 -0.98079 -0.01000 -0.38268 -0.92388 -0.01000 -0.55557 -0.83147 -0.01000 -0.70711 -0.70711 -0.01000 -0.83147 -0.55557 -0.01000 -0.92388 -0.38268 -0.01000 -0.98079 -0.19509 -0.01000 -1.00000 0.00000 -0.01000 -0.98079 0.19509 -0.01000 -0.92388 0.38268 -0.01000 -0.83147 0.55557 -0.01000 -0.70711 0.70711 -0.01000 -0.55557 0.83147 -0.01000 -0.38268 0.92388 -0.01000 -0.19509 0.98079 -0.01000 0.00000 1.00000 -0.01000 0.19509 0.98078 -0.01000 0.38269 0.92388 -0.01000 0.55557 0.83147 -0.01000 0.70711 0.70711 0.01000 0.83147 0.55557 0.01000 0.92388 0.38268 0.01000 0.98079 0.19509 0.01000 1.00000 -0.00000 0.01000 0.98078 -0.19509 0.01000 0.92388 -0.38268 0.01000 0.83147 -0.55557 0.01000 0.70711 -0.70711 0.01000 0.55557 -0.83147 0.01000 0.38268 -0.92388 0.01000 0.19509 -0.98079 0.01000 0.00000 -1.00000 0.01000 -0.19509 -0.98079 0.01000 -0.38268 -0.92388 0.01000 -0.55557 -0.83147 0.01000 -0.70710 -0.70711 0.01000 -0.83147 -0.55557 0.01000 -0.92388 -0.38269 0.01000 -0.98078 -0.19509 0.01000 -1.00000 -0.00000 0.01000 -0.98079 0.19509 0.01000 -0.92388 0.38268 0.01000 -0.83147 0.55557 0.01000 -0.70711 0.70710 0.01000 -0.55558 0.83147 0.01000 -0.38269 0.92388 0.01000 -0.19510 0.98078 0.01000 -0.00001 1.00000 0.01000 0.19508 0.98079 0.01000 0.38268 0.92388 0.01000 0.55556 0.83147 0.01000 0.00000 0.00000 -0.01000 0.00000 0.00000 0.01000 + + + + + + + + + + -0.00000 0.00000 -1.00000 0.00000 0.00000 1.00000 -0.00000 0.00000 -1.00000 0.00000 0.00000 1.00000 -0.00000 0.00000 -1.00000 0.00000 0.00000 1.00000 -0.00000 0.00000 -1.00000 0.00000 0.00000 1.00000 -0.00000 0.00000 -1.00000 0.00000 0.00000 1.00000 0.00000 0.00000 -1.00000 0.00000 0.00000 1.00000 0.00000 0.00000 -1.00000 0.00000 0.00000 1.00000 0.00000 0.00000 -1.00000 0.00000 0.00000 1.00000 0.00000 0.00000 -1.00000 0.00000 0.00000 1.00000 0.00000 0.00000 -1.00000 0.00000 0.00000 1.00000 0.00000 0.00000 -1.00000 0.00000 0.00000 1.00000 0.00000 0.00000 -1.00000 0.00000 0.00000 1.00000 0.00000 0.00000 -1.00000 0.00000 -0.00000 1.00000 0.00000 0.00000 -1.00000 0.00000 -0.00000 1.00000 0.00000 0.00000 -1.00000 0.00000 -0.00000 1.00000 0.00000 0.00000 -1.00000 0.00000 -0.00000 1.00000 0.00000 0.00000 -1.00000 0.00000 -0.00000 1.00000 0.00000 0.00000 -1.00000 0.00000 -0.00000 1.00000 0.00000 0.00000 -1.00000 0.00000 -0.00000 1.00000 0.00000 0.00000 -1.00000 0.00000 -0.00000 1.00000 0.00000 -0.00000 -1.00000 -0.00000 0.00000 1.00000 0.00000 -0.00000 -1.00000 -0.00000 0.00000 1.00000 0.00000 -0.00000 -1.00000 -0.00000 0.00000 1.00000 0.00000 -0.00000 -1.00000 -0.00000 0.00000 1.00000 0.00000 -0.00000 -1.00000 -0.00000 0.00000 1.00000 0.00000 -0.00000 -1.00000 -0.00000 0.00000 1.00000 0.00000 -0.00000 -1.00000 -0.00000 0.00000 1.00000 0.00000 -0.00000 -1.00000 -0.00000 0.00000 1.00000 -0.00000 0.00000 -1.00000 0.00000 0.00000 1.00000 -0.00000 0.00000 -1.00000 0.00000 0.00000 1.00000 -0.00000 0.00000 -1.00000 0.00000 0.00000 1.00000 0.00000 0.00000 -1.00000 0.00000 0.00000 1.00000 0.77301 0.63439 0.00000 0.88192 0.47140 -0.00000 0.95694 0.29028 -0.00000 0.99518 0.09802 -0.00000 0.99518 -0.09802 -0.00000 0.95694 -0.29029 -0.00000 0.88192 -0.47140 -0.00000 0.77301 -0.63439 -0.00000 0.63439 -0.77301 -0.00000 0.47140 -0.88192 -0.00000 0.29028 -0.95694 -0.00000 0.09802 -0.99518 -0.00000 -0.09802 -0.99518 -0.00000 -0.29028 -0.95694 -0.00000 -0.47140 -0.88192 -0.00000 -0.63439 -0.77301 0.00000 -0.77301 -0.63439 0.00000 -0.88192 -0.47140 0.00000 -0.95694 -0.29029 -0.00000 -0.99518 -0.09802 -0.00000 -0.99518 0.09802 -0.00000 -0.95694 0.29028 -0.00000 -0.88192 0.47139 -0.00000 -0.77301 0.63439 -0.00000 -0.63440 0.77301 -0.00000 -0.47140 0.88192 -0.00000 -0.29029 0.95694 -0.00000 -0.09802 0.99518 -0.00000 0.09801 0.99519 -0.00000 0.29028 0.95694 -0.00000 0.47139 0.88192 -0.00000 0.63439 0.77301 0.00003 + + + + + + + + + + 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 1.00000 1.00000 0.00000 1.00000 0.00000 0.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 1.00000 1.00000 0.00000 1.00000 0.00000 0.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 1.00000 1.00000 0.00000 1.00000 0.00000 0.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 1.00000 1.00000 0.00000 1.00000 0.00000 0.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 1.00000 1.00000 0.00000 1.00000 0.00000 0.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 1.00000 1.00000 0.00000 1.00000 0.00000 0.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 1.00000 1.00000 0.00000 1.00000 0.00000 0.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 1.00000 1.00000 0.00000 1.00000 0.00000 0.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 1.00000 1.00000 0.00000 1.00000 0.00000 0.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 1.00000 1.00000 0.00000 1.00000 0.00000 0.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 1.00000 1.00000 0.00000 1.00000 0.00000 0.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 1.00000 1.00000 0.00000 1.00000 0.00000 0.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 1.00000 1.00000 0.00000 1.00000 0.00000 0.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 1.00000 1.00000 0.00000 1.00000 0.00000 0.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 1.00000 1.00000 0.00000 1.00000 0.00000 0.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 1.00000 1.00000 0.00000 1.00000 0.00000 0.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 1.00000 1.00000 0.00000 1.00000 0.00000 0.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 1.00000 1.00000 0.00000 1.00000 0.00000 0.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 1.00000 1.00000 0.00000 1.00000 0.00000 0.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 1.00000 1.00000 0.00000 1.00000 0.00000 0.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 1.00000 1.00000 0.00000 1.00000 0.00000 0.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 1.00000 1.00000 0.00000 1.00000 0.00000 0.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 1.00000 1.00000 0.00000 1.00000 0.00000 0.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 1.00000 1.00000 0.00000 1.00000 0.00000 0.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 1.00000 1.00000 0.00000 1.00000 0.00000 0.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 1.00000 1.00000 0.00000 1.00000 0.00000 0.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 1.00000 1.00000 0.00000 1.00000 0.00000 0.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 1.00000 1.00000 0.00000 1.00000 0.00000 0.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 1.00000 1.00000 0.00000 1.00000 0.00000 0.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 1.00000 1.00000 0.00000 1.00000 0.00000 0.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 1.00000 1.00000 0.00000 1.00000 0.00000 0.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 1.00000 1.00000 0.00000 1.00000 0.00000 0.00000 + + + + + + + + + + + + + + +

64 0 0 0 0 1 1 0 2 65 1 3 33 1 4 32 1 5 64 2 6 1 2 7 2 2 8 65 3 9 34 3 10 33 3 11 64 4 12 2 4 13 3 4 14 65 5 15 35 5 16 34 5 17 64 6 18 3 6 19 4 6 20 65 7 21 36 7 22 35 7 23 64 8 24 4 8 25 5 8 26 65 9 27 37 9 28 36 9 29 64 10 30 5 10 31 6 10 32 65 11 33 38 11 34 37 11 35 64 12 36 6 12 37 7 12 38 65 13 39 39 13 40 38 13 41 64 14 42 7 14 43 8 14 44 65 15 45 40 15 46 39 15 47 64 16 48 8 16 49 9 16 50 65 17 51 41 17 52 40 17 53 64 18 54 9 18 55 10 18 56 65 19 57 42 19 58 41 19 59 64 20 60 10 20 61 11 20 62 65 21 63 43 21 64 42 21 65 64 22 66 11 22 67 12 22 68 65 23 69 44 23 70 43 23 71 64 24 72 12 24 73 13 24 74 65 25 75 45 25 76 44 25 77 64 26 78 13 26 79 14 26 80 65 27 81 46 27 82 45 27 83 64 28 84 14 28 85 15 28 86 65 29 87 47 29 88 46 29 89 64 30 90 15 30 91 16 30 92 65 31 93 48 31 94 47 31 95 64 32 96 16 32 97 17 32 98 65 33 99 49 33 100 48 33 101 64 34 102 17 34 103 18 34 104 65 35 105 50 35 106 49 35 107 64 36 108 18 36 109 19 36 110 65 37 111 51 37 112 50 37 113 64 38 114 19 38 115 20 38 116 65 39 117 52 39 118 51 39 119 64 40 120 20 40 121 21 40 122 65 41 123 53 41 124 52 41 125 64 42 126 21 42 127 22 42 128 65 43 129 54 43 130 53 43 131 64 44 132 22 44 133 23 44 134 65 45 135 55 45 136 54 45 137 64 46 138 23 46 139 24 46 140 65 47 141 56 47 142 55 47 143 64 48 144 24 48 145 25 48 146 65 49 147 57 49 148 56 49 149 64 50 150 25 50 151 26 50 152 65 51 153 58 51 154 57 51 155 64 52 156 26 52 157 27 52 158 65 53 159 59 53 160 58 53 161 64 54 162 27 54 163 28 54 164 65 55 165 60 55 166 59 55 167 64 56 168 28 56 169 29 56 170 65 57 171 61 57 172 60 57 173 64 58 174 29 58 175 30 58 176 65 59 177 62 59 178 61 59 179 64 60 180 30 60 181 31 60 182 65 61 183 63 61 184 62 61 185 31 62 186 0 62 187 64 62 188 65 63 189 32 63 190 63 63 191 0 64 192 32 64 193 33 64 194 33 64 195 1 64 196 0 64 197 1 65 198 33 65 199 34 65 200 34 65 201 2 65 202 1 65 203 2 66 204 34 66 205 35 66 206 35 66 207 3 66 208 2 66 209 3 67 210 35 67 211 36 67 212 36 67 213 4 67 214 3 67 215 4 68 216 36 68 217 37 68 218 37 68 219 5 68 220 4 68 221 5 69 222 37 69 223 38 69 224 38 69 225 6 69 226 5 69 227 6 70 228 38 70 229 39 70 230 39 70 231 7 70 232 6 70 233 7 71 234 39 71 235 40 71 236 40 71 237 8 71 238 7 71 239 8 72 240 40 72 241 41 72 242 41 72 243 9 72 244 8 72 245 9 73 246 41 73 247 42 73 248 42 73 249 10 73 250 9 73 251 10 74 252 42 74 253 43 74 254 43 74 255 11 74 256 10 74 257 11 75 258 43 75 259 44 75 260 44 75 261 12 75 262 11 75 263 12 76 264 44 76 265 45 76 266 45 76 267 13 76 268 12 76 269 13 77 270 45 77 271 46 77 272 46 77 273 14 77 274 13 77 275 14 78 276 46 78 277 47 78 278 47 78 279 15 78 280 14 78 281 15 79 282 47 79 283 48 79 284 48 79 285 16 79 286 15 79 287 16 80 288 48 80 289 49 80 290 49 80 291 17 80 292 16 80 293 17 81 294 49 81 295 50 81 296 50 81 297 18 81 298 17 81 299 18 82 300 50 82 301 51 82 302 51 82 303 19 82 304 18 82 305 19 83 306 51 83 307 52 83 308 52 83 309 20 83 310 19 83 311 20 84 312 52 84 313 53 84 314 53 84 315 21 84 316 20 84 317 21 85 318 53 85 319 54 85 320 54 85 321 22 85 322 21 85 323 22 86 324 54 86 325 55 86 326 55 86 327 23 86 328 22 86 329 23 87 330 55 87 331 56 87 332 56 87 333 24 87 334 23 87 335 24 88 336 56 88 337 57 88 338 57 88 339 25 88 340 24 88 341 25 89 342 57 89 343 58 89 344 58 89 345 26 89 346 25 89 347 26 90 348 58 90 349 59 90 350 59 90 351 27 90 352 26 90 353 27 91 354 59 91 355 60 91 356 60 91 357 28 91 358 27 91 359 28 92 360 60 92 361 61 92 362 61 92 363 29 92 364 28 92 365 29 93 366 61 93 367 62 93 368 62 93 369 30 93 370 29 93 371 30 94 372 62 94 373 63 94 374 63 94 375 31 94 376 30 94 377 32 95 378 0 95 379 31 95 380 31 95 381 63 95 382 32 95 383

+ + + + + + + 1.00000 0.00000 0.00621 -1.00000 -0.00000 -0.99379 -1.00000 0.00000 1.00621 -1.00000 0.05590 1.00621 -1.00000 0.05590 -0.99379 1.00000 0.05590 0.00621 + + + + + + + + + + 0.00000 -1.00000 0.00000 0.44721 0.00000 -0.89443 -1.00000 0.00000 0.00000 0.44721 0.00000 0.89443 -0.00000 1.00000 -0.00000 + + + + + + + + + + + + + + +

1 0 0 0 2 0 0 1 1 1 4 1 4 1 5 1 0 1 1 2 2 2 3 2 3 2 4 2 1 2 2 3 0 3 5 3 5 3 3 3 2 3 3 4 5 4 4 4

+ + + + + + + 0.03536 0.03536 -1.00000 0.04157 0.02778 -1.00000 0.04619 0.01913 -1.00000 0.04904 0.00975 -1.00000 0.05000 0.00000 -1.00000 0.04904 -0.00975 -1.00000 0.04619 -0.01913 -1.00000 0.04157 -0.02778 -1.00000 0.03536 -0.03536 -1.00000 0.02778 -0.04157 -1.00000 0.01913 -0.04619 -1.00000 0.00975 -0.04904 -1.00000 -0.00000 -0.05000 -1.00000 -0.00975 -0.04904 -1.00000 -0.01913 -0.04619 -1.00000 -0.02778 -0.04157 -1.00000 -0.03536 -0.03536 -1.00000 -0.04157 -0.02778 -1.00000 -0.04619 -0.01913 -1.00000 -0.04904 -0.00975 -1.00000 -0.05000 0.00000 -1.00000 -0.04904 0.00975 -1.00000 -0.04619 0.01913 -1.00000 -0.04157 0.02778 -1.00000 -0.03536 0.03536 -1.00000 -0.02778 0.04157 -1.00000 -0.01913 0.04619 -1.00000 -0.00975 0.04904 -1.00000 0.00000 0.05000 -1.00000 0.00975 0.04904 -1.00000 0.01913 0.04619 -1.00000 0.02778 0.04157 -1.00000 0.03536 0.03536 1.00000 0.04157 0.02778 1.00000 0.04619 0.01913 1.00000 0.04904 0.00975 1.00000 0.05000 -0.00000 1.00000 0.04904 -0.00975 1.00000 0.04619 -0.01913 1.00000 0.04157 -0.02778 1.00000 0.03536 -0.03536 1.00000 0.02778 -0.04157 1.00000 0.01913 -0.04619 1.00000 0.00975 -0.04904 1.00000 0.00000 -0.05000 1.00000 -0.00975 -0.04904 1.00000 -0.01913 -0.04619 1.00000 -0.02778 -0.04157 1.00000 -0.03536 -0.03536 1.00000 -0.04157 -0.02778 1.00000 -0.04619 -0.01913 1.00000 -0.04904 -0.00975 1.00000 -0.05000 -0.00000 1.00000 -0.04904 0.00975 1.00000 -0.04619 0.01913 1.00000 -0.04157 0.02778 1.00000 -0.03536 0.03536 1.00000 -0.02778 0.04157 1.00000 -0.01913 0.04619 1.00000 -0.00975 0.04904 1.00000 -0.00000 0.05000 1.00000 0.00975 0.04904 1.00000 0.01913 0.04619 1.00000 0.02778 0.04157 1.00000 0.00000 0.00000 -1.00000 0.00000 0.00000 1.00000 + + + + + + + + + + -0.00000 0.00000 -1.00000 0.00000 0.00000 1.00000 -0.00000 0.00000 -1.00000 0.00000 0.00000 1.00000 -0.00000 0.00000 -1.00000 0.00000 0.00000 1.00000 -0.00000 0.00000 -1.00000 0.00000 0.00000 1.00000 -0.00000 0.00000 -1.00000 0.00000 0.00000 1.00000 0.00000 0.00000 -1.00000 0.00000 0.00000 1.00000 0.00000 0.00000 -1.00000 0.00000 0.00000 1.00000 0.00000 0.00000 -1.00000 0.00000 0.00000 1.00000 0.00000 0.00000 -1.00000 0.00000 0.00000 1.00000 0.00000 0.00000 -1.00000 0.00000 0.00000 1.00000 0.00000 0.00000 -1.00000 0.00000 0.00000 1.00000 0.00000 0.00000 -1.00000 0.00000 0.00000 1.00000 0.00000 0.00000 -1.00000 0.00000 -0.00000 1.00000 0.00000 0.00000 -1.00000 0.00000 -0.00000 1.00000 0.00000 0.00000 -1.00000 0.00000 -0.00000 1.00000 0.00000 0.00000 -1.00000 0.00000 -0.00000 1.00000 0.00000 0.00000 -1.00000 0.00000 -0.00000 1.00000 0.00000 0.00000 -1.00000 0.00000 -0.00000 1.00000 0.00000 0.00000 -1.00000 0.00000 -0.00000 1.00000 0.00000 0.00000 -1.00000 0.00000 -0.00000 1.00000 0.00000 -0.00000 -1.00000 -0.00000 0.00000 1.00000 0.00000 -0.00000 -1.00000 -0.00000 0.00000 1.00000 0.00000 -0.00000 -1.00000 -0.00000 0.00000 1.00000 0.00000 -0.00000 -1.00000 -0.00000 0.00000 1.00000 0.00000 -0.00000 -1.00000 -0.00000 0.00000 1.00000 0.00000 -0.00000 -1.00000 -0.00000 0.00000 1.00000 0.00000 -0.00000 -1.00000 -0.00000 0.00000 1.00000 0.00000 -0.00000 -1.00000 -0.00000 0.00000 1.00000 -0.00000 0.00000 -1.00000 0.00000 0.00000 1.00000 -0.00000 0.00000 -1.00000 0.00000 0.00000 1.00000 -0.00000 0.00000 -1.00000 0.00000 0.00000 1.00000 0.00000 0.00000 -1.00000 0.00000 0.00000 1.00000 0.77301 0.63439 0.00000 0.88192 0.47140 0.00000 0.95694 0.29028 -0.00000 0.99518 0.09802 -0.00000 0.99518 -0.09802 -0.00000 0.95694 -0.29029 -0.00000 0.88192 -0.47140 -0.00000 0.77301 -0.63439 -0.00000 0.63439 -0.77301 0.00000 0.47140 -0.88192 -0.00000 0.29028 -0.95694 -0.00000 0.09802 -0.99518 -0.00000 -0.09802 -0.99518 -0.00000 -0.29028 -0.95694 -0.00000 -0.47140 -0.88192 -0.00000 -0.63439 -0.77301 -0.00000 -0.77301 -0.63439 -0.00000 -0.88192 -0.47140 0.00000 -0.95694 -0.29029 0.00000 -0.99518 -0.09802 -0.00000 -0.99518 0.09802 -0.00000 -0.95694 0.29028 -0.00000 -0.88192 0.47140 -0.00000 -0.77301 0.63439 -0.00000 -0.63440 0.77301 -0.00000 -0.47140 0.88192 0.00000 -0.29029 0.95694 -0.00000 -0.09802 0.99518 -0.00000 0.09801 0.99519 -0.00000 0.29028 0.95694 -0.00000 0.47139 0.88192 -0.00000 0.63439 0.77301 0.00000 + + + + + + + + + + 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 1.00000 1.00000 0.00000 1.00000 0.00000 0.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 1.00000 1.00000 0.00000 1.00000 0.00000 0.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 1.00000 1.00000 0.00000 1.00000 0.00000 0.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 1.00000 1.00000 0.00000 1.00000 0.00000 0.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 1.00000 1.00000 0.00000 1.00000 0.00000 0.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 1.00000 1.00000 0.00000 1.00000 0.00000 0.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 1.00000 1.00000 0.00000 1.00000 0.00000 0.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 1.00000 1.00000 0.00000 1.00000 0.00000 0.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 1.00000 1.00000 0.00000 1.00000 0.00000 0.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 1.00000 1.00000 0.00000 1.00000 0.00000 0.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 1.00000 1.00000 0.00000 1.00000 0.00000 0.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 1.00000 1.00000 0.00000 1.00000 0.00000 0.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 1.00000 1.00000 0.00000 1.00000 0.00000 0.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 1.00000 1.00000 0.00000 1.00000 0.00000 0.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 1.00000 1.00000 0.00000 1.00000 0.00000 0.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 1.00000 1.00000 0.00000 1.00000 0.00000 0.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 1.00000 1.00000 0.00000 1.00000 0.00000 0.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 1.00000 1.00000 0.00000 1.00000 0.00000 0.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 1.00000 1.00000 0.00000 1.00000 0.00000 0.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 1.00000 1.00000 0.00000 1.00000 0.00000 0.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 1.00000 1.00000 0.00000 1.00000 0.00000 0.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 1.00000 1.00000 0.00000 1.00000 0.00000 0.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 1.00000 1.00000 0.00000 1.00000 0.00000 0.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 1.00000 1.00000 0.00000 1.00000 0.00000 0.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 1.00000 1.00000 0.00000 1.00000 0.00000 0.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 1.00000 1.00000 0.00000 1.00000 0.00000 0.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 1.00000 1.00000 0.00000 1.00000 0.00000 0.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 1.00000 1.00000 0.00000 1.00000 0.00000 0.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 1.00000 1.00000 0.00000 1.00000 0.00000 0.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 1.00000 1.00000 0.00000 1.00000 0.00000 0.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 1.00000 1.00000 0.00000 1.00000 0.00000 0.00000 0.00000 0.00000 1.00000 0.00000 1.00000 1.00000 1.00000 1.00000 0.00000 1.00000 0.00000 0.00000 + + + + + + + + + + + + + + +

64 0 0 0 0 1 1 0 2 65 1 3 33 1 4 32 1 5 64 2 6 1 2 7 2 2 8 65 3 9 34 3 10 33 3 11 64 4 12 2 4 13 3 4 14 65 5 15 35 5 16 34 5 17 64 6 18 3 6 19 4 6 20 65 7 21 36 7 22 35 7 23 64 8 24 4 8 25 5 8 26 65 9 27 37 9 28 36 9 29 64 10 30 5 10 31 6 10 32 65 11 33 38 11 34 37 11 35 64 12 36 6 12 37 7 12 38 65 13 39 39 13 40 38 13 41 64 14 42 7 14 43 8 14 44 65 15 45 40 15 46 39 15 47 64 16 48 8 16 49 9 16 50 65 17 51 41 17 52 40 17 53 64 18 54 9 18 55 10 18 56 65 19 57 42 19 58 41 19 59 64 20 60 10 20 61 11 20 62 65 21 63 43 21 64 42 21 65 64 22 66 11 22 67 12 22 68 65 23 69 44 23 70 43 23 71 64 24 72 12 24 73 13 24 74 65 25 75 45 25 76 44 25 77 64 26 78 13 26 79 14 26 80 65 27 81 46 27 82 45 27 83 64 28 84 14 28 85 15 28 86 65 29 87 47 29 88 46 29 89 64 30 90 15 30 91 16 30 92 65 31 93 48 31 94 47 31 95 64 32 96 16 32 97 17 32 98 65 33 99 49 33 100 48 33 101 64 34 102 17 34 103 18 34 104 65 35 105 50 35 106 49 35 107 64 36 108 18 36 109 19 36 110 65 37 111 51 37 112 50 37 113 64 38 114 19 38 115 20 38 116 65 39 117 52 39 118 51 39 119 64 40 120 20 40 121 21 40 122 65 41 123 53 41 124 52 41 125 64 42 126 21 42 127 22 42 128 65 43 129 54 43 130 53 43 131 64 44 132 22 44 133 23 44 134 65 45 135 55 45 136 54 45 137 64 46 138 23 46 139 24 46 140 65 47 141 56 47 142 55 47 143 64 48 144 24 48 145 25 48 146 65 49 147 57 49 148 56 49 149 64 50 150 25 50 151 26 50 152 65 51 153 58 51 154 57 51 155 64 52 156 26 52 157 27 52 158 65 53 159 59 53 160 58 53 161 64 54 162 27 54 163 28 54 164 65 55 165 60 55 166 59 55 167 64 56 168 28 56 169 29 56 170 65 57 171 61 57 172 60 57 173 64 58 174 29 58 175 30 58 176 65 59 177 62 59 178 61 59 179 64 60 180 30 60 181 31 60 182 65 61 183 63 61 184 62 61 185 31 62 186 0 62 187 64 62 188 65 63 189 32 63 190 63 63 191 0 64 192 32 64 193 33 64 194 33 64 195 1 64 196 0 64 197 1 65 198 33 65 199 34 65 200 34 65 201 2 65 202 1 65 203 2 66 204 34 66 205 35 66 206 35 66 207 3 66 208 2 66 209 3 67 210 35 67 211 36 67 212 36 67 213 4 67 214 3 67 215 4 68 216 36 68 217 37 68 218 37 68 219 5 68 220 4 68 221 5 69 222 37 69 223 38 69 224 38 69 225 6 69 226 5 69 227 6 70 228 38 70 229 39 70 230 39 70 231 7 70 232 6 70 233 7 71 234 39 71 235 40 71 236 40 71 237 8 71 238 7 71 239 8 72 240 40 72 241 41 72 242 41 72 243 9 72 244 8 72 245 9 73 246 41 73 247 42 73 248 42 73 249 10 73 250 9 73 251 10 74 252 42 74 253 43 74 254 43 74 255 11 74 256 10 74 257 11 75 258 43 75 259 44 75 260 44 75 261 12 75 262 11 75 263 12 76 264 44 76 265 45 76 266 45 76 267 13 76 268 12 76 269 13 77 270 45 77 271 46 77 272 46 77 273 14 77 274 13 77 275 14 78 276 46 78 277 47 78 278 47 78 279 15 78 280 14 78 281 15 79 282 47 79 283 48 79 284 48 79 285 16 79 286 15 79 287 16 80 288 48 80 289 49 80 290 49 80 291 17 80 292 16 80 293 17 81 294 49 81 295 50 81 296 50 81 297 18 81 298 17 81 299 18 82 300 50 82 301 51 82 302 51 82 303 19 82 304 18 82 305 19 83 306 51 83 307 52 83 308 52 83 309 20 83 310 19 83 311 20 84 312 52 84 313 53 84 314 53 84 315 21 84 316 20 84 317 21 85 318 53 85 319 54 85 320 54 85 321 22 85 322 21 85 323 22 86 324 54 86 325 55 86 326 55 86 327 23 86 328 22 86 329 23 87 330 55 87 331 56 87 332 56 87 333 24 87 334 23 87 335 24 88 336 56 88 337 57 88 338 57 88 339 25 88 340 24 88 341 25 89 342 57 89 343 58 89 344 58 89 345 26 89 346 25 89 347 26 90 348 58 90 349 59 90 350 59 90 351 27 90 352 26 90 353 27 91 354 59 91 355 60 91 356 60 91 357 28 91 358 27 91 359 28 92 360 60 92 361 61 92 362 61 92 363 29 92 364 28 92 365 29 93 366 61 93 367 62 93 368 62 93 369 30 93 370 29 93 371 30 94 372 62 94 373 63 94 374 63 94 375 31 94 376 30 94 377 32 95 378 0 95 379 31 95 380 31 95 381 63 95 382 32 95 383

+ + + + + + + + -0.29478 -1.99835 2.70248 + 0 0 1 0.00000 + 0 1 0 -0.00000 + 1 0 0 0.00000 + 1.00000 1.00000 1.00000 + + + + 0.00334 -0.00071 0.00118 + 0 0 1 0.00000 + 0 1 0 -0.00000 + 1 0 0 0.00000 + 1.00000 1.00000 1.00000 + + + + + + + + + + + + 0.22856 0.00372 1.80362 + 0 0 1 0.00000 + 0 1 0 -0.00000 + 1 0 0 0.00000 + 0.18803 0.18803 0.18803 + + + + + + + + + + + + 0.00580 0.01041 0.99794 + 0 0 1 0.00000 + 0 1 0 -0.00000 + 1 0 0 0.00000 + 1.00000 1.00000 1.00000 + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/orne_rviz_plugins/package.xml b/orne_rviz_plugins/package.xml new file mode 100644 index 0000000..55c71fc --- /dev/null +++ b/orne_rviz_plugins/package.xml @@ -0,0 +1,22 @@ + + orne_rviz_plugins + 0.0.1 + + The orne_rviz_plugins package + + Daiki Maekawa + BSD + + Daiki Maekawa + + catkin + rviz + std_srvs + rviz + std_srvs + + + + + + diff --git a/orne_rviz_plugins/plugin_description.xml b/orne_rviz_plugins/plugin_description.xml new file mode 100644 index 0000000..ccb9e11 --- /dev/null +++ b/orne_rviz_plugins/plugin_description.xml @@ -0,0 +1,9 @@ + + + + TODO + + + diff --git a/orne_rviz_plugins/src/state_trigger_panel.cpp b/orne_rviz_plugins/src/state_trigger_panel.cpp new file mode 100644 index 0000000..7e2a334 --- /dev/null +++ b/orne_rviz_plugins/src/state_trigger_panel.cpp @@ -0,0 +1,63 @@ +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "state_trigger_panel.h" + +namespace orne_rviz_plugins +{ + +StateTriggerPanel::StateTriggerPanel( QWidget* parent ) + : rviz::Panel( parent ) +{ + start_client_ = nh_.serviceClient("start_wp_nav", false); + resume_client_ = nh_.serviceClient("resume_nav", false); + + start_nav_button_ = new QPushButton("StartWaypointsNavigation"); + resume_nav_button_ = new QPushButton("ResumeWaypointsNavigation"); + + QHBoxLayout* layout = new QHBoxLayout; + layout->addWidget(start_nav_button_); + layout->addWidget(resume_nav_button_); + setLayout( layout ); + + connect(start_nav_button_, SIGNAL(clicked()), this, SLOT(pushStartNavigation())); + connect(resume_nav_button_, SIGNAL(clicked()), this, SLOT(pushResumeNavigation())); +} + +void StateTriggerPanel::save( rviz::Config config ) const +{ + rviz::Panel::save( config ); +} + +void StateTriggerPanel::load( const rviz::Config& config ) +{ + rviz::Panel::load( config ); +} + +void StateTriggerPanel::pushStartNavigation() { + ROS_INFO("Service call: start waypoints navigation"); + + std_srvs::Trigger trigger; + start_client_.call(trigger); +} + +void StateTriggerPanel::pushResumeNavigation() { + ROS_INFO("Service call: resume waypoints navigation"); + + std_srvs::Trigger trigger; + resume_client_.call(trigger); +} + +} // end namespace orne_rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(orne_rviz_plugins::StateTriggerPanel,rviz::Panel ) diff --git a/orne_rviz_plugins/src/state_trigger_panel.h b/orne_rviz_plugins/src/state_trigger_panel.h new file mode 100644 index 0000000..461b3a0 --- /dev/null +++ b/orne_rviz_plugins/src/state_trigger_panel.h @@ -0,0 +1,38 @@ +#ifndef STATE_TRIGGER_PANEL_H +#define STATE_TRIGGER_PANEL_H + +#ifndef Q_MOC_RUN +# include + +# include +#endif + +class QPushButton; + +namespace orne_rviz_plugins +{ + +class StateTriggerPanel: public rviz::Panel +{ +Q_OBJECT +public: + StateTriggerPanel( QWidget* parent = 0 ); + + virtual void load( const rviz::Config& config ); + virtual void save( rviz::Config config ) const; + +public Q_SLOTS: + void pushStartNavigation(); + void pushResumeNavigation(); + +protected: + ros::NodeHandle nh_; + ros::ServiceClient start_client_, resume_client_; + QPushButton *start_nav_button_; + QPushButton *resume_nav_button_; + +}; + +} // end namespace orne_rviz_plugins + +#endif // STATE_TRIGGER_PANEL_H diff --git a/waypoint_manager/scripts/devel_GUI.py b/waypoint_manager/scripts/devel_GUI.py index 457493a..93aa27d 100755 --- a/waypoint_manager/scripts/devel_GUI.py +++ b/waypoint_manager/scripts/devel_GUI.py @@ -1,12 +1,13 @@ #import rospy -from textwrap import fill import tkinter as tk import tkinter.filedialog import numpy as np +import quaternion +import math import ruamel.yaml from PIL import Image, ImageTk from pathlib import Path -from ruamel.yaml.comments import CommentedMap, CommentedSeq +from ruamel.yaml.comments import CommentedMap #===== Applicationクラスの定義　tk.Frameクラスを継承 =====# @@ -24,28 +25,45 @@ self.menu_bar = tk.Menu(self) # メニューバーを配置 self.file_menu = tk.Menu(self.menu_bar, tearoff=tk.OFF) # バーに追加するメニューを作成 self.menu_bar.add_cascade(label="File", menu=self.file_menu) # Fileメニューとしてバーに追加 + self.open_menu = tk.Menu(self.file_menu, tearoff=tk.OFF) + self.open_menu.add_command(label="Map", command=self.menu_open_map) + self.open_menu.add_command(label="Waypoints", command=self.menu_open_waypoints, state="disabled") + self.file_menu.add_cascade(label="Open", menu=self.open_menu) + self.file_menu.add_command(label="Save", # FileメニューにSaveコマンドを追加 - command=self.menu_save_clicked, #コールバック関数を設定 - accelerator="Ctrl+S"# 右側に表示するキーボードショートカット - ) + command=self.menu_save_clicked, #コールバック関数を設定 + accelerator="Ctrl+S"# 右側に表示するキーボードショートカット + ) self.file_menu.add_command(label="Save As", # 同様にSave Asコマンドを追加 - command=self.menu_saveas_clicked, - accelerator="Ctrl+Shift+S" - ) + command=self.menu_saveas_clicked, + accelerator="Ctrl+Shift+S" + ) + self.file_menu.add_separator() + self.file_menu.add_command(label="Exit", command=self.menu_exit_clicked, accelerator="Ctrl+Q") self.bind_all("", self.menu_save_clicked) #キーボードショートカットを設定 self.bind_all("", self.menu_saveas_clicked) + self.bind_all("", self.menu_exit_clicked) self.master.config(menu=self.menu_bar) # 大元に作成したメニューバーを設定 + ## 画面上部に、システムからのメッセージを表示するラベルを配置 + self.msg_label = tk.Label(self.master, text="Start up", anchor=tk.E) + self.msg_label.pack(fill=tk.X) + + ## 画面下部に、カーソルの座標やピクセル情報を表示するステータスバーを表示 + self.status_bar = tk.Frame(self.master) + self.mouse_position = tk.Label(self.status_bar, relief=tk.SUNKEN, text="(x, y, val) = ", + width=40, anchor=tk.W, font=("", 15)) + self.mouse_position.pack(side=tk.LEFT, padx=1) + self.waypoint_num = tk.Label(self.status_bar, relief=tk.SUNKEN, text="Waypoint No. -----", + width=20, anchor=tk.W, font=("", 15)) + self.waypoint_num.pack(side=tk.LEFT, padx=1) + self.status_bar.pack(side=tk.BOTTOM, fill=tk.X) + ## 右クリックしたときに表示するポップアップメニューを作成 self.popup_menu = tk.Menu(self, tearoff=tk.OFF) - self.popup_menu.add_command(label="Add waypoint here", command=self.add_waypoint) + self.popup_menu.add_command(label="Add waypoint here", command=self.add_waypoint_here) self.right_click_coord = None # 右クリックしたときの座標を保持する変数 - ## map.pgm, map.yaml, waypoints.yaml の読み込み　3つの変数は再代入禁止 - self.__map_img_pil, self.__map_yaml = self.get_map_info() - self.__waypoints, self.waypoint_filepath = self.get_waypoints() - self.current_waypoints = self.__waypoints # 編集中のウェイポイント情報を保持する - ## canvasを配置 self.canvas = tk.Canvas(self.master, background="#AAA") # 画像を描画するcanvas self.canvas.pack(expand=True, fill=tk.BOTH) # canvasを配置 @@ -54,7 +72,57 @@ ## 画像をcanvasのサイズにフィッティングして描画 self.canv_w = self.canvas.winfo_width() # canvasの幅を取得 self.canv_h = self.canvas.winfo_height() # canvasの高さを取得 - self.mat_affine = np.eye(3) # 同次変換行列の初期化 + + ## マウスイベントに対するコールバックを設定 + self.master.bind("", self.mouse_move) + self.master.bind("", self.mouse_wheel) + self.master.bind("", self.left_click_move) + self.master.bind("", self.left_click) + self.master.bind("", self.left_click_release) + self.master.bind("", self.right_click) + self.master.bind("", self.ctrl_left_click) + self.master.bind("", self.ctrl_right_click) + ## ウィンドウに関するコールバック + self.master.bind("", self.window_resize_callback) + + ## その他必要になる変数 + self.__map_img_pil = None + self.__map_yaml = None + self.map_resolution = 0.05 + self.img_origin = [] + self.mat_affine = np.eye(3) # 同次変換行列 + self.draw_img_tk = None + self.__waypoints = None + self.waypoint_filepath = None + self.current_waypoints = None + self.waypoints_id = np.array([], np.uint16) + self.wplabel_id = np.array([], np.uint16) + self.finish_pose = None + self.finishpose_id = None + self.old_click_point = None # 最後にカーソルのあった座標を保持 + self.wp_info_win = None # ウェイポイント情報を表示するウィンドウ + self.editing_waypoint_id = None # 編集中のウェイポイントを示す図形のオブジェクトID + self.moving_waypoint = False # ウェイポイントをDnDで動かしている最中かどうか + self.add_wp_win = None # add waypoint hereをクリックしたときに表示するウィンドウ + self.point_rad = 10 + return + + + + def menu_open_map(self): + map_path = tkinter.filedialog.askopenfilename( + parent=self.master, + title="Select map yaml file", + initialdir=str(Path(".")), + filetypes=[("YAML", "yaml")] + ) + if not map_path: return + + with open(map_path) as file: # .yamlを読み込む + self.__map_yaml = ruamel.yaml.YAML().load(file) + # .pgmをplillowで読み込む + self.__map_img_pil = Image.open(Path(map_path).with_name(self.__map_yaml["image"]).resolve()) + # 画像を表示 scale = 1 offset_x = 0 offset_y = 0 @@ -71,62 +139,44 @@ self.translate_mat(offset_x, offset_y) # 同次変換行列を更新 self.draw_img_tk = ImageTk.PhotoImage(image=self.__map_img_pil) # 描画する画像を保持する変数を初期化 self.draw_image() # 画像を描画 - ## map.yamlから原点と解像度を取得し、地図上に原点を示す円を描画 - self.point_rad = 10 origin = self.__map_yaml['origin'] # originは地図上の原点から画像の左下までの距離[x,y](m) self.map_resolution = self.__map_yaml['resolution'] # 画像上の原点座標を保持（左上から横x, 縦y） self.img_origin = [-origin[0]/self.map_resolution, self.__map_img_pil.height+origin[1]/self.map_resolution, 1] self.plot_origin() + self.master.title(Path(map_path).name + " - " + self.master.title()) + self.open_menu.entryconfigure("Map", state=tk.DISABLED) + self.open_menu.entryconfigure("Waypoints", state=tk.NORMAL) + return + - ## waypoints.yamlからウェイポイント情報を取得し、画像にポイントを描画 - self.waypoints_id = np.array([], np.uint16) + + def menu_open_waypoints(self): + filepath = tkinter.filedialog.askopenfilename( + parent=self.master, + title="Select map yaml file", + initialdir=str(Path(".")), + filetypes=[("YAML", "yaml")] + ) + if not filepath: return + with open(filepath) as file: + waypoints = ruamel.yaml.YAML().load(file) + self.__waypoints = waypoints + self.current_waypoints = waypoints self.plot_waypoints() - - ## マウスイベントに対するコールバックを設定 - self.master.bind("", self.mouse_wheel) - self.master.bind("", self.left_click_move) - self.master.bind("", self.left_click) - self.master.bind("", self.left_click_release) - self.master.bind("", self.right_click) - ## ウィンドウに関するコールバック - self.master.bind("", self.window_resize_callback) - - self.old_click_point = None - self.wp_info_win = None - self.editing_waypoint_id = None - self.moving_waypoint = False - self.add_wp_win = None - self.adding_waypoint = False + self.master.title(Path(filepath).name + " - " + self.master.title()) return """ - +++++ mapのファイルパスを受け取り、map画像とmapの設定ファイルを読み込む +++++ - --- 今はパスを直接指定して読み込んでいるので、rospy.get_param()を使って読み込めるように --- + +++++ 画面上部にメッセージを表示する +++++ """ - def get_map_info(self): - map_path = Path('waypoint_nav','maps','map') # .pgmと.yamlの手前までのパス - map_img_pil = Image.open(Path(str(map_path)+'.pgm')) # .pgmをplillowで読み込む - with open(str(map_path)+'.yaml') as file: # .yamlを読み込む - map_yaml = ruamel.yaml.YAML().load(file) - self.master.title(map_path.name + " - " + self.master.title()) - return map_img_pil, map_yaml # この2つの変数を戻り値とする - - - - """ - +++++ waypointsのパスを受け取り読み込んだデータを返す +++++ - --- これもget_param()でパスを受け取り、読み込めるようにする --- - """ - def get_waypoints(self): - file_path = Path('waypoint_nav','param','test.yaml') - with open(file_path) as file: - waypoints = ruamel.yaml.YAML().load(file) - self.master.title(file_path.name + " - " + self.master.title()) - return waypoints, file_path + def message(self, msg): + if not isinstance(msg, str): + msg = str(msg) + self.msg_label["text"] = str(msg) @@ -136,14 +186,14 @@ def plot_origin(self): origin_affine = np.dot(self.mat_affine, self.img_origin) # キャンバス上の座標に変換 r = self.point_rad # 円の半径（ピクセル） - x1 = origin_affine[0] - r - y1 = origin_affine[1] - r - x2 = origin_affine[0] + r + 1 - y2 = origin_affine[1] + r + 1 + x0 = origin_affine[0] - r + y0 = origin_affine[1] - r + x1 = origin_affine[0] + r + 1 + y1 = origin_affine[1] + r + 1 if self.canvas.find_withtag("origin"): - self.canvas.moveto("origin", x1, y1) + self.canvas.moveto("origin", x0, y0) else: - self.canvas.create_oval(x1, y1, x2, y2, tags="origin", fill='cyan', outline='blue') + self.canvas.create_oval(x0, y0, x1, y1, tags="origin", fill='cyan', outline='blue') return @@ -151,9 +201,20 @@ """ +++++ 地図上にウェイポイントを示す円を描画する +++++ """ - def plot_waypoints(self): + def plot_waypoints(self, id=None): + if not self.__waypoints: return points = self.current_waypoints['waypoints'] # ウェイポイントのリスト r = self.point_rad + # 引数にidが指定された場合、そのポイントのみを再描画して終了 + if id: + wp_num = np.where(self.waypoints_id == id)[0][0] + 1 # クリックしたウェイポイントの番号を取得 + img_x = float(points[wp_num-1]['point']['x']) / self.map_resolution + self.img_origin[0] + img_y = -float(points[wp_num-1]['point']['y']) / self.map_resolution + self.img_origin[1] + xy_affine = np.dot(self.mat_affine, [img_x, img_y, 1]) + x0 = xy_affine[0] - r + y0 = xy_affine[1] - r + self.canvas.moveto(id, x0, y0) + return # ウェイポイントの追加または削除が行なわれている場合、初期化 if len(self.waypoints_id) != len(points): for id in self.waypoints_id: @@ -164,17 +225,112 @@ img_x = float(points[i]['point']['x']) / self.map_resolution + self.img_origin[0] img_y = -float(points[i]['point']['y']) / self.map_resolution + self.img_origin[1] xy_affine = np.dot(self.mat_affine, [img_x, img_y, 1]) - x1 = xy_affine[0] - r - y1 = xy_affine[1] - r - x2 = xy_affine[0] + r + 1 - y2 = xy_affine[1] + r + 1 + x0 = xy_affine[0] - r + y0 = xy_affine[1] - r + x1 = xy_affine[0] + r + 1 + y1 = xy_affine[1] + r + 1 if len(self.waypoints_id) < len(points): - id = self.canvas.create_oval(x1, y1, x2, y2, - fill='#FDD', outline='red', activefill='red') + id = self.canvas.create_oval(x0, y0, x1, y1, fill='#FDD', outline='red', activefill='red') + self.canvas.tag_bind(id, "", lambda event, wp_id=id: self.waypoint_clicked(event, wp_id)) + self.canvas.tag_bind(id, "", lambda event, wp_id=id: self.waypoint_enter(event, wp_id)) + self.canvas.tag_bind(id, "", self.waypoint_leave) + self.canvas.tag_bind(id, "", lambda event, wp_id=id: self.waypoint_click_move(event, wp_id)) self.waypoints_id = np.append(self.waypoints_id, id) else: id = self.waypoints_id[i] - self.canvas.moveto(id, x1, y1) + self.canvas.moveto(id, x0, y0) + # Finish poseを描画 + if self.finish_pose is None: + pos = self.current_waypoints['finish_pose']['pose']['position'] + img_x = float(pos['x']) / self.map_resolution + self.img_origin[0] + img_y = -float(pos['y']) / self.map_resolution + self.img_origin[1] + orient = self.current_waypoints['finish_pose']['pose']['orientation'] + quat = np.quaternion(orient['x'], orient['y'], orient['z'], orient['w']) + yaw = quaternion.as_euler_angles(quat)[1] + self.finish_pose = [img_x, img_y, yaw] + xy_affine = np.dot(self.mat_affine, [self.finish_pose[0], self.finish_pose[1], 1]) + x0 = xy_affine[0] + y0 = xy_affine[1] + x1 = x0 + math.cos(self.finish_pose[2]) * r * 3 + y1 = y0 - math.sin(self.finish_pose[2]) * r * 3 + if self.finishpose_id is not None: + self.canvas.delete(self.finishpose_id) + self.finishpose_id = self.canvas.create_line(x0, y0, x1, y1, width=10, tags="finish_pose", + arrow=tk.LAST, arrowshape=(12,15,9), fill="#AAF") + return + + + + """ + +++++ ウェイポイントが左クリックされたときのコールバック +++++ + """ + def waypoint_clicked(self, event, wp_id): + if wp_id != self.editing_waypoint_id: # 編集中のウェイポイントを切り替え + self.canvas.itemconfig(self.editing_waypoint_id, fill='#FDD') + self.editing_waypoint_id = wp_id + self.canvas.itemconfig(wp_id, fill='red') + self.disp_waypoint_info(wp_id) + self.message("Show selected waypoint information") + return + + + + """ + +++++ ウェイポイントを左クリックしながら動かしたときのコールバック +++++ + """ + def waypoint_click_move(self, event, wp_id): + if not self.moving_waypoint: return + if self.old_click_point is None: + self.old_click_point = [event.x, event.y] + return + delta_x = event.x-self.old_click_point[0] + delta_y = event.y-self.old_click_point[1] + self.canvas.move(self.editing_waypoint_id, delta_x, delta_y) + box = self.canvas.bbox(self.editing_waypoint_id) + px = (box[2] + box[0]) / 2 # ウィンドウ上の座標 + py = (box[3] + box[1]) / 2 + img_x, img_y = self.canvas2image(px, py) + # マップ画像上の座標を、実際の座標に変換 + x, y = self.image2real(img_x, img_y) + # 編集中のウェイポイント情報を更新 + wp_num = np.where(self.waypoints_id == self.editing_waypoint_id)[0][0] + self.current_waypoints['waypoints'][wp_num]['point']['x'] = x + self.current_waypoints['waypoints'][wp_num]['point']['y'] = y + # 表示中のウェイポイント情報を更新 + txt_box = self.wp_info_win.grid_slaves(column=1, row=0)[0] + txt_box.delete(0, tk.END) + txt_box.insert(tk.END, x) + txt_box = self.wp_info_win.grid_slaves(column=1, row=1)[0] + txt_box.delete(0, tk.END) + txt_box.insert(tk.END, y) + self.old_click_point = [event.x, event.y] + return + + + """ + +++++ ウェイポイントを示す円にカーソルが入ったときと出たときのコールバック +++++ + """ + def waypoint_enter(self, event, wp_id): + wp_num = np.where(self.waypoints_id == wp_id)[0][0] + 1 # ウェイポイントの番号を取得 + self.waypoint_num["text"] = "Waypoint No. " + str(wp_num) + + def waypoint_leave(self, event): + self.waypoint_num["text"] = "Waypoint No. -----" + + + + """ + +++++ キャンバス内でマウスを動かしたときのコールバック +++++ + """ + def mouse_move(self, event): + if not self.__map_img_pil: return + img_x, img_y = self.canvas2image(event.x, event.y) + if (img_x < 0) or (img_y < 0) or (img_x > self.__map_img_pil.width) or (img_y > self.__map_img_pil.height): + self.mouse_position["text"] = " Out of map" + return + x, y = self.image2real(img_x, img_y) + val = self.__map_img_pil.getpixel((img_x, img_y)) + self.mouse_position["text"] = " (x, y, val) = ({}, {}, {})".format(x, y, val) return @@ -184,19 +340,6 @@ """ def left_click(self, event): self.popup_menu.unpost() # 右クリックで出るポップアップメニューを非表示 - # クリックした座標の近くにあるオブジェクトを取得 - clicked_obj = self.canvas.find_enclosed(event.x-20, event.y-20, event.x+20, event.y+20) - if not clicked_obj: return # オブジェクト以外の領域 - id = clicked_obj[0] - tag = self.canvas.gettags(id) - if (len(tag) == 0): return - if (tag[0] == "origin"): return - # ↓ウェイポイントがクリックされた場合、情報を表示 - if id != self.editing_waypoint_id: # 編集中のウェイポイントを切り替え - self.canvas.itemconfig(self.editing_waypoint_id, fill='#FDD') - self.editing_waypoint_id = id - self.canvas.itemconfig(id, fill='red') - self.disp_waypoint_info(id) return @@ -212,10 +355,9 @@ self.wp_info_win = tk.Toplevel() self.wp_info_win.protocol("WM_DELETE_WINDOW", self.close_wp_info) self.wp_info_win.attributes('-topmost', True) # サブウィンドウを最前面で固定 - # ウェイポイントファイルのキーを取得し、ラベルを配置 - label_w = 4 + # ウェイポイントファイルのキーを取得し、ラベルとテキストボックスを配置 for i, key in enumerate(point): - key_label = tk.Label(self.wp_info_win, text=key+":", width=label_w, font=("Consolas",15), anchor=tk.E) + key_label = tk.Label(self.wp_info_win, text=key+":", width=6, font=("Consolas",15), anchor=tk.E) key_label.grid(column=0, row=i, padx=2, pady=5) txt_box = tk.Entry(self.wp_info_win, width=20, font=("Consolas", 15)) txt_box.insert(tk.END, point[key]) @@ -226,8 +368,8 @@ apply_btn = tk.Button(canv, text="Apply", width=5, height=1, background="#FDD", command=self.apply_btn_callback) apply_btn.pack(side=tk.RIGHT, anchor=tk.SE, padx=5, pady=5) - dnd_btn = tk.Button(canv, text="DnD", width=5, height=1, background="#EEE", - command=self.dnd_btn_callback) + dnd_btn = tk.Button(canv, text="DnD", width=5, height=1, background="#EEE") + dnd_btn["command"] = lambda obj=dnd_btn: self.dnd_btn_callback(dnd_btn) dnd_btn.pack(side=tk.RIGHT, anchor=tk.SE, padx=5, pady=5) remove_btn = tk.Button(canv, text="Remove", width=7, height=1, background="#F00", command=self.remove_btn_callback) @@ -255,12 +397,13 @@ +++++ Applyボタンを押したときのコールバック +++++ """ def apply_btn_callback(self): - wp_num = np.where(self.waypoints_id == self.editing_waypoint_id)[0][0] + 1 # クリックしたウェイポイントの番号を取得 + wp_num = np.where(self.waypoints_id == self.editing_waypoint_id)[0][0] + 1 # 編集中のウェイポイントの番号を取得 point = self.current_waypoints['waypoints'][wp_num-1]['point'] for i, key in enumerate(point): txt_box = self.wp_info_win.grid_slaves(column=1, row=i)[0] self.current_waypoints['waypoints'][wp_num-1]['point'][key] = float(txt_box.get()) - self.plot_waypoints() + self.plot_waypoints(id=self.editing_waypoint_id) + self.message("Apply changes of waypoint parameters") return @@ -268,16 +411,20 @@ """ +++++ ドラッグ&ドロップボタン(DnD)を押したときのコールバック +++++ """ - def dnd_btn_callback(self): - c = self.wp_info_win.grid_slaves(row=self.wp_info_win.grid_size()[1]-1, column=0)[0] - btn = c.pack_slaves()[1] + def dnd_btn_callback(self, obj=None): + if obj is None: return + btn = obj # 押された状態とそうでない状態を切り替える if btn["relief"] == tk.RAISED: btn["relief"] = tk.SUNKEN btn["background"] = "#AAA" + self.moving_waypoint = True + self.message("Drag & Drop to move waypoint") elif btn["relief"] == tk.SUNKEN: btn["relief"] = tk.RAISED btn["background"] = "#EEE" + self.moving_waypoint = False + self.message("Show selected waypoint information") return @@ -288,8 +435,10 @@ def remove_btn_callback(self): wp_num = np.where(self.waypoints_id == self.editing_waypoint_id)[0][0] + 1 # クリックしたウェイポイントの番号を取得 self.current_waypoints['waypoints'].pop(wp_num-1) # ウェイポイントを削除 - self.plot_waypoints() # ウェイポイントを再描画、waypoints_idを更新 + self.canvas.delete(self.editing_waypoint_id) # ウェイポイントを示す円を削除 + self.waypoints_id = np.delete(self.waypoints_id, wp_num-1) # waypoints_idから削除 self.close_wp_info() + self.message("Removed waypoint" + str(wp_num)) return @@ -309,53 +458,22 @@ +++++ マウスを左クリックしながらドラッグしたときのコールバック関数 +++++ """ def left_click_move(self, event): - if (self.wp_info_win is not None) and (self.wp_info_win.winfo_exists()) and (not self.moving_waypoint): - # ウェイポイント情報が表示されていて、ウェイポイントを動かしていないとき - c = self.wp_info_win.grid_slaves(row=self.wp_info_win.grid_size()[1]-1, column=0)[0] - dnd_btn = c.pack_slaves()[1] # dndボタンの状態を取得 - clicked_obj = self.canvas.find_enclosed(event.x-20, event.y-20, event.x+20, event.y+20) - if (clicked_obj) and (clicked_obj[0] == self.editing_waypoint_id) and (dnd_btn["relief"] == tk.SUNKEN): - # dndボタンが押された状態で、編集中のウェイポイントがクリックされたとき - self.moving_waypoint = True # ウェイポイントをドラッグで移動させるモードへ - self.old_click_point = [event.x, event.y] - return + if not self.__map_img_pil: return + if self.moving_waypoint: return if self.old_click_point is None: self.old_click_point = [event.x, event.y] return # カーソルの移動量を計算 delta_x = event.x-self.old_click_point[0] delta_y = event.y-self.old_click_point[1] - #ウェイポイントをドラッグで動かしているとき - if self.moving_waypoint: - self.canvas.move(self.editing_waypoint_id, delta_x, delta_y) - box = self.canvas.bbox(self.editing_waypoint_id) - px = (box[2] + box[0]) / 2 # ウィンドウ上の座標 - py = (box[3] + box[1]) / 2 - mat_inv = np.linalg.inv(self.mat_affine) - img_xy = np.dot(mat_inv, [px, py, 1]) # マップ画像上の座標 - # マップ画像上の座標を、実際の座標に変換 - x = (img_xy[0] - self.img_origin[0]) * self.map_resolution - y = (-img_xy[1] + self.img_origin[1]) * self.map_resolution - x = round(x, 6) - y = round(y, 6) - wp_num = np.where(self.waypoints_id == self.editing_waypoint_id)[0][0] - # 編集中のウェイポイント情報を更新 - self.current_waypoints['waypoints'][wp_num]['point']['x'] = x - self.current_waypoints['waypoints'][wp_num]['point']['y'] = y - # 表示中のウェイポイント情報を更新 - txt_box = self.wp_info_win.grid_slaves(column=1, row=0)[0] - txt_box.delete(0, tk.END) - txt_box.insert(tk.END, x) - txt_box = self.wp_info_win.grid_slaves(column=1, row=1)[0] - txt_box.delete(0, tk.END) - txt_box.insert(tk.END, y) - self.old_click_point = [event.x, event.y] - return # ウェイポイント移動モードでないとき、地図を平行移動 self.translate_mat(delta_x, delta_y) self.draw_image() - self.canvas.move("origin", delta_x, delta_y) #self.plot_origin() - self.plot_waypoints() + # origin, waypoints finish_pose を平行移動 + self.canvas.move("origin", delta_x, delta_y) + for id in self.waypoints_id: + self.canvas.move(id, delta_x, delta_y) + self.canvas.move("finish_pose", delta_x, delta_y) self.old_click_point = [event.x, event.y] return @@ -366,7 +484,6 @@ """ def left_click_release(self, event): self.old_click_point = None - self.moving_waypoint = False return @@ -375,32 +492,31 @@ +++++ 右クリックしたときのコールバック関数 +++++ """ def right_click(self, event): + if not self.__map_img_pil: return # クリックした座標の近くにあるオブジェクトを取得 clicked_obj = self.canvas.find_enclosed(event.x-20, event.y-20, event.x+20, event.y+20) if clicked_obj: # 何かオブジェクトがクリックされていた場合 return # クリックされた座標 => 元画像の座標 の変換 - mat_inv = np.linalg.inv(self.mat_affine) - img_xy = np.dot(mat_inv, [event.x, event.y, 1]) + img_x, img_y = self.canvas2image(event.x, event.y) # 変換後の元画像の座標がサイズから外れている場合（地図画像の外をクリックしている） - if (img_xy[0] < 0) or (img_xy[1] < 0) or \ - (img_xy[0] > self.__map_img_pil.width) or (img_xy[1] > self.__map_img_pil.height): + if (img_x < 0) or (img_y < 0) or (img_x > self.__map_img_pil.width) or (img_y > self.__map_img_pil.height): return self.popup_menu.post(event.x_root, event.y_root) # メニューをポップアップ - self.right_click_coord = img_xy[0:2] # クリックされた元画像上の座標を変数に格納 + self.right_click_coord = [img_x, img_y] # クリックされた元画像上の座標を変数に格納 return """ - +++++ 右クリックしてポップアップメニューのadd waypointをクリックしたときのコールバック関数 +++++ + +++++ 右クリックしてポップアップメニューのadd waypoint hereをクリックしたときのコールバック関数 +++++ """ - def add_waypoint(self): + def add_waypoint_here(self): if (self.wp_info_win is not None) and (self.wp_info_win.winfo_exists()): self.close_wp_info() img_xy = self.right_click_coord if self.__map_img_pil.getpixel((img_xy[0], img_xy[1])) == 0: - print("There is obstacles") + self.message("There is obstacles") return # 何番目のウェイポイントの次に追加するか入力させる self.add_wp_win = tk.Toplevel() @@ -420,35 +536,33 @@ y = int((self.canv_h - h) / 2) geometry = "{}x{}+{}+{}".format(w, h, x, y) self.add_wp_win.geometry(geometry) + self.message("Add waypoint") return """ - +++++ add waypoint hereをクリックして開いた別窓のボタンのコールバック +++++ + +++++ add waypoint hereをクリックして開いた別窓のAddボタンのコールバック +++++ """ def add_btn_callback(self): num_box = self.add_wp_win.grid_slaves(row=0, column=1)[0] num = num_box.get() if (num == ""): - print("Please enter number") + self.message("Please enter number") return self.add_wp_win.destroy() num = int(num) img_xy = self.right_click_coord # ウェイポイント座標を計算 - x = (img_xy[0] - self.img_origin[0]) * self.map_resolution - y = (-img_xy[1] + self.img_origin[1]) * self.map_resolution - x = round(x, 6) - y = round(y, 6) + x, y = self.image2real(img_xy[0], img_xy[1]) # ウェイポイントを追加 data = CommentedMap() data['point'] = CommentedMap() for key in self.current_waypoints['waypoints'][0]['point']: - if (key == 'x'): val = x - elif (key == 'y'): val = y - else: val = 0.0 - data['point'][key] = val + if (key == 'x'): v = x + elif (key == 'y'): v = y + else: v = 0.0 + data['point'][key] = v self.current_waypoints['waypoints'].insert(num, data) self.plot_waypoints() # ウェイポイントを再描画、waypoints_idも初期化 id = self.waypoints_id[num] @@ -464,6 +578,7 @@ --- docker コンテナ上だとtkinterでマウスホイールイベントが拾えないっぽいので、これは使えないかも --- """ def mouse_wheel(self, event): + if not self.__map_img_pil: return self.translate_mat(-event.x, -event.y) if event.delta > 0: # 上に回転（タッチパッドなら下にドラッグ）=> 拡大 @@ -480,11 +595,43 @@ """ + +++++ Ctrl押しながら左クリックしたときのコールバック +++++ + """ + def ctrl_left_click(self, event): + if not self.__map_img_pil: return + self.translate_mat(-event.x, -event.y) + self.scale_mat(1.2) + self.translate_mat(event.x, event.y) + self.draw_image() + self.plot_origin() + self.plot_waypoints() + self.message("Zoom In") + return + + + + """ + +++++ Ctrl押しながら右クリックしたときのコールバック +++++ + """ + def ctrl_right_click(self, event): + if not self.__map_img_pil: return + self.translate_mat(-event.x, -event.y) + self.scale_mat(0.8) + self.translate_mat(event.x, event.y) + self.draw_image() + self.plot_origin() + self.plot_waypoints() + self.message("Zoom Out") + return + + + + """ +++++ Fileメニューの"Save"がクリックされたときに実行されるコールバック関数 +++++ """ def menu_save_clicked(self, event=None): self.save_waypoints(self.waypoint_filepath) - print("Saved changes!") + self.message("Saved changes!") return @@ -494,21 +641,33 @@ """ def menu_saveas_clicked(self, event=None): new_filepath = tkinter.filedialog.asksaveasfilename( - parent=self.master, - title="Save As", - initialdir=str(Path('..','..','waypoint_nav','param')), - filetypes=[("YAML", ".yaml")], - defaultextension="yaml" - ) + parent=self.master, + title="Save As", + initialdir=str(Path('..','..','waypoint_nav','param')), + filetypes=[("YAML", ".yaml")], + defaultextension=".yaml" + ) if len(new_filepath) == 0: return # cancel self.save_waypoints(new_filepath) - self.waypoint_filepath = new_filepath - print("Save As", "\"", new_filepath, "\"") + current_title = self.master.title() + old_filename = self.waypoint_filepath.name + self.waypoint_filepath = Path(new_filepath) + self.master.title(current_title.replace(old_filename, self.waypoint_filepath.name)) + self.message("Save As" + "\"" + str(new_filepath) + "\"") return """ + +++++ Fileメニューの"Exit"がクリックされたときに実行されるコールバック関数 +++++ + """ + def menu_exit_clicked(self, event=None): + self.master.destroy() + + + + + """ +++++ 現在のウェイポイント情報を指定のファイルに書き込む +++++ """ def save_waypoints(self, path): @@ -539,7 +698,7 @@ """ - +++++ 引数の同次変換行列(mat_affine)にx,yの平行移動を加えた同次変換行列を返す +++++ + +++++ 同次変換行列(mat_affine)にx,yの平行移動を加える +++++ """ def translate_mat(self, x, y): mat = np.eye(3) @@ -551,7 +710,7 @@ """ - +++++ 引数のmat_affineにscale倍のリサイズを加えた同次変換行列を返す +++++ + +++++ mat_affineにscale倍のリサイズを加える +++++ """ def scale_mat(self, scale): mat = np.eye(3) @@ -563,16 +722,16 @@ """ - +++++ 元画像をaffne変換して描画、その画像を返す +++++ + +++++ 元画像をaffne変換して描画 +++++ """ def draw_image(self): mat_inv = np.linalg.inv(self.mat_affine) img = self.__map_img_pil.transform( - (self.canv_w, self.canv_w), - Image.Transform.AFFINE, tuple(mat_inv.flatten()), - Image.Resampling.NEAREST, - fillcolor = 160 - ) + (self.canv_w, self.canv_w), + Image.Transform.AFFINE, tuple(mat_inv.flatten()), + Image.Resampling.NEAREST, + fillcolor = 160 + ) self.draw_img_tk = ImageTk.PhotoImage(image=img) # 描画する画像を変数に保持 if not self.canvas.find_withtag("map_image"): # 初めて画像を描画するとき self.canvas.create_image(0, 0, anchor='nw', image=self.draw_img_tk, tags="map_image") # 画像の描画 @@ -595,12 +754,33 @@ + """ + +++++ キャンバス上（ウィンドウ上）の座標から、元の地図画像上の座標に変換 +++++ + """ + def canvas2image(self, cx, cy): + mat_inv = np.linalg.inv(self.mat_affine) + img_xy = np.dot(mat_inv, [cx, cy, 1]) + return img_xy[0], img_xy[1] + + + + """ + +++++ 元の地図画像上の座標から、実際の座標に変換 +++++ + """ + def image2real(self, ix, iy): + x = (ix - self.img_origin[0]) * self.map_resolution + y = (-iy + self.img_origin[1]) * self.map_resolution + x = round(x, 6) + y = round(y, 6) + return x, y + + #===== メイン処理　プログラムはここから実行される =====# if __name__ == "__main__": #rospy.init_node("manager_GUI") root = tk.Tk() # 大元になるウィンドウ - w, h = root.winfo_screenwidth()-100, root.winfo_screenheight()-300 + w, h = root.winfo_screenwidth()-10, root.winfo_screenheight()-100 root.geometry("%dx%d+0+0" % (w, h)) app = Application(master=root) # tk.Frameを継承したApplicationクラスのインスタンス app.mainloop() diff --git a/waypoint_manager/scripts/manager_GUI.py b/waypoint_manager/scripts/manager_GUI.py index e69de29..4265cc3 100755 --- a/waypoint_manager/scripts/manager_GUI.py +++ b/waypoint_manager/scripts/manager_GUI.py @@ -0,0 +1 @@ +#!/usr/bin/env python diff --git a/waypoint_nav/CMakeLists.txt b/waypoint_nav/CMakeLists.txt index af262e2..b4c1003 100644 --- a/waypoint_nav/CMakeLists.txt +++ b/waypoint_nav/CMakeLists.txt @@ -7,196 +7,55 @@ ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages -find_package(catkin REQUIRED) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) +find_package(catkin REQUIRED + move_base_msgs + geometry_msgs + move_base + roscpp + actionlib_msgs + actionlib + tf + std_srvs + dynamic_reconfigure + base_local_planner + dwa_local_planner + std_msgs +) -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() -################################################ -## Declare ROS messages, services and actions ## -################################################ +find_package(PkgConfig) +pkg_search_module(yaml-cpp REQUIRED yaml-cpp) +pkg_search_module(waypoint_saver REQUIRED waypoint_saver) -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a exec_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) +if(NOT ${yaml-cpp_VERSION} VERSION_LESS "0.5") +add_definitions(-DNEW_YAMLCPP) +endif() -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# std_msgs # Or other packages containing msgs -# ) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( -# INCLUDE_DIRS include -# LIBRARIES waypoint_nav -# CATKIN_DEPENDS other_catkin_pkg -# DEPENDS system_lib + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} + DEPENDS waypoint_saver ) -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations include_directories( -# include -# ${catkin_INCLUDE_DIRS} + include + ${catkin_INCLUDE_DIRS} + ${yaml-cpp_INCLUDE_DIRS} + ${waypoint_saver_INCLUDE_DIRS} + ../waypoint_saver/include ) -## Declare a C++ library -# add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/waypoint_nav.cpp -# ) +add_executable(waypoint_nav src/waypoint_nav.cpp) -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(waypoint_nav + ${catkin_LIBRARIES} + ${yaml-cpp_LIBRARIES} +) -## Declare a C++ executable -## With catkin_make all packages are built within a single CMake context -## The recommended prefix ensures that target names across packages don't collide -# add_executable(${PROJECT_NAME}_node src/waypoint_nav_node.cpp) +add_executable(velocity_controller src/velocity_controller.cpp) -## Rename C++ executable without prefix -## The above recommended prefix causes long target names, the following renames the -## target back to the shorter version for ease of user use -## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" -# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") +target_link_libraries(velocity_controller + ${catkin_LIBRARIES} +) -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against -# target_link_libraries(${PROJECT_NAME}_node -# ${catkin_LIBRARIES} -# ) - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# catkin_install_python(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html -# install(TARGETS ${PROJECT_NAME}_node -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark libraries for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html -# install(TARGETS ${PROJECT_NAME} -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_waypoint_nav.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) diff --git a/waypoint_nav/include/waypoint_nav.h b/waypoint_nav/include/waypoint_nav.h index e69de29..de4896e 100644 --- a/waypoint_nav/include/waypoint_nav.h +++ b/waypoint_nav/include/waypoint_nav.h @@ -0,0 +1,34 @@ +// #include +// #include // include Waypoint class +// #include + + +// class WaypointArray : public geometry_msgs::PoseArray { +// /* +// std_msgs/Header header +// geometry_msgs/Pose[] poses +// */ +// public: +// std::vector waypoint_list; + + +// void add_waypoint(double x, double y, double z, float vel, float rad) +// { +// Waypoint point; +// point.set_x(x); +// point.set_y(y); +// point.set_z(z); +// point.set_vel(vel); +// point.set_rad(rad); +// waypoint_list.push_back(point); +// } + + +// geometry_msgs::PoseArray get_posearray_msg() +// { +// geometry_msgs::PoseArray posearray; +// posearray.header = header; +// posearray.poses = poses; +// return posearray; +// } +// }; \ No newline at end of file diff --git a/waypoint_nav/launch/waypoint_nav.launch b/waypoint_nav/launch/waypoint_nav.launch index e69de29..b8127ff 100644 --- a/waypoint_nav/launch/waypoint_nav.launch +++ b/waypoint_nav/launch/waypoint_nav.launch @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/waypoint_nav/maps/map_gakunai.yaml b/waypoint_nav/maps/map_gakunai.yaml index 51664a5..27872d5 100644 --- a/waypoint_nav/maps/map_gakunai.yaml +++ b/waypoint_nav/maps/map_gakunai.yaml @@ -1,4 +1,4 @@ -image: /home/ubuntu/catkin_ws/src/orne_navigation/orne_navigation_executor/maps/mymap.pgm +image: map_gakunai.pgm resolution: 0.050000 origin: [-100.000000, -100.000000, 0.000000] negate: 0 diff --git a/waypoint_nav/package.xml b/waypoint_nav/package.xml index 45f9037..b993d36 100644 --- a/waypoint_nav/package.xml +++ b/waypoint_nav/package.xml @@ -16,40 +16,37 @@ TODO - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - catkin + move_base + yaml-cpp + move_base_msgs + geometry_msgs + roscpp + actionlib_msgs + actionlib + tf + std_srvs + dynamic_reconfigure + base_local_planner + dwa_local_planner + waypoint_saver + std_msgs + + move_base + yaml-cpp + move_base_msgs + geometry_msgs + roscpp + actionlib_msgs + actionlib + tf + dynamic_reconfigure + base_local_planner + dwa_local_planner + waypoint_saver + std_msgs + diff --git a/waypoint_nav/param/waypoints.yaml b/waypoint_nav/param/waypoints.yaml deleted file mode 100644 index f8d6b5b..0000000 --- a/waypoint_nav/param/waypoints.yaml +++ /dev/null @@ -1,10 +0,0 @@ -waypoints: -- point: {x: -0.996902, y: -0.499032, z: 0.0, vel: 1.0, rad: 0.5} -- point: {x: 0.203745, y: -0.496466, z: 0.0, vel: 0.5, rad: 0.2} -- point: {x: 0.666450, y: 0.504773, z: 0.0, vel: 0.5, rad: 0.3} -- point: {x: 0.515219, y: 2.015219, z: 0.0, vel: 1.0, rad: 0.5} -finish_pose: - header: {seq: 0.0, stamp: 1.6203674303803937E9, frame_id: base_link} - pose: - position: {x: -0.550981, y: -0.550981, z: 0.0} - orientation: {x: 0.0, y: 0.0, z: 0.99749, w: 0.07074} diff --git a/waypoint_nav/param/waypoints_gakunai.yaml b/waypoint_nav/param/waypoints_gakunai.yaml deleted file mode 100644 index f4c66b1..0000000 --- a/waypoint_nav/param/waypoints_gakunai.yaml +++ /dev/null @@ -1,100 +0,0 @@ -waypoints: - - point: - x: 15.549 - y: 1.99887 - z: 0 - - point: - x: 27.7033 - y: 0.740999 - z: 0 - - point: - x: 44.5033 - y: -3.88193 - z: 0 - - point: - x: 53.4473 - y: -1.98026 - z: 0 - - point: - x: 53.5381 - y: 13.9875 - z: 0 - - point: - x: 53.0202 - y: 25.7033 - z: 0 - - point: - x: 52.624 - y: 40.8326 - z: 0 - - point: - x: 52.4651 - y: 56.1678 - z: 0 - - point: - x: 47.4651 - y: 55.9678 - z: 0 - - point: - x: 41.1833 - y: 55.7508 - z: 0 - - point: - x: 30.9773 - y: 56.3972 - z: 0 - - point: - x: 19.605 - y: 55.9349 - z: 0 - - point: - x: 9.74244 - y: 57.775 - z: 0 - - point: - x: 9.04664 - y: 53.1814 - z: 0 - - point: - x: 12.2474 - y: 45.1187 - z: 0 - - point: - x: 17.5568 - y: 32.2592 - z: 0 - - point: - x: 19.6331 - y: 27.0299 - z: 0 - - point: - x: 23.7635 - y: 17.3945 - z: 0 - - point: - x: 27.6797 - y: 6.92171 - z: 0 - - point: - x: 29.8247 - y: 1.30448 - z: 0 - - point: - x: 12.7818 - y: 1.13019 - z: 0 -finish_pose: - header: - seq: 0 - stamp: 1623386648.025251015 - frame_id: base_link - pose: - position: - x: -0.8863756 - y: 0.5020624 - z: 0 - orientation: - x: 0 - y: 0 - z: 0.999797 - w: -0.0201511 diff --git a/waypoint_nav/rviz_cfg/nav.rviz b/waypoint_nav/rviz_cfg/nav.rviz new file mode 100644 index 0000000..4e51bd4 --- /dev/null +++ b/waypoint_nav/rviz_cfg/nav.rviz @@ -0,0 +1,334 @@ +Panels: + - Class: rviz/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: ~ + Splitter Ratio: 0.5 + Tree Height: 532 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: LaserScan + - Class: orne_rviz_plugins/StateTriggerPanel + Name: StateTriggerPanel +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 0.699999988079071 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: /map + Unreliable: false + Use Timestamp: false + Value: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_scan: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + caster_back_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + imu_link: + Alpha: 1 + Show Axes: false + Show Trail: false + wheel_left_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wheel_right_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: RobotModel + Robot Description: robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/LaserScan + Color: 0; 255; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 170; 0; 0 + Min Color: 0; 85; 0 + Name: LaserScan + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 5 + Size (m): 0.10000000149011612 + Style: Points + Topic: /scan + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Arrow Length: 0.30000001192092896 + Axes Length: 0.30000001192092896 + Axes Radius: 0.009999999776482582 + Class: rviz/PoseArray + Color: 255; 25; 0 + Enabled: true + Head Length: 0.07000000029802322 + Head Radius: 0.029999999329447746 + Name: Particles + Queue Size: 10 + Shaft Length: 0.23000000417232513 + Shaft Radius: 0.009999999776482582 + Shape: Arrow (Flat) + Topic: /particlecloud + Unreliable: false + Value: true + - Alpha: 1 + Class: rviz/Polygon + Color: 25; 255; 0 + Enabled: true + Name: Footprint + Queue Size: 10 + Topic: /move_base/global_costmap/obstacle_layer_footprint/footprint_stamped + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /move_base/NavfnROS/plan + Unreliable: false + Value: true + - Alpha: 0.699999988079071 + Class: rviz/Map + Color Scheme: costmap + Draw Behind: true + Enabled: true + Name: GlobalCostmap + Topic: /move_base/global_costmap/costmap + Unreliable: false + Use Timestamp: false + Value: true + - Alpha: 0.699999988079071 + Class: rviz/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: LocalCostmap + Topic: /move_base/local_costmap/costmap + Unreliable: false + Use Timestamp: false + Value: true + - Alpha: 1 + Arrow Length: 0.30000001192092896 + Axes Length: 0.30000001192092896 + Axes Radius: 0.009999999776482582 + Class: rviz/PoseArray + Color: 0; 0; 127 + Enabled: true + Head Length: 0.07000000029802322 + Head Radius: 0.029999999329447746 + Name: Waypoints + Queue Size: 10 + Shaft Length: 0.23000000417232513 + Shaft Radius: 0.009999999776482582 + Shape: Arrow (Flat) + Topic: /waypoints + Unreliable: false + Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz/Pose + Color: 0; 0; 127 + Enabled: false + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: Pose + Queue Size: 10 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Arrow + Topic: /move_base_simple/goal + Unreliable: false + Value: false + - Alpha: 1 + Class: rviz/PointStamped + Color: 204; 41; 204 + Enabled: false + History Length: 10 + Name: PointStamped + Queue Size: 10 + Radius: 0.20000000298023224 + Topic: /target_point + Unreliable: false + Value: false + - Alpha: 1 + Class: rviz/PointStamped + Color: 239; 41; 41 + Enabled: false + History Length: 1 + Name: PointStamped + Queue Size: 10 + Radius: 0.4000000059604645 + Topic: /filtered_target_point + Unreliable: false + Value: false + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: map + Frame Rate: 10 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/ThirdPersonFollower + Distance: 11.816969871520996 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: -0.31653428077697754 + Y: -0.3555675148963928 + Z: 0 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 1.5697963237762451 + Target Frame: base_link + Yaw: 3.145397424697876 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 778 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd000000040000000000000193000002affc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003e00000251000000cb00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000002200530074006100740065005400720069006700670065007200500061006e0065006c0100000295000000580000004100ffffff000000010000010f000002b3fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003e000002b3000000a500fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005e00000003bfc0100000002fb0000000800540069006d00650000000000000005e00000046900fffffffb0000000800540069006d0065010000000000000450000000000000000000000376000002af00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + StateTriggerPanel: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1295 + X: -1 + Y: -17 diff --git a/waypoint_nav/src/velocity_controller.cpp b/waypoint_nav/src/velocity_controller.cpp new file mode 100644 index 0000000..d064beb --- /dev/null +++ b/waypoint_nav/src/velocity_controller.cpp @@ -0,0 +1,81 @@ +#include +#include +#include +#include +#include + +#include + + +class VelocityController { +public: + VelocityController() : + move_base_action_("move_base", true), + standard_vel_(1.0), + current_max_vel_(1.0), + min_vel_(0.0) + { + while((move_base_action_.waitForServer(ros::Duration(1.0)) == false) && (ros::ok() == true)) { + ROS_INFO("Waiting..."); + } + + ros::NodeHandle nh; + max_vel_sub_ = nh.subscribe("/max_vel", 1, &VelocityController::maxVelCallback, this); + cmd_vel_sub_ = nh.subscribe("/cmd_vel_in", 10, &VelocityController::cmdVelCallback, this); + cmd_vel_pub_ = nh.advertise("/cmd_vel_out", 10); + + std::string planner = "base_local_planner/TrajectoryPlannerROS"; + std::string param_max = "/move_base/TrajectoryPlannerROS/max_vel_x"; + std::string param_min = "/move_base/TrajectoryPlannerROS/min_vel_x"; + nh.param("/move_base/base_local_planner", planner, planner); + if (planner == "dwa_local_planner/DWAPlannerROS") { + param_max = "/move_base/DWAPlannerROS/max_vel_trans"; + param_min = "/move_base/DWAPlannerROS/min_vel_trans"; + } + nh.param(param_max, standard_vel_, standard_vel_); + nh.param(param_min, min_vel_, min_vel_); + ROS_INFO_STREAM("Standard max vel: " << standard_vel_); + } + + + + void maxVelCallback(const std_msgs::Float32 &msg) + { + current_max_vel_ = standard_vel_ * msg.data; + current_max_vel_ = std::max(current_max_vel_, min_vel_); + ROS_INFO_STREAM("Set max vel: " << current_max_vel_); + } + + + + void cmdVelCallback(const geometry_msgs::Twist &msg) + { + pub_msg_.linear.x = std::min(float(msg.linear.x), current_max_vel_); + pub_msg_.angular = msg.angular; + cmd_vel_pub_.publish(pub_msg_); + } + + + +private: + actionlib::SimpleActionClient move_base_action_; + ros::Subscriber max_vel_sub_; + ros::Subscriber cmd_vel_sub_; + ros::Publisher cmd_vel_pub_; + float standard_vel_, current_max_vel_, min_vel_; + geometry_msgs::Twist pub_msg_; +}; + + + + + + + +int main(int argc, char *argv[]) { + ros::init(argc, argv, "velocity_controller"); + VelocityController vc; + + ros::spin(); + return 0; +} \ No newline at end of file diff --git a/waypoint_nav/src/waypoint_nav.cpp b/waypoint_nav/src/waypoint_nav.cpp index e69de29..c4e74a0 100644 --- a/waypoint_nav/src/waypoint_nav.cpp +++ b/waypoint_nav/src/waypoint_nav.cpp @@ -0,0 +1,470 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include // include Waypoint class + +#include +#include +#include +#include +#include + +#ifdef NEW_YAMLCPP +template +void operator >> (const YAML::Node& node, T& i) +{ + i = node.as(); +} +#endif + + +class SwitchRunningStatus : public std::exception { +public: + SwitchRunningStatus() : std::exception() { } +}; + + + +class WaypointsNavigation{ +public: + /* + ++++++++++ Constructer ++++++++++ + */ + WaypointsNavigation() : + has_activate_(false), + move_base_action_("move_base", true), + rate_(10), + last_moved_time_(0), + dist_err_(0.8) + { + while((move_base_action_.waitForServer(ros::Duration(1.0)) == false) && (ros::ok() == true)) + { + ROS_INFO("Waiting..."); + } + ros::NodeHandle private_nh("~"); + private_nh.param("robot_frame", robot_frame_, std::string("base_link")); + private_nh.param("world_frame", world_frame_, std::string("map")); + + double max_update_rate; + private_nh.param("max_update_rate", max_update_rate, 10.0); + rate_ = ros::Rate(max_update_rate); + std::string filename = ""; + private_nh.param("filename", filename, filename); + if (filename != "") { + ROS_INFO_STREAM("Read waypoints data from " << filename); + if (!readFile(filename)) { + ROS_ERROR("Failed loading waypoints file"); + } else { + finish_pose_ = waypoints_.poses.end()-1; + computeWpOrientation(); + } + current_waypoint_ = waypoints_.poses.begin(); + wp_num_.data = 1; + } else { + ROS_ERROR("waypoints file doesn't have name"); + } + + ros::NodeHandle nh; + start_server_ = nh.advertiseService("start_wp_nav", &WaypointsNavigation::startNavigationCallback, this); + stop_server_ = nh.advertiseService("stop_wp_nav", &WaypointsNavigation::stopNavigationCallback, this); + resume_server_ = nh.advertiseService("resume_nav", &WaypointsNavigation::resumeNavigationCallback, this); + cmd_vel_sub_ = nh.subscribe("cmd_vel", 1, &WaypointsNavigation::cmdVelCallback, this); + wp_pub_ = nh.advertise("waypoints", 10); + max_vel_pub_ = nh.advertise("max_vel", 5); + wp_num_pub_ = nh.advertise("waypoint_number", 5); + clear_costmaps_srv_ = nh.serviceClient("/move_base/clear_costmaps"); + } + + + + /* + ++++++++++ Read waypoints file ++++++++++ + */ + bool readFile(const std::string &filename) + { + waypoints_.poses.clear(); + try { + std::ifstream ifs(filename.c_str(), std::ifstream::in); + if (ifs.good() == false) return false; + + YAML::Node node; + #ifdef NEW_YAMLCPP + node = YAML::Load(ifs); + #else + YAML::Parser parser(ifs); + parser.GetNextDocument(node); + #endif + + // Read waypoints from yaml file + #ifdef NEW_YAMLCPP + const YAML::Node &wp_node_tmp = node["waypoints"]; + const YAML::Node *wp_node = wp_node_tmp ? &wp_node_tmp : NULL; + #else + const YAML::Node *wp_node = node.FindValue("waypoints"); + #endif + geometry_msgs::Pose pose; + Waypoint point; + if (wp_node != NULL) { + for(int i=0; i < wp_node->size(); i++) { + (*wp_node)[i]["point"]["x"] >> pose.position.x; + (*wp_node)[i]["point"]["y"] >> pose.position.y; + (*wp_node)[i]["point"]["z"] >> pose.position.z; + waypoints_.poses.push_back(pose); + point.x = pose.position.x; + point.y = pose.position.y; + point.z = pose.position.z; + (*wp_node)[i]["point"]["vel"] >> point.vel; + (*wp_node)[i]["point"]["rad"] >> point.rad; + (*wp_node)[i]["point"]["stop"] >> point.stop; + waypoint_list_.push_back(point); + } + } else { + return false; + } + + // Read finish_pose + #ifdef NEW_YAMLCPP + const YAML::Node &fp_node_tmp = node["finish_pose"]; + const YAML::Node *fp_node = fp_node_tmp ? &fp_node_tmp : NULL; + #else + const YAML::Node *fp_node = node.FindValue("finish_pose"); + #endif + if (fp_node != NULL) { + (*fp_node)["pose"]["position"]["x"] >> pose.position.x; + (*fp_node)["pose"]["position"]["y"] >> pose.position.y; + (*fp_node)["pose"]["position"]["z"] >> pose.position.z; + (*fp_node)["pose"]["orientation"]["x"] >> pose.orientation.x; + (*fp_node)["pose"]["orientation"]["y"] >> pose.orientation.y; + (*fp_node)["pose"]["orientation"]["z"] >> pose.orientation.z; + (*fp_node)["pose"]["orientation"]["w"] >> pose.orientation.w; + waypoints_.poses.push_back(pose); + point.x = pose.position.x; + point.y = pose.position.y; + point.z = pose.position.z; + waypoint_list_.push_back(point); + } else { + return false; + } + + } catch(YAML::ParserException &e) { + return false; + + } catch(YAML::RepresentationException &e) { + return false; + } + + return true; + } + + + + /* + ++++++++++ Compute target orientation for each waypoint ++++++++++ + */ + void computeWpOrientation() + { + for(std::vector::iterator it = waypoints_.poses.begin(); it != finish_pose_; it++) { + double goal_direction = atan2((it+1)->position.y - (it)->position.y, + (it+1)->position.x - (it)->position.x); + (it)->orientation = tf::createQuaternionMsgFromYaw(goal_direction); + } + waypoints_.header.frame_id = world_frame_; + } + + + + /* + ++++++++++ Callback function for "StartWaypointsNavigation" button on rviz ++++++++++ + */ + bool startNavigationCallback(std_srvs::Trigger::Request &request, std_srvs::Trigger::Response &response) + { + if ((has_activate_) || (current_waypoint_ != waypoints_.poses.begin())) { + ROS_WARN("Waypoint navigation is already started"); + response.success = false; + return false; + } + std_srvs::Empty empty; + while(!clear_costmaps_srv_.call(empty)) { + ROS_WARN("Resend clear costmap service"); + sleep(); + } + current_waypoint_ = waypoints_.poses.begin(); + wp_num_.data = 1; + has_activate_ = true; + response.success = true; + return true; + } + + + + /* + ++++++++++ Callback function for "ResumeWaypointsNavigation" button on rviz ++++++++++ + */ + bool resumeNavigationCallback(std_srvs::Trigger::Request &request, std_srvs::Trigger::Response &response) + { + if (has_activate_) { + ROS_WARN("Navigation is already active"); + response.success = false; + } else { + ROS_INFO("Navigation has resumed"); + has_activate_ = true; + response.success = true; + } + return true; + } + + + + /* + ++++++++++ Callback function for service of "stop_wp_nav" ++++++++++ + */ + bool stopNavigationCallback(std_srvs::Trigger::Request &request, std_srvs::Trigger::Response &response) + { + if (has_activate_) { + ROS_INFO("Waypoint navigation has been stopped"); + has_activate_ = false; + response.success = true; + } else { + ROS_WARN("Waypoint navigation is already stopped"); + response.success = false; + } + return true; + } + + + + /* + ++++++++++ Callback function for cmd_vel topic ++++++++++ + */ + void cmdVelCallback(const geometry_msgs::Twist &msg) + { + if (has_activate_ && + msg.linear.x > -0.001 && msg.linear.x < 0.001 && + msg.linear.y > -0.001 && msg.linear.y < 0.001 && + msg.linear.z > -0.001 && msg.linear.z < 0.001 && + msg.angular.x > -0.001 && msg.angular.x < 0.001 && + msg.angular.y > -0.001 && msg.angular.y < 0.001 && + msg.angular.z > -0.001 && msg.angular.z < 0.001) + { + // ROS_INFO("command velocity all zero"); + } else { + last_moved_time_ = ros::Time::now().toSec(); + } + } + + + + /* + ++++++++++ Check if robot reached the goal sent to move_base ++++++++++ + */ + bool navigationFinished() + { + return move_base_action_.getState() == actionlib::SimpleClientGoalState::SUCCEEDED; + } + + + + /* + ++++++++++ Check if robot reached current waypoint ++++++++++ + */ + bool onNavigationPoint(const geometry_msgs::Point &dest, double dist_err = 0.8) + { + if (waypoint_list_[wp_num_.data-1].stop) { + return navigationFinished(); + } + tf::StampedTransform robot_gl = getRobotPosGL(); + const double wx = dest.x; + const double wy = dest.y; + const double rx = robot_gl.getOrigin().x(); + const double ry = robot_gl.getOrigin().y(); + const double dist = std::sqrt(std::pow(wx - rx, 2) + std::pow(wy - ry, 2)); + return dist < dist_err; + } + + + + /* + ++++++++++ Get gloabl position of robot ++++++++++ + */ + tf::StampedTransform getRobotPosGL() + { + tf::StampedTransform robot_gl; + try { + tf_listener_.lookupTransform(world_frame_, robot_frame_, ros::Time(0.0), robot_gl); + } catch(tf::TransformException &e) { + ROS_WARN_STREAM("tf::TransformException: " << e.what()); + } + return robot_gl; + } + + + + /* + ++++++++++ Sleep function used in main loop function ++++++++++ + */ + void sleep() + { + rate_.sleep(); + ros::spinOnce(); + publishPoseArray(); + } + + + + /* + ++++++++++ Publish waypoints to be displayed as arrows on rviz ++++++++++ + */ + void publishPoseArray() + { + waypoints_.header.stamp = ros::Time::now(); + wp_pub_.publish(waypoints_); + } + + + + /* + ++++++++++ Send goal to move_base ++++++++++ + */ + void startNavigationGL(const geometry_msgs::Pose &dest) + { + setMaxVel(); + move_base_msgs::MoveBaseGoal move_base_goal; + move_base_goal.target_pose.header.stamp = ros::Time::now(); + move_base_goal.target_pose.header.frame_id = world_frame_; + move_base_goal.target_pose.pose.position = dest.position; + move_base_goal.target_pose.pose.orientation = dest.orientation; + move_base_action_.sendGoal(move_base_goal); + } + + + + /* + ++++++++++ Publish ratio to maximum speed ++++++++++ + */ + void setMaxVel() + { + float max_vel_rate = waypoint_list_[wp_num_.data-1].vel; + max_vel_msg_.data = max_vel_rate; + max_vel_pub_.publish(max_vel_msg_); + } + + + + /* + ++++++++++ Main loop function ++++++++++ + */ + void run() + { + while(ros::ok()) { + // has_activate_ is false, nothing to do + if (!has_activate_) { + sleep(); + continue; + } + // go to fianal goal and finish process + if (current_waypoint_ == finish_pose_) { + ROS_INFO_STREAM("Go to final goal"); + startNavigationGL(*current_waypoint_); + while(!navigationFinished() && ros::ok()) sleep(); + ROS_INFO("Final goal reached!!"); + has_activate_ = false; + continue; + } + // go to current waypoint + ROS_INFO_STREAM("Go to waypoint " << wp_num_.data); + startNavigationGL(*current_waypoint_); + int resend_goal = 0; + double start_nav_time = ros::Time::now().toSec(); + dist_err_ = waypoint_list_[wp_num_.data-1].rad; + try { + // loop until reach waypoint + while(!onNavigationPoint(current_waypoint_->position, dist_err_)) { + if (!has_activate_) { + throw SwitchRunningStatus(); + } + double time = ros::Time::now().toSec(); + if (time - start_nav_time > 10.0 && time - last_moved_time_ > 10.0) { + ROS_WARN("Resend the navigation goal."); + std_srvs::Empty empty; + clear_costmaps_srv_.call(empty); + startNavigationGL(*current_waypoint_); + resend_goal++; + if (resend_goal == 3) { + ROS_WARN("Skip waypoint."); + current_waypoint_++; + wp_num_.data++; + startNavigationGL(*current_waypoint_); + } + start_nav_time = time; + } + wp_num_pub_.publish(wp_num_); + sleep(); + } + // if current waypoint is stop point + if (waypoint_list_[wp_num_.data-1].stop) { + has_activate_ = false; + move_base_action_.cancelAllGoals(); + ROS_INFO("Waiting for navigation to resume..."); + } + // update current waypoint + current_waypoint_++; + wp_num_.data++; + + } catch(const SwitchRunningStatus &e) { + ROS_INFO_STREAM("Running status switched"); + } + sleep(); + } + } + + + + +private: + actionlib::SimpleActionClient move_base_action_; + geometry_msgs::PoseArray waypoints_; + visualization_msgs::MarkerArray marker_; + std::vector waypoint_list_; + std::vector::iterator current_waypoint_; + std::vector::iterator finish_pose_; + bool has_activate_; + std::string robot_frame_, world_frame_; + tf::TransformListener tf_listener_; + ros::Rate rate_; + ros::ServiceServer start_server_, stop_server_, resume_server_; + ros::Subscriber cmd_vel_sub_; + ros::Publisher wp_pub_, max_vel_pub_, wp_num_pub_; + ros::ServiceClient clear_costmaps_srv_; + double last_moved_time_, dist_err_; + std_msgs::UInt16 wp_num_; + std_msgs::Float32 max_vel_msg_; +}; + + + + + + +int main(int argc, char *argv[]) { + ros::init(argc, argv, ROS_PACKAGE_NAME); + WaypointsNavigation w_nav; + w_nav.run(); + + return 0; +} \ No newline at end of file diff --git a/waypoint_nav/waypoints_cfg/waypoints.yaml b/waypoint_nav/waypoints_cfg/waypoints.yaml new file mode 100644 index 0000000..92f70d6 --- /dev/null +++ b/waypoint_nav/waypoints_cfg/waypoints.yaml @@ -0,0 +1,10 @@ +waypoints: +- point: {x: -0.45, y: -0.499032, z: 0.0, vel: 1.0, rad: 0.3, stop: false} +- point: {x: 0.50, y: -0.576395, z: 0.0, vel: 0.5, rad: 0.1, stop: true} +- point: {x: 0.50, y: 0.80, z: 0.0, vel: 1.0, rad: 0.3, stop: false} +- point: {x: 0.515219, y: 1.8, z: 0.0, vel: 0.3, rad: 0.1, stop: true} +finish_pose: + header: {seq: 0.0, stamp: 1620367430.3803937, frame_id: base_link} + pose: + position: {x: -0.550981, y: 1.8, z: 0.0} + orientation: {x: 0.0, y: 0.0, z: 0.99749, w: 0.07074} \ No newline at end of file diff --git a/waypoint_nav/waypoints_cfg/waypoints_gakunai.yaml b/waypoint_nav/waypoints_cfg/waypoints_gakunai.yaml new file mode 100644 index 0000000..1b42abe --- /dev/null +++ b/waypoint_nav/waypoints_cfg/waypoints_gakunai.yaml @@ -0,0 +1,27 @@ +waypoints: +- point: {x: 15.549, y: 1.99887, z: 0} +- point: {x: 27.7033, y: 0.740999, z: 0} +- point: {x: 44.367489, y: -3.73285, z: 0.0} +- point: {x: 53.4473, y: -1.98026, z: 0} +- point: {x: 53.5381, y: 13.9875, z: 0} +- point: {x: 53.0202, y: 25.7033, z: 0} +- point: {x: 52.624, y: 40.8326, z: 0} +- point: {x: 52.4651, y: 56.1678, z: 0} +- point: {x: 47.4651, y: 55.9678, z: 0} +- point: {x: 41.1833, y: 55.7508, z: 0} +- point: {x: 30.9773, y: 56.3972, z: 0} +- point: {x: 19.605, y: 55.9349, z: 0} +- point: {x: 9.74244, y: 57.775, z: 0} +- point: {x: 9.04664, y: 53.1814, z: 0} +- point: {x: 12.2474, y: 45.1187, z: 0} +- point: {x: 17.5568, y: 32.2592, z: 0} +- point: {x: 19.6331, y: 27.0299, z: 0} +- point: {x: 23.7635, y: 17.3945, z: 0} +- point: {x: 27.6797, y: 6.92171, z: 0} +- point: {x: 29.8247, y: 1.30448, z: 0} +- point: {x: 12.7818, y: 1.13019, z: 0} +finish_pose: + header: {seq: 0, stamp: 1623386648.025251, frame_id: base_link} + pose: + position: {x: -0.8863756, y: 0.5020624, z: 0} + orientation: {x: 0, y: 0, z: 0.999797, w: -0.0201511} \ No newline at end of file diff --git a/waypoint_nav/waypoints_cfg/waypoints_gakunai_edit.yaml b/waypoint_nav/waypoints_cfg/waypoints_gakunai_edit.yaml new file mode 100644 index 0000000..5d13f3a --- /dev/null +++ b/waypoint_nav/waypoints_cfg/waypoints_gakunai_edit.yaml @@ -0,0 +1,27 @@ +waypoints: +- point: {x: 15.549, y: 1.99887, z: 0} +- point: {x: 27.7033, y: 0.740999, z: 0} +- point: {x: 44.5033, y: -3.88193, z: 0} +- point: {x: 53.4473, y: -1.98026, z: 0} +- point: {x: 53.5381, y: 13.9875, z: 0} +- point: {x: 53.0202, y: 25.7033, z: 0} +- point: {x: 52.624, y: 40.8326, z: 0} +- point: {x: 52.4651, y: 56.1678, z: 0} +- point: {x: 47.4651, y: 55.9678, z: 0} +- point: {x: 41.1833, y: 55.7508, z: 0} +- point: {x: 30.9773, y: 56.3972, z: 0} +- point: {x: 19.605, y: 55.9349, z: 0} +- point: {x: 9.74244, y: 57.775, z: 0} +- point: {x: 9.04664, y: 53.1814, z: 0} +- point: {x: 12.2474, y: 45.1187, z: 0} +- point: {x: 17.5568, y: 32.2592, z: 0} +- point: {x: 19.6331, y: 27.0299, z: 0} +- point: {x: 23.7635, y: 17.3945, z: 0} +- point: {x: 27.6797, y: 6.92171, z: 0} +- point: {x: 29.8247, y: 1.30448, z: 0} +- point: {x: 12.7818, y: 1.13019, z: 0} +finish_pose: + header: {seq: 0, stamp: 1623386648.025251, frame_id: base_link} + pose: + position: {x: -0.8863756, y: 0.5020624, z: 0} + orientation: {x: 0, y: 0, z: 0.999797, w: -0.0201511} \ No newline at end of file diff --git a/waypoint_nav/waypoints_cfg/waypoints_test.yaml b/waypoint_nav/waypoints_cfg/waypoints_test.yaml new file mode 100644 index 0000000..713cc07 --- /dev/null +++ b/waypoint_nav/waypoints_cfg/waypoints_test.yaml @@ -0,0 +1,13 @@ +waypoints: +- point: {x: -1.01321, y: -0.528956, z: -0.000870705, vel: 1, rad: 0.8, stop: 0} +- point: {x: -0.0284393, y: -0.471859, z: 0.00520468, vel: 1, rad: 0.8, stop: 0} +- point: {x: 0.981428, y: -0.5452, z: 0.00261879, vel: 1, rad: 0.8, stop: 0} +- point: {x: 1.90456, y: -0.536331, z: 0.00246334, vel: 1, rad: 0.8, stop: 0} +- point: {x: 1.95224, y: 0.538929, z: 0.00476408, vel: 1, rad: 0.8, stop: 0} +- point: {x: 0.603558, y: 0.551442, z: 0.00079298, vel: 1, rad: 0.8, stop: 0} +- point: {x: 0.539775, y: 1.787, z: 0.00275993, vel: 1, rad: 0.8, stop: 0} +finish_pose: + header: {seq: 0, stamp: 244.465000000, frame_id: map} + pose: + position: {x: -0.488029, y: 1.82672, z: 0} + orientation: {x: 0, y: 0, z: 0.999992, w: 0.00399584} diff --git a/waypoint_saver/CMakeLists.txt b/waypoint_saver/CMakeLists.txt index 497b438..4134cad 100644 --- a/waypoint_saver/CMakeLists.txt +++ b/waypoint_saver/CMakeLists.txt @@ -1,150 +1,37 @@ cmake_minimum_required(VERSION 3.0.2) project(waypoint_saver) -## Compile as C++11, supported in ROS Kinetic and newer -# add_compile_options(-std=c++11) - -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() +find_package(catkin REQUIRED + geometry_msgs + visualization_msgs + roscpp + tf +) -################################################ -## Declare ROS messages, services and actions ## -################################################ -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a exec_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# std_msgs # Or other packages containing msgs -# ) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( -# INCLUDE_DIRS include -# LIBRARIES waypoint_saver -# CATKIN_DEPENDS other_catkin_pkg -# DEPENDS system_lib + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} ) -########### -## Build ## -########### -## Specify additional locations of header files -## Your package locations should be listed before other locations + include_directories( -# include -# ${catkin_INCLUDE_DIRS} + include + ${catkin_INCLUDE_DIRS} + ${yaml-cpp_INCLUDE_DIRS} ) -## Declare a C++ library -# add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/waypoint_saver.cpp -# ) -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -## Declare a C++ executable -## With catkin_make all packages are built within a single CMake context -## The recommended prefix ensures that target names across packages don't collide -# add_executable(${PROJECT_NAME}_node src/waypoint_saver_node.cpp) -## Rename C++ executable without prefix -## The above recommended prefix causes long target names, the following renames the -## target back to the shorter version for ease of user use -## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" -# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") +add_executable(waypoint_saver src/waypoint_saver.cpp) -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against -# target_link_libraries(${PROJECT_NAME}_node -# ${catkin_LIBRARIES} -# ) +target_link_libraries(waypoint_saver + ${catkin_LIBRARIES} +) ############# ## Install ## diff --git a/waypoint_saver/include/waypoint_saver.h b/waypoint_saver/include/waypoint_saver.h index e69de29..9cf23da 100644 --- a/waypoint_saver/include/waypoint_saver.h +++ b/waypoint_saver/include/waypoint_saver.h @@ -0,0 +1,36 @@ +#include +#include + +class Waypoint { +public: + double x, y, z; + float vel; + float rad; + bool stop; + + Waypoint() : + x(0.0), + y(0.0), + z(0.0), + vel(1.0), + rad(0.8), + stop(false) + { + } + + + std::string getYAMLstr() + { + std::stringstream ss; + ss << "{"; + ss << "x: " << x << ", " ; + ss << "y: " << y << ", " ; + ss << "z: " << z << ", " ; + ss << "vel: " << vel << ", " ; + ss << "rad: " << rad << ", " ; + ss << "stop: " << (stop ? "true" : "false") << "}" ; + return ss.str(); + // {x: *, y: *, z: *, vel: *, rad: *, stop: *} + } + +}; \ No newline at end of file diff --git a/waypoint_saver/launch/waypoint_saver.launch b/waypoint_saver/launch/waypoint_saver.launch index e69de29..9873494 100644 --- a/waypoint_saver/launch/waypoint_saver.launch +++ b/waypoint_saver/launch/waypoint_saver.launch @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/waypoint_saver/package.xml b/waypoint_saver/package.xml index e64c74b..1ce6999 100644 --- a/waypoint_saver/package.xml +++ b/waypoint_saver/package.xml @@ -16,41 +16,20 @@ TODO - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - catkin + geometry_msgs + visualization_msgs + roscpp + tf + + geometry_msgs + visualization_msgs + roscpp + tf + + diff --git a/waypoint_saver/rviz_cfg/record_waypoints.rviz b/waypoint_saver/rviz_cfg/record_waypoints.rviz new file mode 100644 index 0000000..659846c --- /dev/null +++ b/waypoint_saver/rviz_cfg/record_waypoints.rviz @@ -0,0 +1,240 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: ~ + Splitter Ratio: 0.5 + Tree Height: 418 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: LaserScan +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 0.699999988079071 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: /map + Unreliable: false + Use Timestamp: false + Value: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_scan: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + caster_back_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + imu_link: + Alpha: 1 + Show Axes: false + Show Trail: false + wheel_left_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wheel_right_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: RobotModel + Robot Description: robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: LaserScan + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Points + Topic: /scan + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /waypoints + Name: Waypoints + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 1 + Class: rviz/PointStamped + Color: 204; 41; 204 + Enabled: true + History Length: 1 + Name: PointStamped + Queue Size: 10 + Radius: 0.20000000298023224 + Topic: /clicked_point + Unreliable: false + Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: Pose + Queue Size: 10 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Arrow + Topic: /move_base_simple/goal + Unreliable: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: map + Frame Rate: 10 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 9.116127014160156 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: -0.43540072441101074 + Y: -0.04363036900758743 + Z: 0.47935307025909424 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 1.179796576499939 + Target Frame: + Yaw: 3.1354191303253174 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 716 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000400000000000001560000022dfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003e0000022d000000cb00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000252fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000252000000a500fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000026c00fffffffb0000000800540069006d00650100000000000004500000000000000000000003540000022d00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1200 + X: 195 + Y: 59 diff --git a/waypoint_saver/src/waypoint_saver.cpp b/waypoint_saver/src/waypoint_saver.cpp index e69de29..61b6e02 100644 --- a/waypoint_saver/src/waypoint_saver.cpp +++ b/waypoint_saver/src/waypoint_saver.cpp @@ -0,0 +1,229 @@ +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include // include Waypoint class + + + +class WaypointsSaver{ +public: + WaypointsSaver() : + filename_("waypoints.yaml"), + default_rad_(0.8) + { + waypoints_viz_sub_ = nh_.subscribe("waypoints_viz", 1, &WaypointsSaver::waypointsVizCallback, this); + waypoints_joy_sub_ = nh_.subscribe("waypoints_joy", 1, &WaypointsSaver::waypointsJoyCallback, this); + finish_pose_sub_ = nh_.subscribe("finish_pose", 1, &WaypointsSaver::finishPoseCallback, this); + markers_pub_ = nh_.advertise("waypoints", 10); + + ros::NodeHandle private_nh("~"); + private_nh.param("filename", filename_, filename_); + private_nh.param("save_joy_button", save_joy_button_, 0); + private_nh.param("robot_frame", robot_frame_, std::string("base_link")); + private_nh.param("world_frame", world_frame_, std::string("map")); + private_nh.param("default_rad", default_rad_, default_rad_); + } + + + + /* + ++++++++++ Save waypoint by joy stick button ++++++++++ + */ + void waypointsJoyCallback(const sensor_msgs::Joy &msg) + { + static ros::Time saved_time(0.0); + if (msg.buttons[save_joy_button_] == 1 && (ros::Time::now() - saved_time).toSec() > 3.0) { + pushbackWaypoint(); + saved_time = ros::Time::now(); + } + publishMarkerArray(); + } + + + + /* + ++++++++++ Save waypoint by "Publish Point" on rviz ++++++++++ + */ + void waypointsVizCallback(const geometry_msgs::PointStamped &msg) + { + Waypoint point; + point.x = msg.point.x; + point.y = msg.point.y; + point.z = msg.point.z; + point.rad = default_rad_; + waypoints_.push_back(point); + addWaypointMarker(point); + publishMarkerArray(); + } + + + + /* + ++++++++++ Save finish pose by "2D Nav Goal" on rviz ++++++++++ + */ + void finishPoseCallback(const geometry_msgs::PoseStamped &msg) + { + finish_pose_ = msg; + save(); + waypoints_.clear(); + } + + + + /* + ++++++++++ Get robot's pose and add waypoint ++++++++++ + */ + void pushbackWaypoint() + { + tf::StampedTransform robot_gl; + try { + tf_listener_.lookupTransform(world_frame_, robot_frame_, ros::Time(0.0), robot_gl); + Waypoint point; + point.x = robot_gl.getOrigin().x(); + point.y = robot_gl.getOrigin().y(); + point.z = robot_gl.getOrigin().z(); + point.rad = default_rad_; + waypoints_.push_back(point); + addWaypointMarker(point); + } + catch(tf::TransformException &e) { + ROS_WARN_STREAM("tf::TransformException: " << e.what()); + } + } + + + + /* + ++++++++++ Add marker to display on rviz ++++++++++ + */ + void addWaypointMarker(Waypoint point) + { + double scale = 0.2; + int number = waypoints_.size(); + std::stringstream name; + name << "Waypoint " << number; + + visualization_msgs::Marker marker; + marker.header.frame_id = world_frame_; + marker.header.stamp = ros::Time::now(); + marker.ns = name.str(); + marker.id = number; + marker.type = visualization_msgs::Marker::SPHERE; + marker.action = visualization_msgs::Marker::ADD; + marker.pose.position.x = point.x; + marker.pose.position.y = point.y; + marker.pose.position.z = scale/2; + marker.scale.x = scale; + marker.scale.y = scale; + marker.scale.z = scale; + marker.color.r=0.8f; + marker.color.g=0.2f; + marker.color.b=0.2f; + marker.color.a = 1.0f; + + markers_.push_back(marker); + } + + + + /* + ++++++++++ Publish markerArray to display waypoints on rviz ++++++++++ + */ + void publishMarkerArray() + { + visualization_msgs::MarkerArray marker_array; + marker_array.markers = markers_; + markers_pub_.publish(marker_array); + } + + + + /* + ++++++++++ Save waypoints as yaml file ++++++++++ + */ + void save() + { + std::ofstream ofs(filename_.c_str(), std::ios::out); + /* + waypoints: + - point: {x: *, y: *, z: *, ...} + */ + ofs << "waypoints:" << std::endl; + for(int i=0; i < waypoints_.size(); i++) { + ofs << "- point: " << waypoints_[i].getYAMLstr() << std::endl; + } + /* + finish_pose: + header: {seq: *, stamp: *, frame_id: *} + pose: + positioin: {x: *, y: *, z: *} + orientation: {x: *, y: *, z: *, w: *} + */ + ofs << "finish_pose:" << std::endl; + ofs << " header: {" ; + ofs << "seq: " << finish_pose_.header.seq << ", " ; + ofs << "stamp: " << finish_pose_.header.stamp << ", " ; + ofs << "frame_id: " << finish_pose_.header.frame_id << "}" << std::endl; + ofs << " pose: " << std::endl; + ofs << " position: {" ; + ofs << "x: " << finish_pose_.pose.position.x << ", " ; + ofs << "y: " << finish_pose_.pose.position.y << ", " ; + ofs << "z: " << finish_pose_.pose.position.z << "}" << std::endl; + ofs << " orientation: {" ; + ofs << "x: " << finish_pose_.pose.orientation.x << ", " ; + ofs << "y: " << finish_pose_.pose.orientation.y << ", " ; + ofs << "z: " << finish_pose_.pose.orientation.z << ", " ; + ofs << "w: " << finish_pose_.pose.orientation.w << "}" << std::endl; + + ofs.close(); + ROS_INFO_STREAM("write success"); + } + + + + void run() + { + ros::spin(); + } + + + +private: + ros::Subscriber waypoints_viz_sub_; + ros::Subscriber waypoints_joy_sub_; + ros::Subscriber finish_pose_sub_; + ros::Publisher markers_pub_; + std::vector waypoints_; + std::vector markers_; + geometry_msgs::PoseStamped finish_pose_; + tf::TransformListener tf_listener_; + int save_joy_button_; + ros::NodeHandle nh_; + std::string filename_; + std::string world_frame_; + std::string robot_frame_; + float default_rad_; +}; + + + + + +int main(int argc, char *argv[]) +{ + ros::init(argc, argv, "waypoints_saver"); + WaypointsSaver saver; + saver.run(); + + return 0; +}