diff --git a/.env b/.env index 6c47f55cf..23c886d92 100644 --- a/.env +++ b/.env @@ -11,7 +11,7 @@ PROJECT_NAME="airstack" # If you've run ./airstack.sh setup, then this will auto-generate from the git commit hash every time a change is made # to a Dockerfile or docker-compose.yaml file. Otherwise this can also be set explicitly to make a release version. -VERSION="0.18.0-alpha.3" +VERSION="0.18.0-alpha.4" # Choose "dev" or "prebuilt". "dev" is for mounted code that must be built live. "prebuilt" is for built ros_ws baked into the image DOCKER_IMAGE_BUILD_MODE="dev" # Where to push and pull images from. Can replace with your docker hub username if using docker hub. @@ -30,7 +30,7 @@ RECORD_BAGS="false" # "true" or "false" # ============== ISAAC SIM ===================== ISAAC_SIM_GUI="/isaac-sim/AirStack/simulation/isaac-sim/assets/scenes/simple_pegasus.scene.usd" # Set to "true" to launch Isaac Sim using a standalone Python script instead of USD file -ISAAC_SIM_USE_STANDALONE="false" # "true" or "false" +ISAAC_SIM_USE_STANDALONE="true" # "true" or "false" # Script name (must be in /AirStack/simulation/isaac-sim/launch_scripts/) ISAAC_SIM_SCRIPT_NAME="example_one_px4_pegasus_launch_script.py" PLAY_SIM_ON_START="false" diff --git a/.gitmodules b/.gitmodules index 0b29f3486..4314795a4 100644 --- a/.gitmodules +++ b/.gitmodules @@ -8,9 +8,12 @@ path = robot/ros_ws/src/perception/macvo_ros2/macvo_ros2/macvo url = https://github.com/MAC-VO/MAC-VO.git [submodule "robot/ros_ws/src/rviz_behavior_tree_panel/xdot_cpp"] - path = common/ros_packages/gui/rviz_behavior_tree_panel/xdot_cpp + path = common/ros_packages/gui/rviz/rviz_behavior_tree_panel/xdot_cpp url = https://github.com/castacks/xdot_cpp.git [submodule "simulation/isaac-sim/extensions/PegasusSimulator"] path = simulation/isaac-sim/extensions/PegasusSimulator url = https://github.com/castacks/PegasusSimulator-AirStack-Integration.git +[submodule "common/ros_packages/gui/rviz/rviz_polygon_selection_tool"] + path = common/ros_packages/gui/rviz/rviz_polygon_selection_tool + url = https://github.com/swri-robotics/rviz_polygon_selection_tool.git diff --git a/common/ros_packages/desktop_bringup/launch/gcs.launch.xml b/common/ros_packages/desktop_bringup/launch/gcs.launch.xml index ac3dd2f2a..f0876e990 100644 --- a/common/ros_packages/desktop_bringup/launch/gcs.launch.xml +++ b/common/ros_packages/desktop_bringup/launch/gcs.launch.xml @@ -12,24 +12,6 @@ ?> - - - - - - - - - - - ?> - - - - \ No newline at end of file diff --git a/common/ros_packages/desktop_bringup/launch/robot.launch.xml b/common/ros_packages/desktop_bringup/launch/robot.launch.xml index 53bf44922..2d92337bb 100644 --- a/common/ros_packages/desktop_bringup/launch/robot.launch.xml +++ b/common/ros_packages/desktop_bringup/launch/robot.launch.xml @@ -16,13 +16,6 @@ - - - - - diff --git a/common/ros_packages/desktop_bringup/rviz/robot.rviz b/common/ros_packages/desktop_bringup/rviz/robot.rviz index 183df82e9..6ded1ee37 100644 --- a/common/ros_packages/desktop_bringup/rviz/robot.rviz +++ b/common/ros_packages/desktop_bringup/rviz/robot.rviz @@ -1,27 +1,24 @@ Panels: - Class: rviz_common/Displays - Help Height: 70 + Help Height: 0 Name: Displays Property Tree Widget: Expanded: - /Global Options1 - /TF1/Frames1 - - /Sensors1 - - /Perception1/StereoImageProc Disparity1 + - /Local1 - /Local1/DROAN1 - /Local1/DROAN1/Trimmed Global Plan for DROAN1/Topic1 - /Local1/DROAN1/Droan GPU1 - /Local1/DROAN1/Droan GPU1/Traj Debug1/Namespaces1 - - /Global1 + - /Local1/DROAN1/Global Plan Vis1 - /Global1/Global Plan1 - Splitter Ratio: 0.3440559506416321 - Tree Height: 480 + Splitter Ratio: 0.5792563557624817 + Tree Height: 693 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties - Expanded: - - /2D Goal Pose1 - - /Publish Point1 + Expanded: ~ Name: Tool Properties Splitter Ratio: 0.5886790156364441 - Class: rviz_common/Views @@ -33,11 +30,23 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: StereoImageProc Disparity + SyncSource: Foreground Background Cloud - Class: rviz_behavior_tree_panel::BehaviorTreePanel Name: BehaviorTreePanel - topic: behavior/behavior_tree_graphviz + topic: /robot_1/behavior/behavior_tree_graphviz zoom_factor: 0.1919851303100586 + - Class: rviz_tasks_panel::TasksPanel + Name: TasksPanel + executor_0: /robot_1/tasks/takeoff + executor_1: /robot_1/tasks/land + executor_2: /robot_1/tasks/navigate + executor_3: /robot_1/tasks/exploration + executor_4: "" + executor_5: "" + executor_6: "" + executor_7: "" + executor_8: /robot_1/tasks/fixed_trajectory + robot: /robot_1 Visualization Manager: Class: "" Displays: @@ -77,7 +86,7 @@ Visualization Manager: base_link_frd: Value: true base_link_stabilized: - Value: false + Value: true camera_left: Value: true camera_right: @@ -87,11 +96,9 @@ Visualization Manager: lidar: Value: false look_ahead_point: - Value: false + Value: true look_ahead_point_stabilized: - Value: false - macvo_ned: - Value: false + Value: true map: Value: true map_ned: @@ -109,9 +116,9 @@ Visualization Manager: rotor3: Value: true tracking_point: - Value: false + Value: true tracking_point_stabilized: - Value: false + Value: true world: Value: false Marker Scale: 0.30000001192092896 @@ -129,8 +136,7 @@ Visualization Manager: {} base_link_ZED_X: camera_left: - macvo_ned: - {} + {} camera_right: {} imu: @@ -476,7 +482,8 @@ Visualization Manager: Enabled: true Name: Trimmed Global Plan for DROAN Namespaces: - {} + global_plan: true + global_plan_extra_text: true Topic: Depth: 5 Durability Policy: Volatile @@ -641,7 +648,7 @@ Visualization Manager: Reliability Policy: Reliable Value: droan/graph_vis Value: true - Enabled: true + Enabled: false Name: Droan GPU - Class: rviz_default_plugins/MarkerArray Enabled: true @@ -659,7 +666,8 @@ Visualization Manager: Enabled: true Name: Global Plan Vis Namespaces: - {} + global_plan: true + global_plan_extra_text: true Topic: Depth: 5 Durability Policy: Volatile @@ -671,7 +679,7 @@ Visualization Manager: Enabled: true Name: Rewind Info Namespaces: - {} + rewind_info: true Topic: Depth: 5 Durability Policy: Volatile @@ -710,6 +718,15 @@ Visualization Manager: Value: false Enabled: true Name: Trajectory Controller + - Class: rviz_default_plugins/InteractiveMarkers + Enable Transparency: true + Enabled: true + Interactive Markers Namespace: /robot_1/waypoint_plugin + Name: Waypoints InteractiveMarkers + Show Axes: true + Show Descriptions: true + Show Visual Aids: false + Value: true Enabled: true Name: Local - Class: rviz_common/Group @@ -757,20 +774,6 @@ Visualization Manager: Value: true Enabled: true Name: Global - - Class: rviz_default_plugins/Image - Enabled: true - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: Image - Normalize Range: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: sensors/front_stereo/left/image_rect - Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -810,6 +813,23 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: /clicked_point + - Class: rviz_polygon_selection_tool/PolygonSelectionTool + Close loop: true + Lasso mode: true + Line Color: 255; 0; 0 + Patch size: 15 + Point Color: 255; 0; 0 + Point Generation Gap: 0.0020000000949949026 + Point Size: 50 + Render as Overlay: false + Service name: get_selection + Show Text: true + Text Size: 0.014999999664723873 + - Class: waypoint_rviz2_plugin/WaypointTool + WaypointTool: + default_height: 10 + frame_id: map + topic: waypoints Transformation: Current: Class: rviz_default_plugins/TF @@ -817,25 +837,25 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 26.301408767700195 + Distance: 37.564666748046875 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 6.924289703369141 - Y: -7.847400188446045 - Z: -4.3323540687561035 + X: 25.773038864135742 + Y: -5.22532320022583 + Z: -18.542400360107422 Focal Shape Fixed Size: false Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.5103968977928162 + Pitch: 0.6403977274894714 Target Frame: base_link Value: Orbit (rviz) - Yaw: 2.279876232147217 + Yaw: 2.878566026687622 Saved: ~ Window Geometry: BehaviorTreePanel: @@ -849,13 +869,13 @@ Window Geometry: Height: 1376 Hide Left Dock: false Hide Right Dock: false - Image: - collapsed: false MACVO Disparity: collapsed: false - QMainWindow State: 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 + QMainWindow State: 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 Selection: collapsed: false + TasksPanel: + collapsed: false Time: collapsed: false Tool Properties: diff --git a/common/ros_packages/gui/rqt_behavior_tree/resource/rqt_behavior_tree b/common/ros_packages/gui/foxglove/.gitkeep similarity index 100% rename from common/ros_packages/gui/rqt_behavior_tree/resource/rqt_behavior_tree rename to common/ros_packages/gui/foxglove/.gitkeep diff --git a/common/ros_packages/gui/rqt_behavior_tree/.gitignore b/common/ros_packages/gui/rqt/rqt_behavior_tree/.gitignore similarity index 100% rename from common/ros_packages/gui/rqt_behavior_tree/.gitignore rename to common/ros_packages/gui/rqt/rqt_behavior_tree/.gitignore diff --git a/common/ros_packages/gui/rqt_behavior_tree/CHANGELOG.rst b/common/ros_packages/gui/rqt/rqt_behavior_tree/CHANGELOG.rst similarity index 100% rename from common/ros_packages/gui/rqt_behavior_tree/CHANGELOG.rst rename to common/ros_packages/gui/rqt/rqt_behavior_tree/CHANGELOG.rst diff --git a/common/ros_packages/gui/rqt_behavior_tree/package.xml b/common/ros_packages/gui/rqt/rqt_behavior_tree/package.xml similarity index 100% rename from common/ros_packages/gui/rqt_behavior_tree/package.xml rename to common/ros_packages/gui/rqt/rqt_behavior_tree/package.xml diff --git a/common/ros_packages/gui/rqt_behavior_tree/plugin.xml b/common/ros_packages/gui/rqt/rqt_behavior_tree/plugin.xml similarity index 100% rename from common/ros_packages/gui/rqt_behavior_tree/plugin.xml rename to common/ros_packages/gui/rqt/rqt_behavior_tree/plugin.xml diff --git a/common/ros_packages/gui/rqt_behavior_tree/resource/py_console_widget.ui b/common/ros_packages/gui/rqt/rqt_behavior_tree/resource/py_console_widget.ui similarity index 100% rename from common/ros_packages/gui/rqt_behavior_tree/resource/py_console_widget.ui rename to common/ros_packages/gui/rqt/rqt_behavior_tree/resource/py_console_widget.ui diff --git a/common/ros_packages/gui/rqt_behavior_tree/src/rqt_behavior_tree/__init__.py b/common/ros_packages/gui/rqt/rqt_behavior_tree/resource/rqt_behavior_tree similarity index 100% rename from common/ros_packages/gui/rqt_behavior_tree/src/rqt_behavior_tree/__init__.py rename to common/ros_packages/gui/rqt/rqt_behavior_tree/resource/rqt_behavior_tree diff --git a/common/ros_packages/gui/rqt_behavior_tree/setup.cfg b/common/ros_packages/gui/rqt/rqt_behavior_tree/setup.cfg similarity index 100% rename from common/ros_packages/gui/rqt_behavior_tree/setup.cfg rename to common/ros_packages/gui/rqt/rqt_behavior_tree/setup.cfg diff --git a/common/ros_packages/gui/rqt_behavior_tree/setup.py b/common/ros_packages/gui/rqt/rqt_behavior_tree/setup.py similarity index 100% rename from common/ros_packages/gui/rqt_behavior_tree/setup.py rename to common/ros_packages/gui/rqt/rqt_behavior_tree/setup.py diff --git a/common/ros_packages/gui/rqt_behavior_tree/src/rqt_behavior_tree/xdot/__init__.py b/common/ros_packages/gui/rqt/rqt_behavior_tree/src/rqt_behavior_tree/__init__.py similarity index 100% rename from common/ros_packages/gui/rqt_behavior_tree/src/rqt_behavior_tree/xdot/__init__.py rename to common/ros_packages/gui/rqt/rqt_behavior_tree/src/rqt_behavior_tree/__init__.py diff --git a/common/ros_packages/gui/rqt_behavior_tree/src/rqt_behavior_tree/main.py b/common/ros_packages/gui/rqt/rqt_behavior_tree/src/rqt_behavior_tree/main.py similarity index 100% rename from common/ros_packages/gui/rqt_behavior_tree/src/rqt_behavior_tree/main.py rename to common/ros_packages/gui/rqt/rqt_behavior_tree/src/rqt_behavior_tree/main.py diff --git a/common/ros_packages/gui/rqt_behavior_tree/src/rqt_behavior_tree/py_console.py b/common/ros_packages/gui/rqt/rqt_behavior_tree/src/rqt_behavior_tree/py_console.py similarity index 100% rename from common/ros_packages/gui/rqt_behavior_tree/src/rqt_behavior_tree/py_console.py rename to common/ros_packages/gui/rqt/rqt_behavior_tree/src/rqt_behavior_tree/py_console.py diff --git a/common/ros_packages/gui/rqt_behavior_tree/src/rqt_behavior_tree/py_console_text_edit.py b/common/ros_packages/gui/rqt/rqt_behavior_tree/src/rqt_behavior_tree/py_console_text_edit.py similarity index 100% rename from common/ros_packages/gui/rqt_behavior_tree/src/rqt_behavior_tree/py_console_text_edit.py rename to common/ros_packages/gui/rqt/rqt_behavior_tree/src/rqt_behavior_tree/py_console_text_edit.py diff --git a/common/ros_packages/gui/rqt_behavior_tree/src/rqt_behavior_tree/py_console_widget.py b/common/ros_packages/gui/rqt/rqt_behavior_tree/src/rqt_behavior_tree/py_console_widget.py similarity index 100% rename from common/ros_packages/gui/rqt_behavior_tree/src/rqt_behavior_tree/py_console_widget.py rename to common/ros_packages/gui/rqt/rqt_behavior_tree/src/rqt_behavior_tree/py_console_widget.py diff --git a/common/ros_packages/gui/rqt_behavior_tree/src/rqt_behavior_tree/spyder_console_widget.py b/common/ros_packages/gui/rqt/rqt_behavior_tree/src/rqt_behavior_tree/spyder_console_widget.py similarity index 100% rename from common/ros_packages/gui/rqt_behavior_tree/src/rqt_behavior_tree/spyder_console_widget.py rename to common/ros_packages/gui/rqt/rqt_behavior_tree/src/rqt_behavior_tree/spyder_console_widget.py diff --git a/common/ros_packages/gui/rqt_behavior_tree/src/rqt_behavior_tree/test.py b/common/ros_packages/gui/rqt/rqt_behavior_tree/src/rqt_behavior_tree/test.py similarity index 100% rename from common/ros_packages/gui/rqt_behavior_tree/src/rqt_behavior_tree/test.py rename to common/ros_packages/gui/rqt/rqt_behavior_tree/src/rqt_behavior_tree/test.py diff --git a/common/ros_packages/gui/rqt_py_template/src/rqt_py_template/__init__.py b/common/ros_packages/gui/rqt/rqt_behavior_tree/src/rqt_behavior_tree/xdot/__init__.py similarity index 100% rename from common/ros_packages/gui/rqt_py_template/src/rqt_py_template/__init__.py rename to common/ros_packages/gui/rqt/rqt_behavior_tree/src/rqt_behavior_tree/xdot/__init__.py diff --git a/common/ros_packages/gui/rqt_behavior_tree/src/rqt_behavior_tree/xdot/wxxdot.py b/common/ros_packages/gui/rqt/rqt_behavior_tree/src/rqt_behavior_tree/xdot/wxxdot.py similarity index 100% rename from common/ros_packages/gui/rqt_behavior_tree/src/rqt_behavior_tree/xdot/wxxdot.py rename to common/ros_packages/gui/rqt/rqt_behavior_tree/src/rqt_behavior_tree/xdot/wxxdot.py diff --git a/common/ros_packages/gui/rqt_behavior_tree/src/rqt_behavior_tree/xdot/xdot.py b/common/ros_packages/gui/rqt/rqt_behavior_tree/src/rqt_behavior_tree/xdot/xdot.py similarity index 100% rename from common/ros_packages/gui/rqt_behavior_tree/src/rqt_behavior_tree/xdot/xdot.py rename to common/ros_packages/gui/rqt/rqt_behavior_tree/src/rqt_behavior_tree/xdot/xdot.py diff --git a/common/ros_packages/gui/rqt_behavior_tree/src/rqt_behavior_tree/xdot/xdot_qt.py b/common/ros_packages/gui/rqt/rqt_behavior_tree/src/rqt_behavior_tree/xdot/xdot_qt.py similarity index 100% rename from common/ros_packages/gui/rqt_behavior_tree/src/rqt_behavior_tree/xdot/xdot_qt.py rename to common/ros_packages/gui/rqt/rqt_behavior_tree/src/rqt_behavior_tree/xdot/xdot_qt.py diff --git a/common/ros_packages/gui/rqt_py_template/CHANGELOG.rst b/common/ros_packages/gui/rqt/rqt_py_template/CHANGELOG.rst similarity index 100% rename from common/ros_packages/gui/rqt_py_template/CHANGELOG.rst rename to common/ros_packages/gui/rqt/rqt_py_template/CHANGELOG.rst diff --git a/common/ros_packages/gui/rqt_py_template/README.md b/common/ros_packages/gui/rqt/rqt_py_template/README.md similarity index 100% rename from common/ros_packages/gui/rqt_py_template/README.md rename to common/ros_packages/gui/rqt/rqt_py_template/README.md diff --git a/common/ros_packages/gui/rqt_py_template/generate_rqt_py_package.sh b/common/ros_packages/gui/rqt/rqt_py_template/generate_rqt_py_package.sh similarity index 100% rename from common/ros_packages/gui/rqt_py_template/generate_rqt_py_package.sh rename to common/ros_packages/gui/rqt/rqt_py_template/generate_rqt_py_package.sh diff --git a/common/ros_packages/gui/rqt_py_template/package.xml b/common/ros_packages/gui/rqt/rqt_py_template/package.xml similarity index 100% rename from common/ros_packages/gui/rqt_py_template/package.xml rename to common/ros_packages/gui/rqt/rqt_py_template/package.xml diff --git a/common/ros_packages/gui/rqt_py_template/plugin.xml b/common/ros_packages/gui/rqt/rqt_py_template/plugin.xml similarity index 100% rename from common/ros_packages/gui/rqt_py_template/plugin.xml rename to common/ros_packages/gui/rqt/rqt_py_template/plugin.xml diff --git a/common/ros_packages/gui/rqt_py_template/resource/py_console_widget.ui b/common/ros_packages/gui/rqt/rqt_py_template/resource/py_console_widget.ui similarity index 100% rename from common/ros_packages/gui/rqt_py_template/resource/py_console_widget.ui rename to common/ros_packages/gui/rqt/rqt_py_template/resource/py_console_widget.ui diff --git a/common/ros_packages/gui/rqt_py_template/resource/rqt_py_template b/common/ros_packages/gui/rqt/rqt_py_template/resource/rqt_py_template similarity index 100% rename from common/ros_packages/gui/rqt_py_template/resource/rqt_py_template rename to common/ros_packages/gui/rqt/rqt_py_template/resource/rqt_py_template diff --git a/common/ros_packages/gui/rqt_py_template/setup.cfg b/common/ros_packages/gui/rqt/rqt_py_template/setup.cfg similarity index 100% rename from common/ros_packages/gui/rqt_py_template/setup.cfg rename to common/ros_packages/gui/rqt/rqt_py_template/setup.cfg diff --git a/common/ros_packages/gui/rqt_py_template/setup.py b/common/ros_packages/gui/rqt/rqt_py_template/setup.py similarity index 100% rename from common/ros_packages/gui/rqt_py_template/setup.py rename to common/ros_packages/gui/rqt/rqt_py_template/setup.py diff --git a/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/src/rqt_fixed_trajectory_generator/__init__.py b/common/ros_packages/gui/rqt/rqt_py_template/src/rqt_py_template/__init__.py similarity index 100% rename from robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/src/rqt_fixed_trajectory_generator/__init__.py rename to common/ros_packages/gui/rqt/rqt_py_template/src/rqt_py_template/__init__.py diff --git a/common/ros_packages/gui/rqt_py_template/src/rqt_py_template/main.py b/common/ros_packages/gui/rqt/rqt_py_template/src/rqt_py_template/main.py similarity index 100% rename from common/ros_packages/gui/rqt_py_template/src/rqt_py_template/main.py rename to common/ros_packages/gui/rqt/rqt_py_template/src/rqt_py_template/main.py diff --git a/common/ros_packages/gui/rqt_py_template/src/rqt_py_template/py_console_text_edit.py b/common/ros_packages/gui/rqt/rqt_py_template/src/rqt_py_template/py_console_text_edit.py similarity index 100% rename from common/ros_packages/gui/rqt_py_template/src/rqt_py_template/py_console_text_edit.py rename to common/ros_packages/gui/rqt/rqt_py_template/src/rqt_py_template/py_console_text_edit.py diff --git a/common/ros_packages/gui/rqt_py_template/src/rqt_py_template/py_console_widget.py b/common/ros_packages/gui/rqt/rqt_py_template/src/rqt_py_template/py_console_widget.py similarity index 100% rename from common/ros_packages/gui/rqt_py_template/src/rqt_py_template/py_console_widget.py rename to common/ros_packages/gui/rqt/rqt_py_template/src/rqt_py_template/py_console_widget.py diff --git a/common/ros_packages/gui/rqt_py_template/src/rqt_py_template/spyder_console_widget.py b/common/ros_packages/gui/rqt/rqt_py_template/src/rqt_py_template/spyder_console_widget.py similarity index 100% rename from common/ros_packages/gui/rqt_py_template/src/rqt_py_template/spyder_console_widget.py rename to common/ros_packages/gui/rqt/rqt_py_template/src/rqt_py_template/spyder_console_widget.py diff --git a/common/ros_packages/gui/rqt_py_template/src/rqt_py_template/template.py b/common/ros_packages/gui/rqt/rqt_py_template/src/rqt_py_template/template.py similarity index 100% rename from common/ros_packages/gui/rqt_py_template/src/rqt_py_template/template.py rename to common/ros_packages/gui/rqt/rqt_py_template/src/rqt_py_template/template.py diff --git a/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/.gitignore b/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/.gitignore new file mode 100644 index 000000000..88cd635a1 --- /dev/null +++ b/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/.gitignore @@ -0,0 +1,39 @@ +# Build artifacts +build/ +install/ +log/ + +# IDE files +.vscode/ +.idea/ +*.swp +*.swo +*~ + +# Qt generated files +*.autosave +moc_*.cpp +moc_*.h +ui_*.h + +# Bag files +*.bag +*.db3 +*.db3-shm +*.db3-wal + +# CMake +CMakeCache.txt +CMakeFiles/ +cmake_install.cmake +Makefile + +# Python +__pycache__/ +*.pyc +*.pyo +*.pyd + +# OS files +.DS_Store +Thumbs.db diff --git a/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/CMakeLists.txt b/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/CMakeLists.txt new file mode 100644 index 000000000..c19c09c71 --- /dev/null +++ b/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/CMakeLists.txt @@ -0,0 +1,119 @@ +cmake_minimum_required(VERSION 3.5) +project(waypoint_rviz2_plugin) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# Find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rviz_common REQUIRED) +find_package(rviz_default_plugins REQUIRED) +find_package(rviz_rendering REQUIRED) +find_package(rviz_ogre_vendor REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(interactive_markers REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(rosbag2_cpp REQUIRED) +find_package(pluginlib REQUIRED) + +# Qt5 +find_package(Qt5 REQUIRED COMPONENTS Core Widgets) +set(QT_LIBRARIES Qt5::Widgets) + +# Qt signals and slots to avoid defining "emit", "slots" +add_definitions(-DQT_NO_KEYWORDS) + +# Header files that need Qt Moc pre-processing +qt5_wrap_cpp(MOC_FILES + include/waypoint_rviz2_plugin/waypoint_manager.hpp + include/waypoint_rviz2_plugin/waypoint_tool.hpp +) + +# Plugin source files +set(SOURCES + src/waypoint_manager.cpp + src/waypoint_tool.cpp + ${MOC_FILES} +) + +# Declare the library +add_library(${PROJECT_NAME} SHARED + ${SOURCES} +) + +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ + $ +) + +# Dependencies +set(DEPENDENCIES + rclcpp + rviz_common + rviz_default_plugins + rviz_rendering + rviz_ogre_vendor + geometry_msgs + visualization_msgs + interactive_markers + nav_msgs + tf2 + tf2_geometry_msgs + rosbag2_cpp + pluginlib +) + +ament_target_dependencies(${PROJECT_NAME} + ${DEPENDENCIES} +) + +target_link_libraries(${PROJECT_NAME} + ${QT_LIBRARIES} +) + +# Prevent pluginlib from using boost +target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") + +# Export plugin description file for pluginlib +pluginlib_export_plugin_description_file(rviz_common plugin_description.xml) + +# Install +install(TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(DIRECTORY include/ + DESTINATION include/ +) + +install(FILES plugin_description.xml + DESTINATION share/${PROJECT_NAME} +) + +install(DIRECTORY media/ + DESTINATION share/${PROJECT_NAME}/media +) + +install(DIRECTORY launch/ + DESTINATION share/${PROJECT_NAME}/launch +) + +# Export +ament_export_include_directories(include) +ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies(${DEPENDENCIES}) + +ament_package() diff --git a/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/README.md b/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/README.md new file mode 100644 index 000000000..57ecd587b --- /dev/null +++ b/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/README.md @@ -0,0 +1,142 @@ +# 3D Waypoint RViz2 Plugin + +An interactive RViz2 tool plugin for creating, manipulating, and managing 3D waypoints directly in the RViz2 viewport. Waypoint state is managed by a shared `WaypointManager` singleton so that both this tool (3D interaction) and the Tasks Panel's Navigate tab (UI controls) operate on the same data. + +![Waypoint Tool Demo](rviz_task_panel_waypoints.png) + +## Architecture + +``` + WaypointTool (rviz_common::Tool) Tasks Panel Navigate tab + mouse click in 3D viewport UI: height, X/Y/Z/Yaw, + | Clear, Save, Load, Execute + v | + +------------------------------------------+ + | WaypointManager (singleton) | + | | + | - InteractiveMarkerServer | + | - Ogre SceneNode map (waypoint visuals) | + | - add / remove / clear / reorder | + | - save / load (.db3 bag files) | + | - getPath() -> nav_msgs/Path | + | - Qt signals for UI updates | + +------------------------------------------+ +``` + +- **WaypointTool** (`rviz_common::Tool`) -- handles mouse events in the 3D viewport. Left-click to add, right-click near an existing waypoint to delete. Delegates all state to `WaypointManager`. +- **WaypointManager** (`QObject` singleton) -- owns all waypoint state: the interactive marker server, the Ogre scene nodes, persistence, and publishing. Emits Qt signals (`waypointCountChanged`, `selectedMarkerChanged`, `waypointsCleared`) so external UI can stay in sync. +- The **Tasks Panel** (separate `rviz_tasks_panel` package) holds a `shared_ptr` to the same `WaypointManager` instance and provides the full control UI in its Navigate tab. + +## Features + +- **Interactive Waypoint Creation**: Click to add waypoints at any position in the 3D environment +- **3D Manipulation**: Drag and move waypoints using interactive markers (MOVE_ROTATE, MOVE_AXIS) +- **Orientation Control**: Set and adjust waypoint yaw via interactive markers or Navigate tab spinboxes +- **Visual Feedback**: Real-time axis indicator meshes at each waypoint +- **Waypoint Management**: + - Add waypoints with left-click + - Delete waypoints with right-click or context menu + - "Set manual" context menu selects the waypoint for editing in the Navigate tab + - Automatic reordering after deletion (no gaps in numbering) + - Clear all waypoints at once +- **Data Persistence**: Save and load waypoint configurations using ROS 2 bag files (.db3) +- **Publishing**: Publish waypoints as `nav_msgs/msg/Path` messages +- **Configurable**: Default height, frame ID, and topic name (persisted in RViz config) + +## Requirements + +- ROS 2 Jazzy (or Humble and above) +- RViz2 +- Qt5 +- C++17 + +## Dependencies + +- rclcpp +- rviz_common +- rviz_default_plugins +- rviz_rendering +- rviz_ogre_vendor +- geometry_msgs +- visualization_msgs +- interactive_markers +- nav_msgs +- tf2 +- tf2_geometry_msgs +- rosbag2_cpp +- pluginlib + +## Building + +```bash +colcon build --packages-select waypoint_rviz2_plugin +``` + +## Usage + +### Adding the Tool to RViz2 + +1. Open RViz2 +2. In the toolbar, click the "+" icon to add a new tool +3. Select **waypoint_rviz2_plugin / WaypointTool** from the list +4. The tool appears in the toolbar with keyboard shortcut **1** + +### Placing Waypoints + +1. **Select the Tool**: Click the Waypoint Tool icon or press **1** +2. **Add Waypoints**: Left-click anywhere in the 3D view +3. **Move Waypoints**: Add the interactive marker display (Add > By topic > `/waypoint_plugin/update/InteractiveMarkers`), then drag markers to reposition +4. **Delete Waypoints**: Right-click near a waypoint, or use the context menu "delete" +5. **Select for Editing**: Right-click a marker and choose "set manual" to select it in the Navigate tab + +### Navigate Tab Controls (in Tasks Panel) + +All waypoint management UI lives in the Tasks Panel's **Navigate** tab: + +- **Default Height** -- Z-height for newly placed waypoints +- **X / Y / Z / Yaw spinboxes** -- edit the currently selected waypoint's pose +- **Clear All** -- remove all waypoints +- **Save / Load** -- persist waypoints to/from `.db3` bag files +- **Waypoint count** and **selected waypoint** labels update in real time + +### Listening to Published Waypoints + +```bash +ros2 topic echo /waypoints +``` + +## File Structure + +``` +waypoint_rviz2_plugin/ +├── CMakeLists.txt +├── package.xml +├── plugin_description.xml +├── README.md +├── include/ +│ └── waypoint_rviz2_plugin/ +│ ├── waypoint_manager.hpp # Shared singleton (state + operations) +│ └── waypoint_tool.hpp # RViz Tool (mouse events only) +├── src/ +│ ├── waypoint_manager.cpp +│ └── waypoint_tool.cpp +├── media/ +│ └── axis.dae # 3D axis indicator mesh +└── launch/ + └── rviz2.launch.py +``` + +## License + +Apache 2.0 + +## Credits + +Original ROS 1 version by KoKoLates (the21515@gmail.com). + +ROS 2 port and WaypointManager refactor for AirStack (Andrew Jong ajong@andrew.cmu.edu). + +## References + +- [RViz2 Plugin Tutorial](https://docs.ros.org/en/jazzy/Tutorials/Advanced/RViz-Plugins.html) +- [Interactive Markers](https://docs.ros.org/en/jazzy/Tutorials/Advanced/RViz/Interactive-Markers.html) diff --git a/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/include/waypoint_rviz2_plugin/waypoint_manager.hpp b/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/include/waypoint_rviz2_plugin/waypoint_manager.hpp new file mode 100644 index 000000000..9001d1592 --- /dev/null +++ b/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/include/waypoint_rviz2_plugin/waypoint_manager.hpp @@ -0,0 +1,128 @@ +/** + * @file waypoint_manager.hpp + * + * Singleton that owns all waypoint state (interactive markers, Ogre scene + * nodes, persistence). Both WaypointTool (3-D mouse interaction) and the + * Tasks Panel Navigate tab (UI controls) hold a shared_ptr to the same + * instance so they share a single source of truth. + */ + +#pragma once + +#ifndef Q_MOC_RUN +#include +#include +#include +#include + +#include +#include +#include +#include +#endif + +#include +#include +#include +#include +#include +#include + +namespace rviz_common { +class DisplayContext; +} + +namespace waypoint_rviz2_plugin { + +constexpr char kWaypointPrefix[] = "point#"; + +class WaypointManager : public QObject +{ + Q_OBJECT + +public: + // ── singleton ────────────────────────────────────────────────────────────── + static std::shared_ptr instance(); + + ~WaypointManager() override; + + /** Called once by WaypointTool::onInitialize() which has the DisplayContext. */ + void initialize(rviz_common::DisplayContext * context, + rclcpp::Node::SharedPtr node); + bool isInitialized() const { return initialized_; } + + // ── waypoint operations ──────────────────────────────────────────────────── + void addWaypoint(const Ogre::Vector3 & position, const Ogre::Quaternion & quaternion); + void removeWaypoint(int idx); + void clearAll(); + + /** Build a nav_msgs/Path from current waypoint state. */ + nav_msgs::msg::Path getPath() const; + + /** Publish current waypoints on the configured topic. */ + void publishPath(); + + void saveBag(const std::string & filename); + void loadBag(const std::string & filename); + + /** Update pose of the currently-selected marker from spinbox values. */ + void updateSelectedPose(double x, double y, double z, double yaw); + + // ── accessors / mutators ─────────────────────────────────────────────────── + std::string frameId() const; + void setFrameId(const std::string & frame); + + std::string outputTopic() const; + void setOutputTopic(const std::string & topic); + + double defaultHeight() const; + void setDefaultHeight(double h); + + int waypointCount() const; + + /** Read-only access to the node map (used by WaypointTool for proximity). */ + const std::map & nodeMap() const { return node_map_; } + + /** Expose the interactive marker server (needed by WaypointTool for menu). */ + std::shared_ptr server() const { return server_; } + + interactive_markers::MenuHandler & menuHandler() { return menu_handler_; } + +Q_SIGNALS: + void waypointCountChanged(int count); + void selectedMarkerChanged(const QString & name, double x, double y, double z, double yaw); + void waypointsCleared(); + +private: + WaypointManager(); // private – use instance() + + void processFeedback( + const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr & feedback); + + /** Rebuild node_map_ with sequential 1-based keys and rename interactive + * markers so there are no gaps after a deletion. */ + void reorderWaypoints(); + + bool initialized_{false}; + rclcpp::Node::SharedPtr node_; + rviz_common::DisplayContext * context_{nullptr}; + Ogre::SceneManager * scene_manager_{nullptr}; + + std::shared_ptr server_; + interactive_markers::MenuHandler menu_handler_; + + std::map node_map_; + int unique_idx_{0}; + + std::string frame_id_{"map"}; + std::string output_topic_{"/waypoints"}; + double default_height_{0.0}; + std::string selected_marker_name_; + + std::string axis_resource_; + rclcpp::Publisher::SharedPtr path_publisher_; + + mutable std::mutex mtx_; +}; + +} // namespace waypoint_rviz2_plugin diff --git a/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/include/waypoint_rviz2_plugin/waypoint_tool.hpp b/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/include/waypoint_rviz2_plugin/waypoint_tool.hpp new file mode 100644 index 000000000..b599df90f --- /dev/null +++ b/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/include/waypoint_rviz2_plugin/waypoint_tool.hpp @@ -0,0 +1,52 @@ +/** + * @file waypoint_tool.hpp + * @author KoKoLates (the21515@gmail.com) + * @version 2.0.0 + * + * RViz2 Tool for placing/removing waypoints via mouse clicks in the 3-D + * viewport. All state is owned by WaypointManager; this class only converts + * mouse events into manager calls. + */ + +#pragma once + +#ifndef Q_MOC_RUN +#include +#include +#endif + +#include +#include +#include + +#include "waypoint_manager.hpp" + +namespace rviz_common { +class DisplayContext; +class ViewportMouseEvent; +} + +namespace waypoint_rviz2_plugin { + +class WaypointTool : public rviz_common::Tool +{ + Q_OBJECT +public: + WaypointTool(); + ~WaypointTool() override; + + void onInitialize() override; + void activate() override; + void deactivate() override; + int processMouseEvent(rviz_common::ViewportMouseEvent & event) override; + + void load(const rviz_common::Config & config) override; + void save(rviz_common::Config config) const override; + +private: + Ogre::SceneNode * move_axis_node_{nullptr}; + std::shared_ptr manager_; + rclcpp::Node::SharedPtr node_; +}; + +} // namespace waypoint_rviz2_plugin diff --git a/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/include/waypoint_rviz2_plugin/waypoint_widget.hpp b/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/include/waypoint_rviz2_plugin/waypoint_widget.hpp new file mode 100644 index 000000000..89544e07d --- /dev/null +++ b/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/include/waypoint_rviz2_plugin/waypoint_widget.hpp @@ -0,0 +1,108 @@ +/** + * @file waypoint_widget.hpp + * @author KoKoLates (the21515@gmail.com) + * @version 1.0.0 + * @date 2023-12-10 + * + * ROS2 port of the interactive waypoint widget for RViz2 + */ + +#pragma once + +#ifndef Q_MOC_RUN +#include +#include +#include +#include +#include +#endif + +#include +#include +#include + +#include "ui_waypoint_plugin.h" + +namespace rviz_common { + class DisplayContext; +} + +namespace interactive_markers { + class InteractiveMarkerServer; +} + +namespace Ui { + class PluginWidget; +} + +namespace waypoint_rviz2_plugin { + class WaypointTool; +} + +namespace waypoint_rviz2_plugin { + constexpr char waypoint_name_prefix[] = "point#"; + + class WaypointWidget: public QWidget { + friend class WaypointTool; + Q_OBJECT + + public: + WaypointWidget(rviz_common::DisplayContext *context, + rclcpp::Node::SharedPtr node, + std::map *map_ptr, + std::shared_ptr server, + int *unique_idx, + QWidget *parent=nullptr, + WaypointTool *waypoint_tool=nullptr); + ~WaypointWidget(); + + void enable(); + void disable(); + + void setPose(const Ogre::Vector3&, const Ogre::Quaternion&); + void setConfig(QString topic, QString frame, float height); + void setWaypointLabel(Ogre::Vector3 position); + void setWaypointCount(int size); + void setSelectedMarkerName(std::string name); + + void getPose(Ogre::Vector3&, Ogre::Quaternion&); + QString getFrameId(); + QString getOutputTopic(); + double getDefaultHeight(); + + void saveBag(const std::string& filename); + void loadBag(const std::string& filename); + + protected: + Ui::PluginWidget *ui_; + rviz_common::DisplayContext *context_; + + private: + rclcpp::Node::SharedPtr node_; + rclcpp::Publisher::SharedPtr path_publisher_; + + WaypointTool *waypoint_tool_; + std::map *map_ptr_; + Ogre::SceneManager *scene_manager_; + std::shared_ptr server_; + + int* unique_idx_; + double default_height_; + + QString output_topic_; + QString frame_id_; + + std::mutex frame_updates_mutex_; + std::string selected_marker_name_; + + private Q_SLOTS: + void saveHandler(); + void loadHandler(); + void clearHandler(); + void publishHandler(); + void frameChange(); + void topicChange(); + void poseChange(double value); + void heightChange(double height); + }; +} diff --git a/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/launch/rviz2.launch.py b/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/launch/rviz2.launch.py new file mode 100644 index 000000000..39284886d --- /dev/null +++ b/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/launch/rviz2.launch.py @@ -0,0 +1,28 @@ +""" +RViz2 launch file for waypoint plugin demonstration +""" + +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +import os + + +def generate_launch_description(): + pkg_share = get_package_share_directory('waypoint_rviz2_plugin') + + rviz_config_file = os.path.join(pkg_share, 'config', 'waypoint_demo.rviz') + + use_rviz_config = os.path.exists(rviz_config_file) + + rviz_node = Node( + package='rviz2', + executable='rviz2', + name='rviz2', + arguments=['-d', rviz_config_file] if use_rviz_config else [], + output='screen' + ) + + return LaunchDescription([ + rviz_node + ]) diff --git a/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/media/axis.dae b/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/media/axis.dae new file mode 100644 index 000000000..a2f7b2ddc --- /dev/null +++ b/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/media/axis.dae @@ -0,0 +1,400 @@ + + + + + Blender User + Blender 3.5.1 commit date:2023-04-24, commit time:18:11, hash:e1ccd9d4a1d3 + + 2023-06-25T21:02:12 + 2023-06-25T21:02:12 + + Z_UP + + + + + + + 39.59775 + 1.777778 + 0.1 + 100 + + + + + + 0 + 0 + 10 + + + + + + + + 39.59775 + 1.777778 + 0.1 + 100 + + + + + + 0 + 0 + 10 + + + + + + + + + 1000 1000 1000 + 1 + 0 + 0.00111109 + + + + + 0 + 0 + 1 + 1 + 1 + 1 + 1 + 0 + 0 + 0 + 1000 + 29.99998 + 75 + 0.15 + 0 + 1 + 2 + 0.04999995 + 30.002 + 1 + 2880 + 3 + 1 + 0.1 + 1 + 0.1 + 0.1 + 1 + + + + + + + 1000 1000 1000 + 1 + 0 + 0.00111109 + + + + + 0 + 0 + 1 + 1 + 1 + 1 + 1 + 0 + 0 + 0 + 1000 + 29.99998 + 75 + 0.15 + 0 + 1 + 2 + 0.04999995 + 30.002 + 1 + 2880 + 3 + 1 + 0.1 + 1 + 0.1 + 0.1 + 1 + + + + + + + + + + + 0 0 0 1 + + + 0 0 1 1 + + + 0.5 + + + 1.45 + + + + + + + + + + + 0 0 0 1 + + + 1 0 0 1 + + + 0.5 + + + 1.45 + + + + + + + + + + + 0 0 0 1 + + + 0 1 0 1 + + + 0.5 + + + 1.45 + + + + + + + + + + + + + + + + + + + + + + + 0 1 -1 0 1 1 0.1950903 0.9807853 -1 0.1950903 0.9807853 1 0.3826835 0.9238795 -1 0.3826835 0.9238795 1 0.5555703 0.8314696 -1 0.5555703 0.8314696 1 0.7071068 0.7071068 -1 0.7071068 0.7071068 1 0.8314696 0.5555703 -1 0.8314696 0.5555703 1 0.9238795 0.3826835 -1 0.9238795 0.3826835 1 0.9807853 0.1950903 -1 0.9807853 0.1950903 1 1 0 -1 1 0 1 0.9807853 -0.1950903 -1 0.9807853 -0.1950903 1 0.9238795 -0.3826835 -1 0.9238795 -0.3826835 1 0.8314696 -0.5555703 -1 0.8314696 -0.5555703 1 0.7071068 -0.7071068 -1 0.7071068 -0.7071068 1 0.5555703 -0.8314696 -1 0.5555703 -0.8314696 1 0.3826835 -0.9238795 -1 0.3826835 -0.9238795 1 0.1950903 -0.9807853 -1 0.1950903 -0.9807853 1 0 -1 -1 0 -1 1 -0.1950903 -0.9807853 -1 -0.1950903 -0.9807853 1 -0.3826835 -0.9238795 -1 -0.3826835 -0.9238795 1 -0.5555703 -0.8314696 -1 -0.5555703 -0.8314696 1 -0.7071068 -0.7071068 -1 -0.7071068 -0.7071068 1 -0.8314696 -0.5555703 -1 -0.8314696 -0.5555703 1 -0.9238795 -0.3826835 -1 -0.9238795 -0.3826835 1 -0.9807853 -0.1950903 -1 -0.9807853 -0.1950903 1 -1 0 -1 -1 0 1 -0.9807853 0.1950903 -1 -0.9807853 0.1950903 1 -0.9238795 0.3826835 -1 -0.9238795 0.3826835 1 -0.8314696 0.5555703 -1 -0.8314696 0.5555703 1 -0.7071068 0.7071068 -1 -0.7071068 0.7071068 1 -0.5555703 0.8314696 -1 -0.5555703 0.8314696 1 -0.3826835 0.9238795 -1 -0.3826835 0.9238795 1 -0.1950903 0.9807853 -1 -0.1950903 0.9807853 1 + + + + + + + + + + 0.09801697 0.9951848 0 0.2902848 0.9569403 0 0.4713967 0.8819213 0 0.634393 0.7730107 0 0.7730108 0.634393 0 0.8819212 0.471397 0 0.9569405 0.2902846 0 0.9951848 0.09801727 0 0.9951848 -0.09801667 0 0.9569403 -0.2902851 0 0.8819215 -0.4713965 0 0.7730105 -0.6343934 0 0.6343933 -0.7730104 0 0.4713967 -0.8819213 0 0.2902848 -0.9569404 0 0.09801697 -0.9951848 0 -0.09801697 -0.9951848 0 -0.2902848 -0.9569403 0 -0.4713967 -0.8819213 0 -0.634393 -0.7730107 0 -0.7730108 -0.634393 0 -0.8819212 -0.471397 0 -0.9569405 -0.2902846 0 -0.9951848 -0.09801727 0 -0.9951848 0.09801667 0 -0.9569403 0.2902851 0 -0.8819215 0.4713965 0 -0.7730105 0.6343934 0 -0.6343933 0.7730104 0 -0.4713967 0.8819213 0 0 0 1 -0.2902848 0.9569404 0 -0.09801697 0.9951848 0 0 0 -1 0.2902848 0.9569404 0 0.6343933 0.7730104 0 0.7730105 0.6343934 0 0.8819215 0.4713965 0 0.9569403 0.2902851 0 0.9951848 0.09801667 0 0.9951848 -0.09801727 0 0.9569405 -0.2902846 0 0.8819212 -0.471397 0 0.7730108 -0.634393 0 0.634393 -0.7730107 0 0.2902848 -0.9569403 0 -0.2902848 -0.9569404 0 -0.6343933 -0.7730104 0 -0.7730105 -0.6343934 0 -0.8819215 -0.4713965 0 -0.9569403 -0.2902851 0 -0.9951848 -0.09801667 0 -0.9951848 0.09801727 0 -0.9569405 0.2902846 0 -0.8819212 0.471397 0 -0.7730108 0.634393 0 -0.634393 0.7730107 0 -0.2902848 0.9569403 0 3.97512e-6 0 -1 3.97512e-6 0 -1 -3.97512e-6 0 -1 -3.97512e-6 0 -1 3.88857e-7 0 -1 -3.88857e-7 0 -1 + + + + + + + + + + 1 1 0.96875 0.5 1 0.5 0.96875 1 0.9375 0.5 0.96875 0.5 0.9375 1 0.90625 0.5 0.9375 0.5 0.90625 1 0.8750001 0.5 0.90625 0.5 0.8750001 1 0.84375 0.5 0.8750001 0.5 0.84375 1 0.8125 0.5 0.84375 0.5 0.8125 1 0.78125 0.5 0.8125 0.5 0.78125 1 0.75 0.5 0.78125 0.5 0.75 1 0.71875 0.5 0.75 0.5 0.71875 1 0.6875 0.5 0.71875 0.5 0.6875 1 0.65625 0.5 0.6875 0.5 0.65625 1 0.625 0.5 0.65625 0.5 0.625 1 0.59375 0.5 0.625 0.5 0.59375 1 0.5625 0.5 0.59375 0.5 0.5625 1 0.53125 0.5 0.5625 0.5 0.53125 1 0.5 0.5 0.53125 0.5 0.5 1 0.46875 0.5 0.5 0.5 0.46875 1 0.4375 0.5 0.46875 0.5 0.4375 1 0.40625 0.5 0.4375 0.5 0.40625 1 0.375 0.5 0.40625 0.5 0.375 1 0.34375 0.5 0.375 0.5 0.34375 1 0.3125 0.5 0.34375 0.5 0.3125 1 0.28125 0.5 0.3125 0.5 0.28125 1 0.25 0.5 0.28125 0.5 0.25 1 0.21875 0.5 0.25 0.5 0.21875 1 0.1875 0.5 0.21875 0.5 0.1875 1 0.15625 0.5 0.1875 0.5 0.15625 1 0.125 0.5 0.15625 0.5 0.125 1 0.09375 0.5 0.125 0.5 0.09375 1 0.0625 0.5 0.09375 0.5 0.1581559 0.02826887 0.4717311 0.1581559 0.341844 0.4717311 0.0625 1 0.03125 0.5 0.0625 0.5 0.03125 1 0 0.5 0.03125 0.5 0.7968217 0.01461154 0.5146116 0.2031783 0.7031784 0.4853885 1 1 0.96875 1 0.96875 0.5 0.96875 1 0.9375 1 0.9375 0.5 0.9375 1 0.90625 1 0.90625 0.5 0.90625 1 0.8750001 1 0.8750001 0.5 0.8750001 1 0.84375 1 0.84375 0.5 0.84375 1 0.8125 1 0.8125 0.5 0.8125 1 0.78125 1 0.78125 0.5 0.78125 1 0.75 1 0.75 0.5 0.75 1 0.71875 1 0.71875 0.5 0.71875 1 0.6875 1 0.6875 0.5 0.6875 1 0.65625 1 0.65625 0.5 0.65625 1 0.625 1 0.625 0.5 0.625 1 0.59375 1 0.59375 0.5 0.59375 1 0.5625 1 0.5625 0.5 0.5625 1 0.53125 1 0.53125 0.5 0.53125 1 0.5 1 0.5 0.5 0.5 1 0.46875 1 0.46875 0.5 0.46875 1 0.4375 1 0.4375 0.5 0.4375 1 0.40625 1 0.40625 0.5 0.40625 1 0.375 1 0.375 0.5 0.375 1 0.34375 1 0.34375 0.5 0.34375 1 0.3125 1 0.3125 0.5 0.3125 1 0.28125 1 0.28125 0.5 0.28125 1 0.25 1 0.25 0.5 0.25 1 0.21875 1 0.21875 0.5 0.21875 1 0.1875 1 0.1875 0.5 0.1875 1 0.15625 1 0.15625 0.5 0.15625 1 0.125 1 0.125 0.5 0.125 1 0.09375 1 0.09375 0.5 0.09375 1 0.0625 1 0.0625 0.5 0.341844 0.4717311 0.2968217 0.4853885 0.25 0.49 0.25 0.49 0.2031783 0.4853885 0.341844 0.4717311 0.2031783 0.4853885 0.1581559 0.4717311 0.341844 0.4717311 0.1581559 0.4717311 0.116663 0.4495527 0.08029437 0.4197056 0.08029437 0.4197056 0.05044728 0.3833369 0.02826887 0.341844 0.02826887 0.341844 0.01461154 0.2968217 0.00999999 0.25 0.00999999 0.25 0.01461154 0.2031783 0.02826887 0.341844 0.01461154 0.2031783 0.02826887 0.1581559 0.02826887 0.341844 0.02826887 0.1581559 0.05044728 0.116663 0.08029437 0.08029437 0.08029437 0.08029437 0.116663 0.05044728 0.1581559 0.02826887 0.1581559 0.02826887 0.2031783 0.01461154 0.341844 0.02826887 0.2031783 0.01461154 0.25 0.00999999 0.341844 0.02826887 0.25 0.00999999 0.2968217 0.01461154 0.341844 0.02826887 0.341844 0.02826887 0.3833369 0.05044728 0.4197056 0.08029437 0.4197056 0.08029437 0.4495527 0.116663 0.4717311 0.1581559 0.4717311 0.1581559 0.4853885 0.2031783 0.4717311 0.341844 0.4853885 0.2031783 0.49 0.25 0.4717311 0.341844 0.49 0.25 0.4853885 0.2968217 0.4717311 0.341844 0.4717311 0.341844 0.4495527 0.3833369 0.4197056 0.4197056 0.4197056 0.4197056 0.3833369 0.4495527 0.341844 0.4717311 0.1581559 0.4717311 0.08029437 0.4197056 0.341844 0.4717311 0.08029437 0.4197056 0.02826887 0.341844 0.341844 0.4717311 0.02826887 0.1581559 0.08029437 0.08029437 0.02826887 0.341844 0.08029437 0.08029437 0.1581559 0.02826887 0.02826887 0.341844 0.341844 0.02826887 0.4197056 0.08029437 0.1581559 0.02826887 0.4197056 0.08029437 0.4717311 0.1581559 0.1581559 0.02826887 0.4717311 0.341844 0.4197056 0.4197056 0.4717311 0.1581559 0.4197056 0.4197056 0.341844 0.4717311 0.4717311 0.1581559 0.341844 0.4717311 0.02826887 0.341844 0.1581559 0.02826887 0.0625 1 0.03125 1 0.03125 0.5 0.03125 1 0 1 0 0.5 0.7031784 0.4853885 0.75 0.49 0.7968217 0.4853885 0.7968217 0.4853885 0.841844 0.4717311 0.8833369 0.4495527 0.8833369 0.4495527 0.9197056 0.4197056 0.9495527 0.3833369 0.9495527 0.3833369 0.9717311 0.341844 0.9853885 0.2968217 0.9853885 0.2968217 0.99 0.25 0.9853885 0.2031783 0.9853885 0.2031783 0.9717311 0.1581559 0.9495527 0.116663 0.9495527 0.116663 0.9197056 0.08029437 0.8833369 0.05044728 0.8833369 0.05044728 0.841844 0.02826887 0.7968217 0.01461154 0.7968217 0.01461154 0.75 0.00999999 0.7031784 0.01461154 0.7031784 0.01461154 0.658156 0.02826887 0.6166632 0.05044728 0.6166632 0.05044728 0.5802944 0.08029437 0.5504473 0.116663 0.5504473 0.116663 0.5282689 0.1581559 0.5146116 0.2031783 0.5146116 0.2031783 0.51 0.25 0.5146116 0.2968217 0.5146116 0.2968217 0.5282689 0.341844 0.5504473 0.3833369 0.5504473 0.3833369 0.5802944 0.4197056 0.6166632 0.4495527 0.6166632 0.4495527 0.658156 0.4717311 0.7031784 0.4853885 0.7031784 0.4853885 0.7968217 0.4853885 0.9853885 0.2968217 0.7968217 0.4853885 0.8833369 0.4495527 0.9853885 0.2968217 0.8833369 0.4495527 0.9495527 0.3833369 0.9853885 0.2968217 0.9853885 0.2968217 0.9853885 0.2031783 0.7968217 0.01461154 0.9853885 0.2031783 0.9495527 0.116663 0.7968217 0.01461154 0.9495527 0.116663 0.8833369 0.05044728 0.7968217 0.01461154 0.7968217 0.01461154 0.7031784 0.01461154 0.5146116 0.2031783 0.7031784 0.01461154 0.6166632 0.05044728 0.5146116 0.2031783 0.6166632 0.05044728 0.5504473 0.116663 0.5146116 0.2031783 0.5146116 0.2031783 0.5146116 0.2968217 0.7031784 0.4853885 0.5146116 0.2968217 0.5504473 0.3833369 0.7031784 0.4853885 0.5504473 0.3833369 0.6166632 0.4495527 0.7031784 0.4853885 0.7031784 0.4853885 0.9853885 0.2968217 0.7968217 0.01461154 + + + + + + + + + + + + + + +

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

+
+
+
+ + + + 0 1 -1 0 1 1 0.1950903 0.9807853 -1 0.1950903 0.9807853 1 0.3826835 0.9238795 -1 0.3826835 0.9238795 1 0.5555703 0.8314696 -1 0.5555703 0.8314696 1 0.7071068 0.7071068 -1 0.7071068 0.7071068 1 0.8314696 0.5555703 -1 0.8314696 0.5555703 1 0.9238795 0.3826835 -1 0.9238795 0.3826835 1 0.9807853 0.1950903 -1 0.9807853 0.1950903 1 1 0 -1 1 0 1 0.9807853 -0.1950903 -1 0.9807853 -0.1950903 1 0.9238795 -0.3826835 -1 0.9238795 -0.3826835 1 0.8314696 -0.5555703 -1 0.8314696 -0.5555703 1 0.7071068 -0.7071068 -1 0.7071068 -0.7071068 1 0.5555703 -0.8314696 -1 0.5555703 -0.8314696 1 0.3826835 -0.9238795 -1 0.3826835 -0.9238795 1 0.1950903 -0.9807853 -1 0.1950903 -0.9807853 1 0 -1 -1 0 -1 1 -0.1950903 -0.9807853 -1 -0.1950903 -0.9807853 1 -0.3826835 -0.9238795 -1 -0.3826835 -0.9238795 1 -0.5555703 -0.8314696 -1 -0.5555703 -0.8314696 1 -0.7071068 -0.7071068 -1 -0.7071068 -0.7071068 1 -0.8314696 -0.5555703 -1 -0.8314696 -0.5555703 1 -0.9238795 -0.3826835 -1 -0.9238795 -0.3826835 1 -0.9807853 -0.1950903 -1 -0.9807853 -0.1950903 1 -1 0 -1 -1 0 1 -0.9807853 0.1950903 -1 -0.9807853 0.1950903 1 -0.9238795 0.3826835 -1 -0.9238795 0.3826835 1 -0.8314696 0.5555703 -1 -0.8314696 0.5555703 1 -0.7071068 0.7071068 -1 -0.7071068 0.7071068 1 -0.5555703 0.8314696 -1 -0.5555703 0.8314696 1 -0.3826835 0.9238795 -1 -0.3826835 0.9238795 1 -0.1950903 0.9807853 -1 -0.1950903 0.9807853 1 + + + + + + + + + + 0.09801697 0.9951848 0 0.2902848 0.9569403 0 0.4713967 0.8819213 0 0.634393 0.7730107 0 0.7730108 0.634393 0 0.8819212 0.471397 0 0.9569405 0.2902846 0 0.9951848 0.09801727 0 0.9951848 -0.09801667 0 0.9569403 -0.2902851 0 0.8819215 -0.4713965 0 0.7730105 -0.6343934 0 0.6343933 -0.7730104 0 0.4713967 -0.8819213 0 0.2902848 -0.9569404 0 0.09801697 -0.9951848 0 -0.09801697 -0.9951848 0 -0.2902848 -0.9569403 0 -0.4713967 -0.8819213 0 -0.634393 -0.7730107 0 -0.7730108 -0.634393 0 -0.8819212 -0.471397 0 -0.9569405 -0.2902846 0 -0.9951848 -0.09801727 0 -0.9951848 0.09801667 0 -0.9569403 0.2902851 0 -0.8819215 0.4713965 0 -0.7730105 0.6343934 0 -0.6343933 0.7730104 0 -0.4713967 0.8819213 0 0 0 1 -0.2902848 0.9569404 0 -0.09801697 0.9951848 0 0 0 -1 0.2902848 0.9569404 0 0.6343933 0.7730104 0 0.7730105 0.6343934 0 0.8819215 0.4713965 0 0.9569403 0.2902851 0 0.9951848 0.09801667 0 0.9951848 -0.09801727 0 0.9569405 -0.2902846 0 0.8819212 -0.471397 0 0.7730108 -0.634393 0 0.634393 -0.7730107 0 0.2902848 -0.9569403 0 -0.2902848 -0.9569404 0 -0.6343933 -0.7730104 0 -0.7730105 -0.6343934 0 -0.8819215 -0.4713965 0 -0.9569403 -0.2902851 0 -0.9951848 -0.09801667 0 -0.9951848 0.09801727 0 -0.9569405 0.2902846 0 -0.8819212 0.471397 0 -0.7730108 0.634393 0 -0.634393 0.7730107 0 -0.2902848 0.9569403 0 3.97512e-6 0 -1 3.97512e-6 0 -1 -3.97512e-6 0 -1 -3.97512e-6 0 -1 3.88857e-7 0 -1 -3.88857e-7 0 -1 + + + + + + + + + + 1 1 0.96875 0.5 1 0.5 0.96875 1 0.9375 0.5 0.96875 0.5 0.9375 1 0.90625 0.5 0.9375 0.5 0.90625 1 0.8750001 0.5 0.90625 0.5 0.8750001 1 0.84375 0.5 0.8750001 0.5 0.84375 1 0.8125 0.5 0.84375 0.5 0.8125 1 0.78125 0.5 0.8125 0.5 0.78125 1 0.75 0.5 0.78125 0.5 0.75 1 0.71875 0.5 0.75 0.5 0.71875 1 0.6875 0.5 0.71875 0.5 0.6875 1 0.65625 0.5 0.6875 0.5 0.65625 1 0.625 0.5 0.65625 0.5 0.625 1 0.59375 0.5 0.625 0.5 0.59375 1 0.5625 0.5 0.59375 0.5 0.5625 1 0.53125 0.5 0.5625 0.5 0.53125 1 0.5 0.5 0.53125 0.5 0.5 1 0.46875 0.5 0.5 0.5 0.46875 1 0.4375 0.5 0.46875 0.5 0.4375 1 0.40625 0.5 0.4375 0.5 0.40625 1 0.375 0.5 0.40625 0.5 0.375 1 0.34375 0.5 0.375 0.5 0.34375 1 0.3125 0.5 0.34375 0.5 0.3125 1 0.28125 0.5 0.3125 0.5 0.28125 1 0.25 0.5 0.28125 0.5 0.25 1 0.21875 0.5 0.25 0.5 0.21875 1 0.1875 0.5 0.21875 0.5 0.1875 1 0.15625 0.5 0.1875 0.5 0.15625 1 0.125 0.5 0.15625 0.5 0.125 1 0.09375 0.5 0.125 0.5 0.09375 1 0.0625 0.5 0.09375 0.5 0.1581559 0.02826887 0.4717311 0.1581559 0.341844 0.4717311 0.0625 1 0.03125 0.5 0.0625 0.5 0.03125 1 0 0.5 0.03125 0.5 0.7968217 0.01461154 0.5146116 0.2031783 0.7031784 0.4853885 1 1 0.96875 1 0.96875 0.5 0.96875 1 0.9375 1 0.9375 0.5 0.9375 1 0.90625 1 0.90625 0.5 0.90625 1 0.8750001 1 0.8750001 0.5 0.8750001 1 0.84375 1 0.84375 0.5 0.84375 1 0.8125 1 0.8125 0.5 0.8125 1 0.78125 1 0.78125 0.5 0.78125 1 0.75 1 0.75 0.5 0.75 1 0.71875 1 0.71875 0.5 0.71875 1 0.6875 1 0.6875 0.5 0.6875 1 0.65625 1 0.65625 0.5 0.65625 1 0.625 1 0.625 0.5 0.625 1 0.59375 1 0.59375 0.5 0.59375 1 0.5625 1 0.5625 0.5 0.5625 1 0.53125 1 0.53125 0.5 0.53125 1 0.5 1 0.5 0.5 0.5 1 0.46875 1 0.46875 0.5 0.46875 1 0.4375 1 0.4375 0.5 0.4375 1 0.40625 1 0.40625 0.5 0.40625 1 0.375 1 0.375 0.5 0.375 1 0.34375 1 0.34375 0.5 0.34375 1 0.3125 1 0.3125 0.5 0.3125 1 0.28125 1 0.28125 0.5 0.28125 1 0.25 1 0.25 0.5 0.25 1 0.21875 1 0.21875 0.5 0.21875 1 0.1875 1 0.1875 0.5 0.1875 1 0.15625 1 0.15625 0.5 0.15625 1 0.125 1 0.125 0.5 0.125 1 0.09375 1 0.09375 0.5 0.09375 1 0.0625 1 0.0625 0.5 0.341844 0.4717311 0.2968217 0.4853885 0.25 0.49 0.25 0.49 0.2031783 0.4853885 0.341844 0.4717311 0.2031783 0.4853885 0.1581559 0.4717311 0.341844 0.4717311 0.1581559 0.4717311 0.116663 0.4495527 0.08029437 0.4197056 0.08029437 0.4197056 0.05044728 0.3833369 0.02826887 0.341844 0.02826887 0.341844 0.01461154 0.2968217 0.00999999 0.25 0.00999999 0.25 0.01461154 0.2031783 0.02826887 0.341844 0.01461154 0.2031783 0.02826887 0.1581559 0.02826887 0.341844 0.02826887 0.1581559 0.05044728 0.116663 0.08029437 0.08029437 0.08029437 0.08029437 0.116663 0.05044728 0.1581559 0.02826887 0.1581559 0.02826887 0.2031783 0.01461154 0.341844 0.02826887 0.2031783 0.01461154 0.25 0.00999999 0.341844 0.02826887 0.25 0.00999999 0.2968217 0.01461154 0.341844 0.02826887 0.341844 0.02826887 0.3833369 0.05044728 0.4197056 0.08029437 0.4197056 0.08029437 0.4495527 0.116663 0.4717311 0.1581559 0.4717311 0.1581559 0.4853885 0.2031783 0.4717311 0.341844 0.4853885 0.2031783 0.49 0.25 0.4717311 0.341844 0.49 0.25 0.4853885 0.2968217 0.4717311 0.341844 0.4717311 0.341844 0.4495527 0.3833369 0.4197056 0.4197056 0.4197056 0.4197056 0.3833369 0.4495527 0.341844 0.4717311 0.1581559 0.4717311 0.08029437 0.4197056 0.341844 0.4717311 0.08029437 0.4197056 0.02826887 0.341844 0.341844 0.4717311 0.02826887 0.1581559 0.08029437 0.08029437 0.02826887 0.341844 0.08029437 0.08029437 0.1581559 0.02826887 0.02826887 0.341844 0.341844 0.02826887 0.4197056 0.08029437 0.1581559 0.02826887 0.4197056 0.08029437 0.4717311 0.1581559 0.1581559 0.02826887 0.4717311 0.341844 0.4197056 0.4197056 0.4717311 0.1581559 0.4197056 0.4197056 0.341844 0.4717311 0.4717311 0.1581559 0.341844 0.4717311 0.02826887 0.341844 0.1581559 0.02826887 0.0625 1 0.03125 1 0.03125 0.5 0.03125 1 0 1 0 0.5 0.7031784 0.4853885 0.75 0.49 0.7968217 0.4853885 0.7968217 0.4853885 0.841844 0.4717311 0.8833369 0.4495527 0.8833369 0.4495527 0.9197056 0.4197056 0.9495527 0.3833369 0.9495527 0.3833369 0.9717311 0.341844 0.9853885 0.2968217 0.9853885 0.2968217 0.99 0.25 0.9853885 0.2031783 0.9853885 0.2031783 0.9717311 0.1581559 0.9495527 0.116663 0.9495527 0.116663 0.9197056 0.08029437 0.8833369 0.05044728 0.8833369 0.05044728 0.841844 0.02826887 0.7968217 0.01461154 0.7968217 0.01461154 0.75 0.00999999 0.7031784 0.01461154 0.7031784 0.01461154 0.658156 0.02826887 0.6166632 0.05044728 0.6166632 0.05044728 0.5802944 0.08029437 0.5504473 0.116663 0.5504473 0.116663 0.5282689 0.1581559 0.5146116 0.2031783 0.5146116 0.2031783 0.51 0.25 0.5146116 0.2968217 0.5146116 0.2968217 0.5282689 0.341844 0.5504473 0.3833369 0.5504473 0.3833369 0.5802944 0.4197056 0.6166632 0.4495527 0.6166632 0.4495527 0.658156 0.4717311 0.7031784 0.4853885 0.7031784 0.4853885 0.7968217 0.4853885 0.9853885 0.2968217 0.7968217 0.4853885 0.8833369 0.4495527 0.9853885 0.2968217 0.8833369 0.4495527 0.9495527 0.3833369 0.9853885 0.2968217 0.9853885 0.2968217 0.9853885 0.2031783 0.7968217 0.01461154 0.9853885 0.2031783 0.9495527 0.116663 0.7968217 0.01461154 0.9495527 0.116663 0.8833369 0.05044728 0.7968217 0.01461154 0.7968217 0.01461154 0.7031784 0.01461154 0.5146116 0.2031783 0.7031784 0.01461154 0.6166632 0.05044728 0.5146116 0.2031783 0.6166632 0.05044728 0.5504473 0.116663 0.5146116 0.2031783 0.5146116 0.2031783 0.5146116 0.2968217 0.7031784 0.4853885 0.5146116 0.2968217 0.5504473 0.3833369 0.7031784 0.4853885 0.5504473 0.3833369 0.6166632 0.4495527 0.7031784 0.4853885 0.7031784 0.4853885 0.9853885 0.2968217 0.7968217 0.01461154 + + + + + + + + + + + + + + +

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

+
+
+
+ + + + 0 1 -1 0 1 1 0.1950903 0.9807853 -1 0.1950903 0.9807853 1 0.3826835 0.9238795 -1 0.3826835 0.9238795 1 0.5555703 0.8314696 -1 0.5555703 0.8314696 1 0.7071068 0.7071068 -1 0.7071068 0.7071068 1 0.8314696 0.5555703 -1 0.8314696 0.5555703 1 0.9238795 0.3826835 -1 0.9238795 0.3826835 1 0.9807853 0.1950903 -1 0.9807853 0.1950903 1 1 0 -1 1 0 1 0.9807853 -0.1950903 -1 0.9807853 -0.1950903 1 0.9238795 -0.3826835 -1 0.9238795 -0.3826835 1 0.8314696 -0.5555703 -1 0.8314696 -0.5555703 1 0.7071068 -0.7071068 -1 0.7071068 -0.7071068 1 0.5555703 -0.8314696 -1 0.5555703 -0.8314696 1 0.3826835 -0.9238795 -1 0.3826835 -0.9238795 1 0.1950903 -0.9807853 -1 0.1950903 -0.9807853 1 0 -1 -1 0 -1 1 -0.1950903 -0.9807853 -1 -0.1950903 -0.9807853 1 -0.3826835 -0.9238795 -1 -0.3826835 -0.9238795 1 -0.5555703 -0.8314696 -1 -0.5555703 -0.8314696 1 -0.7071068 -0.7071068 -1 -0.7071068 -0.7071068 1 -0.8314696 -0.5555703 -1 -0.8314696 -0.5555703 1 -0.9238795 -0.3826835 -1 -0.9238795 -0.3826835 1 -0.9807853 -0.1950903 -1 -0.9807853 -0.1950903 1 -1 0 -1 -1 0 1 -0.9807853 0.1950903 -1 -0.9807853 0.1950903 1 -0.9238795 0.3826835 -1 -0.9238795 0.3826835 1 -0.8314696 0.5555703 -1 -0.8314696 0.5555703 1 -0.7071068 0.7071068 -1 -0.7071068 0.7071068 1 -0.5555703 0.8314696 -1 -0.5555703 0.8314696 1 -0.3826835 0.9238795 -1 -0.3826835 0.9238795 1 -0.1950903 0.9807853 -1 -0.1950903 0.9807853 1 + + + + + + + + + + 0.09801697 0.9951848 0 0.2902848 0.9569403 0 0.4713967 0.8819213 0 0.634393 0.7730107 0 0.7730108 0.634393 0 0.8819212 0.471397 0 0.9569405 0.2902846 0 0.9951848 0.09801727 0 0.9951848 -0.09801667 0 0.9569403 -0.2902851 0 0.8819215 -0.4713965 0 0.7730105 -0.6343934 0 0.6343933 -0.7730104 0 0.4713967 -0.8819213 0 0.2902848 -0.9569404 0 0.09801697 -0.9951848 0 -0.09801697 -0.9951848 0 -0.2902848 -0.9569403 0 -0.4713967 -0.8819213 0 -0.634393 -0.7730107 0 -0.7730108 -0.634393 0 -0.8819212 -0.471397 0 -0.9569405 -0.2902846 0 -0.9951848 -0.09801727 0 -0.9951848 0.09801667 0 -0.9569403 0.2902851 0 -0.8819215 0.4713965 0 -0.7730105 0.6343934 0 -0.6343933 0.7730104 0 -0.4713967 0.8819213 0 0 0 1 -0.2902848 0.9569404 0 -0.09801697 0.9951848 0 0 0 -1 0.2902848 0.9569404 0 0.6343933 0.7730104 0 0.7730105 0.6343934 0 0.8819215 0.4713965 0 0.9569403 0.2902851 0 0.9951848 0.09801667 0 0.9951848 -0.09801727 0 0.9569405 -0.2902846 0 0.8819212 -0.471397 0 0.7730108 -0.634393 0 0.634393 -0.7730107 0 0.2902848 -0.9569403 0 -0.2902848 -0.9569404 0 -0.6343933 -0.7730104 0 -0.7730105 -0.6343934 0 -0.8819215 -0.4713965 0 -0.9569403 -0.2902851 0 -0.9951848 -0.09801667 0 -0.9951848 0.09801727 0 -0.9569405 0.2902846 0 -0.8819212 0.471397 0 -0.7730108 0.634393 0 -0.634393 0.7730107 0 -0.2902848 0.9569403 0 3.97512e-6 0 -1 3.97512e-6 0 -1 -3.97512e-6 0 -1 -3.97512e-6 0 -1 3.88857e-7 0 -1 -3.88857e-7 0 -1 + + + + + + + + + + 1 1 0.96875 0.5 1 0.5 0.96875 1 0.9375 0.5 0.96875 0.5 0.9375 1 0.90625 0.5 0.9375 0.5 0.90625 1 0.8750001 0.5 0.90625 0.5 0.8750001 1 0.84375 0.5 0.8750001 0.5 0.84375 1 0.8125 0.5 0.84375 0.5 0.8125 1 0.78125 0.5 0.8125 0.5 0.78125 1 0.75 0.5 0.78125 0.5 0.75 1 0.71875 0.5 0.75 0.5 0.71875 1 0.6875 0.5 0.71875 0.5 0.6875 1 0.65625 0.5 0.6875 0.5 0.65625 1 0.625 0.5 0.65625 0.5 0.625 1 0.59375 0.5 0.625 0.5 0.59375 1 0.5625 0.5 0.59375 0.5 0.5625 1 0.53125 0.5 0.5625 0.5 0.53125 1 0.5 0.5 0.53125 0.5 0.5 1 0.46875 0.5 0.5 0.5 0.46875 1 0.4375 0.5 0.46875 0.5 0.4375 1 0.40625 0.5 0.4375 0.5 0.40625 1 0.375 0.5 0.40625 0.5 0.375 1 0.34375 0.5 0.375 0.5 0.34375 1 0.3125 0.5 0.34375 0.5 0.3125 1 0.28125 0.5 0.3125 0.5 0.28125 1 0.25 0.5 0.28125 0.5 0.25 1 0.21875 0.5 0.25 0.5 0.21875 1 0.1875 0.5 0.21875 0.5 0.1875 1 0.15625 0.5 0.1875 0.5 0.15625 1 0.125 0.5 0.15625 0.5 0.125 1 0.09375 0.5 0.125 0.5 0.09375 1 0.0625 0.5 0.09375 0.5 0.1581559 0.02826887 0.4717311 0.1581559 0.341844 0.4717311 0.0625 1 0.03125 0.5 0.0625 0.5 0.03125 1 0 0.5 0.03125 0.5 0.7968217 0.01461154 0.5146116 0.2031783 0.7031784 0.4853885 1 1 0.96875 1 0.96875 0.5 0.96875 1 0.9375 1 0.9375 0.5 0.9375 1 0.90625 1 0.90625 0.5 0.90625 1 0.8750001 1 0.8750001 0.5 0.8750001 1 0.84375 1 0.84375 0.5 0.84375 1 0.8125 1 0.8125 0.5 0.8125 1 0.78125 1 0.78125 0.5 0.78125 1 0.75 1 0.75 0.5 0.75 1 0.71875 1 0.71875 0.5 0.71875 1 0.6875 1 0.6875 0.5 0.6875 1 0.65625 1 0.65625 0.5 0.65625 1 0.625 1 0.625 0.5 0.625 1 0.59375 1 0.59375 0.5 0.59375 1 0.5625 1 0.5625 0.5 0.5625 1 0.53125 1 0.53125 0.5 0.53125 1 0.5 1 0.5 0.5 0.5 1 0.46875 1 0.46875 0.5 0.46875 1 0.4375 1 0.4375 0.5 0.4375 1 0.40625 1 0.40625 0.5 0.40625 1 0.375 1 0.375 0.5 0.375 1 0.34375 1 0.34375 0.5 0.34375 1 0.3125 1 0.3125 0.5 0.3125 1 0.28125 1 0.28125 0.5 0.28125 1 0.25 1 0.25 0.5 0.25 1 0.21875 1 0.21875 0.5 0.21875 1 0.1875 1 0.1875 0.5 0.1875 1 0.15625 1 0.15625 0.5 0.15625 1 0.125 1 0.125 0.5 0.125 1 0.09375 1 0.09375 0.5 0.09375 1 0.0625 1 0.0625 0.5 0.341844 0.4717311 0.2968217 0.4853885 0.25 0.49 0.25 0.49 0.2031783 0.4853885 0.341844 0.4717311 0.2031783 0.4853885 0.1581559 0.4717311 0.341844 0.4717311 0.1581559 0.4717311 0.116663 0.4495527 0.08029437 0.4197056 0.08029437 0.4197056 0.05044728 0.3833369 0.02826887 0.341844 0.02826887 0.341844 0.01461154 0.2968217 0.00999999 0.25 0.00999999 0.25 0.01461154 0.2031783 0.02826887 0.341844 0.01461154 0.2031783 0.02826887 0.1581559 0.02826887 0.341844 0.02826887 0.1581559 0.05044728 0.116663 0.08029437 0.08029437 0.08029437 0.08029437 0.116663 0.05044728 0.1581559 0.02826887 0.1581559 0.02826887 0.2031783 0.01461154 0.341844 0.02826887 0.2031783 0.01461154 0.25 0.00999999 0.341844 0.02826887 0.25 0.00999999 0.2968217 0.01461154 0.341844 0.02826887 0.341844 0.02826887 0.3833369 0.05044728 0.4197056 0.08029437 0.4197056 0.08029437 0.4495527 0.116663 0.4717311 0.1581559 0.4717311 0.1581559 0.4853885 0.2031783 0.4717311 0.341844 0.4853885 0.2031783 0.49 0.25 0.4717311 0.341844 0.49 0.25 0.4853885 0.2968217 0.4717311 0.341844 0.4717311 0.341844 0.4495527 0.3833369 0.4197056 0.4197056 0.4197056 0.4197056 0.3833369 0.4495527 0.341844 0.4717311 0.1581559 0.4717311 0.08029437 0.4197056 0.341844 0.4717311 0.08029437 0.4197056 0.02826887 0.341844 0.341844 0.4717311 0.02826887 0.1581559 0.08029437 0.08029437 0.02826887 0.341844 0.08029437 0.08029437 0.1581559 0.02826887 0.02826887 0.341844 0.341844 0.02826887 0.4197056 0.08029437 0.1581559 0.02826887 0.4197056 0.08029437 0.4717311 0.1581559 0.1581559 0.02826887 0.4717311 0.341844 0.4197056 0.4197056 0.4717311 0.1581559 0.4197056 0.4197056 0.341844 0.4717311 0.4717311 0.1581559 0.341844 0.4717311 0.02826887 0.341844 0.1581559 0.02826887 0.0625 1 0.03125 1 0.03125 0.5 0.03125 1 0 1 0 0.5 0.7031784 0.4853885 0.75 0.49 0.7968217 0.4853885 0.7968217 0.4853885 0.841844 0.4717311 0.8833369 0.4495527 0.8833369 0.4495527 0.9197056 0.4197056 0.9495527 0.3833369 0.9495527 0.3833369 0.9717311 0.341844 0.9853885 0.2968217 0.9853885 0.2968217 0.99 0.25 0.9853885 0.2031783 0.9853885 0.2031783 0.9717311 0.1581559 0.9495527 0.116663 0.9495527 0.116663 0.9197056 0.08029437 0.8833369 0.05044728 0.8833369 0.05044728 0.841844 0.02826887 0.7968217 0.01461154 0.7968217 0.01461154 0.75 0.00999999 0.7031784 0.01461154 0.7031784 0.01461154 0.658156 0.02826887 0.6166632 0.05044728 0.6166632 0.05044728 0.5802944 0.08029437 0.5504473 0.116663 0.5504473 0.116663 0.5282689 0.1581559 0.5146116 0.2031783 0.5146116 0.2031783 0.51 0.25 0.5146116 0.2968217 0.5146116 0.2968217 0.5282689 0.341844 0.5504473 0.3833369 0.5504473 0.3833369 0.5802944 0.4197056 0.6166632 0.4495527 0.6166632 0.4495527 0.658156 0.4717311 0.7031784 0.4853885 0.7031784 0.4853885 0.7968217 0.4853885 0.9853885 0.2968217 0.7968217 0.4853885 0.8833369 0.4495527 0.9853885 0.2968217 0.8833369 0.4495527 0.9495527 0.3833369 0.9853885 0.2968217 0.9853885 0.2968217 0.9853885 0.2031783 0.7968217 0.01461154 0.9853885 0.2031783 0.9495527 0.116663 0.7968217 0.01461154 0.9495527 0.116663 0.8833369 0.05044728 0.7968217 0.01461154 0.7968217 0.01461154 0.7031784 0.01461154 0.5146116 0.2031783 0.7031784 0.01461154 0.6166632 0.05044728 0.5146116 0.2031783 0.6166632 0.05044728 0.5504473 0.116663 0.5146116 0.2031783 0.5146116 0.2031783 0.5146116 0.2968217 0.7031784 0.4853885 0.5146116 0.2968217 0.5504473 0.3833369 0.7031784 0.4853885 0.5504473 0.3833369 0.6166632 0.4495527 0.7031784 0.4853885 0.7031784 0.4853885 0.9853885 0.2968217 0.7968217 0.01461154 + + + + + + + + + + + + + + +

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

+
+
+
+
+ + + + -0.2908648 -0.7711008 0.5663932 4.076245 0.9551711 -0.1998834 0.2183913 1.005454 -0.05518911 0.6045247 0.7946723 5.903862 0 0 0 1 + + + + 0.6859207 -0.3240135 0.6515582 7.358891 0.7276763 0.3054209 -0.6141704 -6.925791 -4.01133e-9 0.8953956 0.4452714 4.958309 0 0 0 1 + + + + 0.025 0 0 0 0 0.025 0 0 0 0 0.25 0.255 0 0 0 1 + + + + + + + + + + + + 1.88745e-9 0 0.25 0.25 0 0.025 0 0 -0.025 0 1.88745e-8 0 0 0 0 1 + + + + + + + + + + + + 0.025 0 0 0 0 1.88745e-9 -0.25 0.25 0 0.025 1.88745e-8 0 0 0 0 1 + + + + + + + + + + + + 0.6859207 -0.3240135 0.6515582 7.358891 0.7276763 0.3054208 -0.6141704 -6.925791 0 0.8953956 0.4452714 4.958309 0 0 0 1 + + + + -0.2908646 -0.7711008 0.5663932 4.076245 0.9551712 -0.1998834 0.2183912 1.005454 -0.05518906 0.6045247 0.7946723 5.903862 0 0 0 1 + + + + + + + +
\ No newline at end of file diff --git a/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/package.xml b/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/package.xml new file mode 100644 index 000000000..f3e2934ba --- /dev/null +++ b/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/package.xml @@ -0,0 +1,37 @@ + + + + waypoint_rviz2_plugin + 1.0.0 + RViz2 plugin for creating and manipulating interactive 3D waypoints + + koko + Apache-2.0 + + ament_cmake + + rclcpp + rviz_common + rviz_default_plugins + rviz_rendering + rviz_ogre_vendor + geometry_msgs + visualization_msgs + interactive_markers + nav_msgs + tf2 + tf2_geometry_msgs + rosbag2_cpp + pluginlib + qtbase5-dev + + qt5-qmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + + diff --git a/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/plugin_description.xml b/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/plugin_description.xml new file mode 100644 index 000000000..3aa60feef --- /dev/null +++ b/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/plugin_description.xml @@ -0,0 +1,10 @@ + + + + Interactive waypoint creation and manipulation tool for RViz2. + Click to add waypoints, drag to move them, right-click to delete. + + + diff --git a/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/rviz_task_panel_waypoints.png b/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/rviz_task_panel_waypoints.png new file mode 100644 index 000000000..f58a48502 Binary files /dev/null and b/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/rviz_task_panel_waypoints.png differ diff --git a/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/src/waypoint_manager.cpp b/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/src/waypoint_manager.cpp new file mode 100644 index 000000000..4f9374e83 --- /dev/null +++ b/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/src/waypoint_manager.cpp @@ -0,0 +1,664 @@ +/** + * @file waypoint_manager.cpp + * + * Implementation of the shared WaypointManager singleton. + */ + +#include "waypoint_rviz2_plugin/waypoint_manager.hpp" + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +namespace waypoint_rviz2_plugin { + +// ─────────────────────────── singleton ───────────────────────────────────────── + +std::shared_ptr WaypointManager::instance() +{ + static std::weak_ptr weak_instance; + static std::mutex singleton_mtx; + std::lock_guard lock(singleton_mtx); + + auto sp = weak_instance.lock(); + if (!sp) { + sp = std::shared_ptr(new WaypointManager()); + weak_instance = sp; + } + return sp; +} + +WaypointManager::WaypointManager() +{ + selected_marker_name_ = std::string(kWaypointPrefix) + "1"; +} + +WaypointManager::~WaypointManager() +{ + if (scene_manager_) { + for (auto & [idx, node_ptr] : node_map_) { + scene_manager_->destroySceneNode(node_ptr); + } + } +} + +// ─────────────────────────── initialization ─────────────────────────────────── + +void WaypointManager::initialize(rviz_common::DisplayContext * context, + rclcpp::Node::SharedPtr node) +{ + if (initialized_) { return; } + + context_ = context; + node_ = node; + scene_manager_ = context_->getSceneManager(); + + axis_resource_ = "package://waypoint_rviz2_plugin/media/axis.dae"; + if (rviz_rendering::loadMeshFromResource(axis_resource_).isNull()) { + RCLCPP_ERROR(node_->get_logger(), + "WaypointManager: failed to load model resource '%s'.", + axis_resource_.c_str()); + return; + } + + server_ = std::make_shared( + "waypoint_plugin", node_); + + path_publisher_ = + node_->create_publisher(output_topic_, 1); + + menu_handler_.insert( + "delete", + std::bind(&WaypointManager::processFeedback, this, std::placeholders::_1)); + menu_handler_.insert( + "set manual", + std::bind(&WaypointManager::processFeedback, this, std::placeholders::_1)); + + initialized_ = true; +} + +// ─────────────────────────── waypoint operations ────────────────────────────── + +void WaypointManager::addWaypoint(const Ogre::Vector3 & position, + const Ogre::Quaternion & quaternion) +{ + if (!initialized_) { return; } + + unique_idx_++; + std::stringstream wp_name; + wp_name << kWaypointPrefix << unique_idx_; + std::string str_name(wp_name.str()); + + if (rviz_rendering::loadMeshFromResource(axis_resource_).isNull()) { + RCLCPP_ERROR(node_->get_logger(), + "WaypointManager: failed to load model resource '%s'.", + axis_resource_.c_str()); + return; + } + + Ogre::SceneNode * node_ptr = + scene_manager_->getRootSceneNode()->createChildSceneNode(); + Ogre::Entity * entity = scene_manager_->createEntity(axis_resource_); + node_ptr->attachObject(entity); + node_ptr->setVisible(true); + node_ptr->setPosition(position); + node_ptr->setOrientation(quaternion); + + auto node_entry = node_map_.find(unique_idx_); + if (node_entry == node_map_.end()) { + node_map_.insert(std::make_pair(unique_idx_, node_ptr)); + } else { + RCLCPP_WARN(node_->get_logger(), "%s already in map", str_name.c_str()); + return; + } + + // Build interactive marker + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = position.x; + pose.pose.position.y = position.y; + pose.pose.position.z = position.z; + pose.pose.orientation.x = quaternion.x; + pose.pose.orientation.y = quaternion.y; + pose.pose.orientation.z = quaternion.z; + pose.pose.orientation.w = quaternion.w; + + visualization_msgs::msg::InteractiveMarker int_marker; + int_marker.header.stamp = node_->now(); + { + std::lock_guard lock(mtx_); + int_marker.header.frame_id = frame_id_; + } + int_marker.pose = pose.pose; + int_marker.scale = 1.0; + int_marker.name = str_name; + + // Invisible cylinder for picking + visualization_msgs::msg::Marker marker; + marker.type = visualization_msgs::msg::Marker::CYLINDER; + marker.scale.x = 0.5; + marker.scale.y = 0.5; + marker.scale.z = 0.1; + marker.color.r = 0.0; + marker.color.g = 0.0; + marker.color.b = 0.0; + marker.color.a = 0.0; + + visualization_msgs::msg::InteractiveMarkerControl c_control; + c_control.always_visible = true; + c_control.markers.push_back(marker); + int_marker.controls.push_back(c_control); + + visualization_msgs::msg::InteractiveMarkerControl control; + control.orientation.w = 0.707106781; + control.orientation.x = 0; + control.orientation.y = 0.707106781; + control.orientation.z = 0; + control.interaction_mode = + visualization_msgs::msg::InteractiveMarkerControl::MOVE_ROTATE; + int_marker.controls.push_back(control); + + control.interaction_mode = + visualization_msgs::msg::InteractiveMarkerControl::MOVE_AXIS; + int_marker.controls.push_back(control); + + control.interaction_mode = + visualization_msgs::msg::InteractiveMarkerControl::MENU; + control.name = "menu_delete"; + control.description = str_name; + int_marker.controls.push_back(control); + + server_->insert(int_marker); + server_->setCallback( + int_marker.name, + std::bind(&WaypointManager::processFeedback, this, std::placeholders::_1)); + menu_handler_.apply(*server_, int_marker.name); + server_->applyChanges(); + + selected_marker_name_ = str_name; + + // Convert quaternion to yaw for signal + tf2::Quaternion qt(quaternion.x, quaternion.y, quaternion.z, quaternion.w); + tf2::Matrix3x3 m(qt); + double roll, pitch, yaw; + m.getRPY(roll, pitch, yaw); + + Q_EMIT waypointCountChanged(static_cast(node_map_.size())); + Q_EMIT selectedMarkerChanged( + QString::fromStdString(str_name), position.x, position.y, position.z, yaw); +} + +void WaypointManager::removeWaypoint(int idx) +{ + if (!initialized_) { return; } + + auto it = node_map_.find(idx); + if (it == node_map_.end()) { return; } + + // Destroy the scene node for the deleted waypoint + it->second->detachAllObjects(); + scene_manager_->destroySceneNode(it->second); + + std::stringstream wp_name; + wp_name << kWaypointPrefix << idx; + server_->erase(wp_name.str()); + node_map_.erase(it); + + // Reorder: rebuild node_map_ with sequential 1-based keys and rename + // interactive markers so there are no gaps. + reorderWaypoints(); + + server_->applyChanges(); + + // Set unique_idx_ so the next added waypoint gets the right number + unique_idx_ = node_map_.empty() ? 0 : node_map_.rbegin()->first; + + Q_EMIT waypointCountChanged(static_cast(node_map_.size())); +} + +void WaypointManager::reorderWaypoints() +{ + // Collect entries in current order (std::map iterates by key, ascending) + std::vector> entries( + node_map_.begin(), node_map_.end()); + + // Erase all interactive markers from the server (but keep scene nodes alive) + for (const auto & [old_idx, node_ptr] : entries) { + std::stringstream old_name; + old_name << kWaypointPrefix << old_idx; + server_->erase(old_name.str()); + } + + // Rebuild node_map_ with sequential 1-based keys + node_map_.clear(); + for (size_t i = 0; i < entries.size(); ++i) { + int new_idx = static_cast(i) + 1; + Ogre::SceneNode * node_ptr = entries[i].second; + node_map_[new_idx] = node_ptr; + + // Re-create the interactive marker with the new name + std::stringstream new_name; + new_name << kWaypointPrefix << new_idx; + std::string str_name = new_name.str(); + + Ogre::Vector3 pos = node_ptr->getPosition(); + Ogre::Quaternion quat = node_ptr->getOrientation(); + + visualization_msgs::msg::InteractiveMarker int_marker; + int_marker.header.stamp = node_->now(); + { + std::lock_guard lock(mtx_); + int_marker.header.frame_id = frame_id_; + } + int_marker.pose.position.x = pos.x; + int_marker.pose.position.y = pos.y; + int_marker.pose.position.z = pos.z; + int_marker.pose.orientation.x = quat.x; + int_marker.pose.orientation.y = quat.y; + int_marker.pose.orientation.z = quat.z; + int_marker.pose.orientation.w = quat.w; + int_marker.scale = 1.0; + int_marker.name = str_name; + + // Invisible cylinder for picking + visualization_msgs::msg::Marker marker; + marker.type = visualization_msgs::msg::Marker::CYLINDER; + marker.scale.x = 0.5; + marker.scale.y = 0.5; + marker.scale.z = 0.1; + marker.color.a = 0.0; + + visualization_msgs::msg::InteractiveMarkerControl c_control; + c_control.always_visible = true; + c_control.markers.push_back(marker); + int_marker.controls.push_back(c_control); + + visualization_msgs::msg::InteractiveMarkerControl control; + control.orientation.w = 0.707106781; + control.orientation.x = 0; + control.orientation.y = 0.707106781; + control.orientation.z = 0; + control.interaction_mode = + visualization_msgs::msg::InteractiveMarkerControl::MOVE_ROTATE; + int_marker.controls.push_back(control); + + control.interaction_mode = + visualization_msgs::msg::InteractiveMarkerControl::MOVE_AXIS; + int_marker.controls.push_back(control); + + control.interaction_mode = + visualization_msgs::msg::InteractiveMarkerControl::MENU; + control.name = "menu_delete"; + control.description = str_name; + int_marker.controls.push_back(control); + + server_->insert(int_marker); + server_->setCallback( + int_marker.name, + std::bind(&WaypointManager::processFeedback, this, + std::placeholders::_1)); + menu_handler_.apply(*server_, int_marker.name); + } +} + +void WaypointManager::clearAll() +{ + if (!initialized_) { return; } + + for (auto & [idx, node_ptr] : node_map_) { + scene_manager_->destroySceneNode(node_ptr); + } + node_map_.clear(); + unique_idx_ = 0; + + server_->clear(); + server_->applyChanges(); + + Q_EMIT waypointCountChanged(0); + Q_EMIT waypointsCleared(); +} + +nav_msgs::msg::Path WaypointManager::getPath() const +{ + nav_msgs::msg::Path path; + for (const auto & [idx, node_ptr] : node_map_) { + Ogre::Vector3 pos = node_ptr->getPosition(); + Ogre::Quaternion quat = node_ptr->getOrientation(); + + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = pos.x; + pose.pose.position.y = pos.y; + pose.pose.position.z = pos.z; + pose.pose.orientation.x = quat.x; + pose.pose.orientation.y = quat.y; + pose.pose.orientation.z = quat.z; + pose.pose.orientation.w = quat.w; + path.poses.push_back(pose); + } + { + std::lock_guard lock(mtx_); + path.header.frame_id = frame_id_; + } + if (node_) { + path.header.stamp = node_->now(); + } + return path; +} + +void WaypointManager::publishPath() +{ + if (!path_publisher_) { return; } + path_publisher_->publish(getPath()); +} + +void WaypointManager::updateSelectedPose(double x, double y, double z, + double yaw) +{ + if (!initialized_) { return; } + + int selected_idx = 0; + try { + selected_idx = std::stoi( + selected_marker_name_.substr(strlen(kWaypointPrefix))); + } catch (const std::logic_error & e) { + RCLCPP_ERROR_STREAM(node_->get_logger(), e.what()); + return; + } + + auto it = node_map_.find(selected_idx); + if (it == node_map_.end()) { + RCLCPP_ERROR(node_->get_logger(), "%s not found in map", + selected_marker_name_.c_str()); + return; + } + + Ogre::Vector3 position(x, y, z); + tf2::Quaternion qt; + qt.setRPY(0.0, 0.0, yaw); + Ogre::Quaternion quaternion(qt.w(), qt.x(), qt.y(), qt.z()); + + it->second->setPosition(position); + it->second->setOrientation(quaternion); + + std::stringstream wp_name; + wp_name << kWaypointPrefix << it->first; + std::string wp_name_str(wp_name.str()); + + visualization_msgs::msg::InteractiveMarker int_marker; + if (server_->get(wp_name_str, int_marker)) { + int_marker.pose.position.x = x; + int_marker.pose.position.y = y; + int_marker.pose.position.z = z; + int_marker.pose.orientation.x = qt.x(); + int_marker.pose.orientation.y = qt.y(); + int_marker.pose.orientation.z = qt.z(); + int_marker.pose.orientation.w = qt.w(); + server_->setPose(wp_name_str, int_marker.pose, int_marker.header); + } + server_->applyChanges(); +} + +// ─────────────────────────── persistence ────────────────────────────────────── + +void WaypointManager::saveBag(const std::string & filename) +{ + nav_msgs::msg::Path path = getPath(); + + try { + rosbag2_cpp::Writer writer; + + rosbag2_storage::StorageOptions storage_options; + storage_options.uri = filename; + storage_options.storage_id = "sqlite3"; + + rosbag2_cpp::ConverterOptions converter_options; + converter_options.input_serialization_format = "cdr"; + converter_options.output_serialization_format = "cdr"; + + writer.open(storage_options, converter_options); + + rosbag2_storage::TopicMetadata topic_metadata; + topic_metadata.name = "waypoints"; + topic_metadata.type = "nav_msgs/msg/Path"; + topic_metadata.serialization_format = rmw_get_serialization_format(); + writer.create_topic(topic_metadata); + + auto serialized_msg = std::make_shared(); + rclcpp::Serialization serialization; + serialization.serialize_message(&path, serialized_msg.get()); + + auto bag_message = + std::make_shared(); + bag_message->topic_name = "waypoints"; + bag_message->recv_timestamp = node_->now().nanoseconds(); + bag_message->serialized_data = std::shared_ptr( + new rcutils_uint8_array_t, + [](rcutils_uint8_array_t * data) { + auto fini_return = rcutils_uint8_array_fini(data); + delete data; + if (fini_return != RCUTILS_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "waypoint_rviz2_plugin", + "Failed to destroy serialized message %s", + rcutils_get_error_string().str); + } + }); + *bag_message->serialized_data = + serialized_msg->release_rcl_serialized_message(); + + writer.write(bag_message); + + RCLCPP_INFO(node_->get_logger(), "Waypoints saved successfully"); + } catch (const std::exception & e) { + RCLCPP_ERROR(node_->get_logger(), "Failed to save bag: %s", e.what()); + } +} + +void WaypointManager::loadBag(const std::string & filename) +{ + try { + rosbag2_cpp::Reader reader; + + rosbag2_storage::StorageOptions storage_options; + storage_options.uri = filename; + storage_options.storage_id = "sqlite3"; + + rosbag2_cpp::ConverterOptions converter_options; + converter_options.input_serialization_format = "cdr"; + converter_options.output_serialization_format = "cdr"; + + reader.open(storage_options, converter_options); + + while (reader.has_next()) { + auto bag_message = reader.read_next(); + + if (bag_message->topic_name == "waypoints") { + rclcpp::SerializedMessage serialized_msg( + *bag_message->serialized_data); + nav_msgs::msg::Path path; + + rclcpp::Serialization serialization; + serialization.deserialize_message(&serialized_msg, &path); + + RCLCPP_INFO(node_->get_logger(), "Loading %zu waypoints", + path.poses.size()); + + for (size_t i = 0; i < path.poses.size(); i++) { + const auto & pos = path.poses[i]; + Ogre::Vector3 position( + pos.pose.position.x, pos.pose.position.y, pos.pose.position.z); + Ogre::Quaternion quaternion( + pos.pose.orientation.w, pos.pose.orientation.x, + pos.pose.orientation.y, pos.pose.orientation.z); + addWaypoint(position, quaternion); + } + } + } + } catch (const std::exception & e) { + RCLCPP_ERROR(node_->get_logger(), "Failed to load bag: %s", e.what()); + } +} + +// ─────────────────────────── accessors ──────────────────────────────────────── + +std::string WaypointManager::frameId() const +{ + std::lock_guard lock(mtx_); + return frame_id_; +} + +void WaypointManager::setFrameId(const std::string & frame) +{ + std::lock_guard lock(mtx_); + frame_id_ = frame; +} + +std::string WaypointManager::outputTopic() const +{ + std::lock_guard lock(mtx_); + return output_topic_; +} + +void WaypointManager::setOutputTopic(const std::string & topic) +{ + std::lock_guard lock(mtx_); + if (topic == output_topic_) { return; } + output_topic_ = topic; + if (node_ && !topic.empty() && topic != "/") { + path_publisher_.reset(); + path_publisher_ = + node_->create_publisher(output_topic_, 1); + } +} + +double WaypointManager::defaultHeight() const +{ + std::lock_guard lock(mtx_); + return default_height_; +} + +void WaypointManager::setDefaultHeight(double h) +{ + std::lock_guard lock(mtx_); + default_height_ = h; +} + +int WaypointManager::waypointCount() const +{ + return static_cast(node_map_.size()); +} + +// ─────────────────────────── feedback ───────────────────────────────────────── + +void WaypointManager::processFeedback( + const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr & + feedback) +{ + switch (feedback->event_type) { + case visualization_msgs::msg::InteractiveMarkerFeedback::MENU_SELECT: { + int idx = 0; + try { + idx = std::stoi( + feedback->marker_name.substr(strlen(kWaypointPrefix))); + } catch (const std::logic_error & e) { + RCLCPP_ERROR_STREAM(node_->get_logger(), e.what()); + return; + } + + auto node_entry = node_map_.find(idx); + if (node_entry == node_map_.end()) { + RCLCPP_ERROR(node_->get_logger(), "%s not found in map", + feedback->marker_name.c_str()); + return; + } + + if (feedback->menu_entry_id == 1) { + // delete + node_entry->second->detachAllObjects(); + scene_manager_->destroySceneNode(node_entry->second); + + std::stringstream wp_name; + wp_name << kWaypointPrefix << node_entry->first; + server_->erase(wp_name.str()); + node_map_.erase(node_entry); + + reorderWaypoints(); + server_->applyChanges(); + + unique_idx_ = node_map_.empty() ? 0 : node_map_.rbegin()->first; + + Q_EMIT waypointCountChanged(static_cast(node_map_.size())); + } else { + // "set manual" – select this marker for editing in the Navigate panel + selected_marker_name_ = feedback->marker_name; + + Ogre::Vector3 position = node_entry->second->getPosition(); + Ogre::Quaternion quaternion = node_entry->second->getOrientation(); + + tf2::Quaternion qt(quaternion.x, quaternion.y, quaternion.z, + quaternion.w); + tf2::Matrix3x3 m(qt); + double roll, pitch, yaw; + m.getRPY(roll, pitch, yaw); + + Q_EMIT selectedMarkerChanged( + QString::fromStdString(feedback->marker_name), + position.x, position.y, position.z, yaw); + } + break; + } + case visualization_msgs::msg::InteractiveMarkerFeedback::POSE_UPDATE: { + int idx = 0; + try { + idx = std::stoi( + feedback->marker_name.substr(strlen(kWaypointPrefix))); + } catch (const std::logic_error & e) { + RCLCPP_ERROR_STREAM(node_->get_logger(), e.what()); + return; + } + + auto node_entry = node_map_.find(idx); + if (node_entry == node_map_.end()) { + RCLCPP_ERROR(node_->get_logger(), "%s not found in map", + feedback->marker_name.c_str()); + return; + } + + const auto & p = feedback->pose; + Ogre::Vector3 position(p.position.x, p.position.y, p.position.z); + Ogre::Quaternion quaternion( + p.orientation.w, p.orientation.x, p.orientation.y, p.orientation.z); + + node_entry->second->setPosition(position); + node_entry->second->setOrientation(quaternion); + + selected_marker_name_ = feedback->marker_name; + + tf2::Quaternion qt(quaternion.x, quaternion.y, quaternion.z, + quaternion.w); + tf2::Matrix3x3 m(qt); + double roll, pitch, yaw; + m.getRPY(roll, pitch, yaw); + + Q_EMIT selectedMarkerChanged( + QString::fromStdString(feedback->marker_name), + position.x, position.y, position.z, yaw); + break; + } + } +} + +} // namespace waypoint_rviz2_plugin diff --git a/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/src/waypoint_tool.cpp b/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/src/waypoint_tool.cpp new file mode 100644 index 000000000..f41882d53 --- /dev/null +++ b/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/src/waypoint_tool.cpp @@ -0,0 +1,184 @@ +/** + * @file waypoint_tool.cpp + * @author KoKoLates (the21515@gmail.com) + * @version 2.0.0 + * + * RViz2 Tool for placing/removing waypoints via mouse clicks. + * All waypoint state is delegated to WaypointManager. + */ + +#include "waypoint_rviz2_plugin/waypoint_tool.hpp" + +#ifndef Q_MOC_RUN +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#endif + +// Define slots macro for render_window.hpp compatibility +#ifndef slots +#define slots Q_SLOTS +#define signals Q_SIGNALS +#endif + +#include + +namespace waypoint_rviz2_plugin { + +WaypointTool::WaypointTool() +{ + shortcut_key_ = '1'; +} + +WaypointTool::~WaypointTool() +{ + // move_axis_node_ is owned by the Ogre SceneManager; it is cleaned up when + // RViz destroys the scene. +} + +void WaypointTool::onInitialize() +{ + node_ = context_->getRosNodeAbstraction().lock()->get_raw_node(); + + std::string axis_resource = "package://waypoint_rviz2_plugin/media/axis.dae"; + if (rviz_rendering::loadMeshFromResource(axis_resource).isNull()) { + RCLCPP_ERROR(node_->get_logger(), + "WaypointTool: failed to load model resource '%s'.", + axis_resource.c_str()); + return; + } + + move_axis_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode(); + Ogre::Entity * entity = scene_manager_->createEntity(axis_resource); + move_axis_node_->attachObject(entity); + move_axis_node_->setVisible(false); + + // Create / retrieve the shared WaypointManager singleton + manager_ = WaypointManager::instance(); + if (!manager_->isInitialized()) { + manager_->initialize(context_, node_); + } +} + +void WaypointTool::activate() +{ + if (move_axis_node_) { + move_axis_node_->setVisible(true); + } +} + +void WaypointTool::deactivate() +{ + if (move_axis_node_) { + move_axis_node_->setVisible(false); + } +} + +int WaypointTool::processMouseEvent(rviz_common::ViewportMouseEvent & event) +{ + if (!move_axis_node_ || !manager_ || !manager_->isInitialized()) { + return Render; + } + + double height = manager_->defaultHeight(); + Ogre::Vector3 intersection; + Ogre::Quaternion quaternion; + Ogre::Plane ground_plane(Ogre::Vector3::UNIT_Z, height); + + auto render_window = event.panel->getRenderWindow(); + Ogre::Camera * camera = + rviz_rendering::RenderWindowOgreAdapter::getOgreCamera(render_window); + Ogre::Viewport * viewport = + rviz_rendering::RenderWindowOgreAdapter::getOgreViewport(render_window); + + int width = viewport->getActualWidth(); + int height_px = viewport->getActualHeight(); + float screen_x = static_cast(event.x) / static_cast(width); + float screen_y = static_cast(event.y) / static_cast(height_px); + + Ogre::Ray mouse_ray = camera->getCameraToViewportRay(screen_x, screen_y); + std::pair result = mouse_ray.intersects(ground_plane); + + if (result.first) { + intersection = mouse_ray.getPoint(result.second); + move_axis_node_->setVisible(true); + move_axis_node_->setPosition(intersection); + + // Check proximity to existing waypoints + const auto & node_map = manager_->nodeMap(); + for (auto it = node_map.begin(); it != node_map.end(); ++it) { + Ogre::Vector3 stored_pose = it->second->getPosition(); + double distance = std::sqrt( + std::pow(stored_pose.x - intersection.x, 2) + + std::pow(stored_pose.y - intersection.y, 2)); + + if (distance < 0.4) { + move_axis_node_->setVisible(false); + + if (event.rightDown()) { + manager_->removeWaypoint(it->first); + move_axis_node_->setVisible(true); + return Render | Finished; + } + } + } + + if (event.leftDown()) { + manager_->addWaypoint(intersection, quaternion); + return Render | Finished; + } + } else { + move_axis_node_->setVisible(false); + } + return Render; +} + +void WaypointTool::save(rviz_common::Config config) const +{ + config.mapSetValue("Class", getClassId()); + rviz_common::Config wp_config = config.mapMakeChild("WaypointTool"); + + if (manager_) { + wp_config.mapSetValue("topic", + QString::fromStdString(manager_->outputTopic())); + wp_config.mapSetValue("frame_id", + QString::fromStdString(manager_->frameId())); + wp_config.mapSetValue("default_height", manager_->defaultHeight()); + } +} + +void WaypointTool::load(const rviz_common::Config & config) +{ + rviz_common::Config wp_config = config.mapGetChild("WaypointTool"); + QString topic, frame; + float height = 0.0f; + + if (!wp_config.mapGetString("topic", &topic)) { + topic = "/waypoints"; + } + if (!wp_config.mapGetString("frame_id", &frame)) { + frame = "map"; + } + wp_config.mapGetFloat("default_height", &height); + + if (manager_) { + manager_->setOutputTopic(topic.toStdString()); + manager_->setFrameId(frame.toStdString()); + manager_->setDefaultHeight(height); + } +} + +} // namespace waypoint_rviz2_plugin + +#include +PLUGINLIB_EXPORT_CLASS(waypoint_rviz2_plugin::WaypointTool, rviz_common::Tool) diff --git a/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/src/waypoint_widget.cpp b/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/src/waypoint_widget.cpp new file mode 100644 index 000000000..2fc25c6d8 --- /dev/null +++ b/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/src/waypoint_widget.cpp @@ -0,0 +1,503 @@ +/** + * @file waypoint_widget.cpp + * @author KoKoLates (the21515@gmail.com) + * @version 1.0.0 + * @date 2023-06-18 + * + * ROS2 port of the interactive waypoint widget implementation + */ + +#include "waypoint_rviz2_plugin/waypoint_widget.hpp" +#include "waypoint_rviz2_plugin/waypoint_tool.hpp" + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include + +namespace waypoint_rviz2_plugin { + +WaypointWidget::WaypointWidget(rviz_common::DisplayContext *context, + rclcpp::Node::SharedPtr node, + std::map *map_ptr, + std::shared_ptr server, + int *unique_idx, + QWidget *parent, + WaypointTool *waypoint_tool): QWidget(parent) +{ + context_ = context; + node_ = node; + ui_ = new Ui::PluginWidget(); + map_ptr_ = map_ptr; + unique_idx_ = unique_idx; + server_ = server; + frame_id_ = "map"; + waypoint_tool_ = waypoint_tool; + default_height_ = 0.0; + selected_marker_name_ = std::string(waypoint_name_prefix) + "1"; + + scene_manager_ = context_->getSceneManager(); + ui_->setupUi(this); + + path_publisher_ = node_->create_publisher("waypoints", 1); + + connect(ui_->button_save, SIGNAL(clicked()), this, SLOT(saveHandler())); + connect(ui_->button_load, SIGNAL(clicked()), this, SLOT(loadHandler())); + connect(ui_->button_clear, SIGNAL(clicked()), this, SLOT(clearHandler())); + connect(ui_->button_publish, SIGNAL(clicked()), this, SLOT(publishHandler())); + + connect(ui_->x_spinbox, SIGNAL(valueChanged(double)), this, SLOT(poseChange(double))); + connect(ui_->y_spinbox, SIGNAL(valueChanged(double)), this, SLOT(poseChange(double))); + connect(ui_->z_spinbox, SIGNAL(valueChanged(double)), this, SLOT(poseChange(double))); + connect(ui_->yaw_spinbox, SIGNAL(valueChanged(double)), this, SLOT(poseChange(double))); + connect(ui_->height_spinbox, SIGNAL(valueChanged(double)), this, SLOT(heightChange(double))); + + connect(ui_->topic_input, SIGNAL(editingFinished()), this, SLOT(topicChange())); + connect(ui_->frame_input, SIGNAL(editingFinished()), this, SLOT(frameChange())); +} + +WaypointWidget::~WaypointWidget() { + delete ui_; + map_ptr_ = nullptr; +} + +void WaypointWidget::enable() { + show(); +} + +void WaypointWidget::disable() { + path_publisher_.reset(); + hide(); +} + +void WaypointWidget::saveHandler() { + QString filename = QFileDialog::getSaveFileName( + 0, tr("Waypoints Save"), "waypoints", + tr("Save Files (*.db3)") + ); + if (filename.isEmpty()) { + RCLCPP_ERROR(node_->get_logger(), "No saving file selected"); + return; + } + const std::string str_filename = filename.toStdString(); + RCLCPP_INFO_STREAM(node_->get_logger(), "Saving the waypoints into " << str_filename); + + if (filename.endsWith(".db3")) { + saveBag(str_filename); + } + else { + RCLCPP_INFO_STREAM(node_->get_logger(), "Invalid saving file format: " << str_filename); + } +} + +void WaypointWidget::loadHandler() { + const QString filename = QFileDialog::getOpenFileName( + 0, tr("Waypoint load"), "~/", + tr("Load Files (*.db3)") + ); + if (filename.isEmpty()) { + RCLCPP_ERROR(node_->get_logger(), "No loading file selected"); + return; + } + const std::string str_filename = filename.toStdString(); + RCLCPP_INFO(node_->get_logger(), "loading waypoints from %s", str_filename.c_str()); + if (filename.endsWith(".db3")) { + loadBag(str_filename); + } + else { + RCLCPP_INFO_STREAM(node_->get_logger(), "Invalid loading file format: " << str_filename); + } +} + +void WaypointWidget::clearHandler() { + std::map::iterator node_itr; + for (node_itr = map_ptr_->begin(); node_itr != map_ptr_->end(); node_itr++) { + scene_manager_->destroySceneNode(node_itr->second); + } + map_ptr_->clear(); + *unique_idx_ = 0; + + server_->clear(); + server_->applyChanges(); +} + +void WaypointWidget::publishHandler() { + nav_msgs::msg::Path path; + std::map::iterator node_itr; + + for (node_itr = map_ptr_->begin(); node_itr != map_ptr_->end(); node_itr++) { + Ogre::Vector3 position; + position = node_itr->second->getPosition(); + + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = position.x; + pose.pose.position.y = position.y; + pose.pose.position.z = position.z; + + Ogre::Quaternion quat; + quat = node_itr->second->getOrientation(); + pose.pose.orientation.x = quat.x; + pose.pose.orientation.y = quat.y; + pose.pose.orientation.z = quat.z; + pose.pose.orientation.w = quat.w; + + path.poses.push_back(pose); + } + path.header.frame_id = frame_id_.toStdString(); + path.header.stamp = node_->now(); + path_publisher_->publish(path); +} + +void WaypointWidget::poseChange(double value) { + auto node_entry = map_ptr_->end(); + try { + const int selected_marker_idx = std::stoi(selected_marker_name_.substr(strlen(waypoint_name_prefix))); + node_entry = map_ptr_->find(selected_marker_idx); + } + catch (const std::logic_error& e) { + RCLCPP_ERROR_STREAM(node_->get_logger(), e.what()); + return; + } + + if (node_entry == map_ptr_->end()) + RCLCPP_ERROR(node_->get_logger(), "%s not found in map", selected_marker_name_.c_str()); + else + { + Ogre::Vector3 position; + Ogre::Quaternion quaternion; + getPose(position, quaternion); + + node_entry->second->setPosition(position); + node_entry->second->setOrientation(quaternion); + + std::stringstream waypoint_name; + waypoint_name << waypoint_name_prefix << node_entry->first; + std::string waypoint_name_str(waypoint_name.str()); + + visualization_msgs::msg::InteractiveMarker int_marker; + if(server_->get(waypoint_name_str, int_marker)) { + int_marker.pose.position.x = position.x; + int_marker.pose.position.y = position.y; + int_marker.pose.position.z = position.z; + + int_marker.pose.orientation.x = quaternion.x; + int_marker.pose.orientation.y = quaternion.y; + int_marker.pose.orientation.z = quaternion.z; + int_marker.pose.orientation.w = quaternion.w; + + server_->setPose(waypoint_name_str, int_marker.pose, int_marker.header); + } + server_->applyChanges(); + } +} + +void WaypointWidget::frameChange() { + std::lock_guard lock(frame_updates_mutex_); + QString new_frame = ui_->frame_input->text(); + + if ((new_frame != frame_id_) && (new_frame != "")) { + frame_id_ = new_frame; + RCLCPP_INFO(node_->get_logger(), "new frame: %s", frame_id_.toStdString().c_str()); + + std::map::iterator node_itr; + for (node_itr = map_ptr_->begin(); node_itr != map_ptr_->end(); node_itr++) { + std::stringstream waypoint_name; + waypoint_name << "waypoint" << node_itr->first; + std::string waypoint_name_str(waypoint_name.str()); + + visualization_msgs::msg::InteractiveMarker int_marker; + if(server_->get(waypoint_name_str, int_marker)) { + int_marker.header.frame_id = new_frame.toStdString(); + server_->setPose(waypoint_name_str, int_marker.pose, int_marker.header); + } + } + server_->applyChanges(); + } +} + +void WaypointWidget::topicChange() { + QString new_topic = ui_->topic_input->text(); + + if(new_topic != output_topic_) { + path_publisher_.reset(); + output_topic_ = new_topic; + if((output_topic_ != "") && (output_topic_ != "/")) { + path_publisher_ = node_->create_publisher( + output_topic_.toStdString(), 1); + } + } +} + +void WaypointWidget::heightChange(double value) { + auto sn_entry = map_ptr_->end(); + try { + const int selected_marker_idx = std::stoi( + selected_marker_name_.substr((strlen((waypoint_name_prefix)))) + ); + sn_entry = map_ptr_->find(selected_marker_idx); + } catch(const std::logic_error &error) { + RCLCPP_ERROR_STREAM(node_->get_logger(), error.what()); + return; + } + + if (sn_entry == map_ptr_->end()) { + RCLCPP_ERROR(node_->get_logger(), "%s not found in map", selected_marker_name_.c_str()); + } else { + Ogre::Vector3 position; + Ogre::Quaternion quaternion; + getPose(position, quaternion); + + sn_entry->second->setPosition(position); + sn_entry->second->setOrientation(quaternion); + + std::stringstream wp_name; + wp_name << waypoint_name_prefix << sn_entry->first; + std::string wp_name_str(wp_name.str()); + + visualization_msgs::msg::InteractiveMarker int_marker; + if (server_->get(wp_name_str, int_marker)) { + int_marker.pose.position.x = position.x; + int_marker.pose.position.y = position.y; + int_marker.pose.position.z = position.z; + + int_marker.pose.orientation.x = quaternion.x; + int_marker.pose.orientation.y = quaternion.y; + int_marker.pose.orientation.z = quaternion.z; + int_marker.pose.orientation.w = quaternion.w; + + server_->setPose(wp_name_str, int_marker.pose, int_marker.header); + } + server_->applyChanges(); + } +} + +void WaypointWidget::saveBag(const std::string& filename) { + nav_msgs::msg::Path path; + std::map::iterator node_itr; + for (node_itr = map_ptr_->begin(); node_itr != map_ptr_->end(); ++node_itr) { + Ogre::Vector3 position; + position = node_itr->second->getPosition(); + + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = position.x; + pose.pose.position.y = position.y; + pose.pose.position.z = position.z; + + Ogre::Quaternion quat; + quat = node_itr->second->getOrientation(); + pose.pose.orientation.x = quat.x; + pose.pose.orientation.y = quat.y; + pose.pose.orientation.z = quat.z; + pose.pose.orientation.w = quat.w; + + path.poses.push_back(pose); + } + path.header.frame_id = frame_id_.toStdString(); + path.header.stamp = node_->now(); + + try { + rosbag2_cpp::Writer writer; + + rosbag2_storage::StorageOptions storage_options; + storage_options.uri = filename; + storage_options.storage_id = "sqlite3"; + + rosbag2_cpp::ConverterOptions converter_options; + converter_options.input_serialization_format = "cdr"; + converter_options.output_serialization_format = "cdr"; + + writer.open(storage_options, converter_options); + + rosbag2_storage::TopicMetadata topic_metadata; + topic_metadata.name = "waypoints"; + topic_metadata.type = "nav_msgs/msg/Path"; + topic_metadata.serialization_format = rmw_get_serialization_format(); + writer.create_topic(topic_metadata); + + auto serialized_msg = std::make_shared(); + rclcpp::Serialization serialization; + serialization.serialize_message(&path, serialized_msg.get()); + + auto bag_message = std::make_shared(); + bag_message->topic_name = "waypoints"; + bag_message->recv_timestamp = node_->now().nanoseconds(); + bag_message->serialized_data = std::shared_ptr( + new rcutils_uint8_array_t, + [](rcutils_uint8_array_t* data) { + auto fini_return = rcutils_uint8_array_fini(data); + delete data; + if (fini_return != RCUTILS_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "waypoint_rviz2_plugin", + "Failed to destroy serialized message %s", rcutils_get_error_string().str); + } + }); + *bag_message->serialized_data = serialized_msg->release_rcl_serialized_message(); + + writer.write(bag_message); + + RCLCPP_INFO(node_->get_logger(), "Waypoints saved successfully"); + } + catch (const std::exception& e) { + RCLCPP_ERROR(node_->get_logger(), "Failed to save bag: %s", e.what()); + } +} + +void WaypointWidget::loadBag(const std::string& filename) { + try { + rosbag2_cpp::Reader reader; + + rosbag2_storage::StorageOptions storage_options; + storage_options.uri = filename; + storage_options.storage_id = "sqlite3"; + + rosbag2_cpp::ConverterOptions converter_options; + converter_options.input_serialization_format = "cdr"; + converter_options.output_serialization_format = "cdr"; + + reader.open(storage_options, converter_options); + + while (reader.has_next()) { + auto bag_message = reader.read_next(); + + if (bag_message->topic_name == "waypoints") { + rclcpp::SerializedMessage serialized_msg(*bag_message->serialized_data); + nav_msgs::msg::Path path; + + rclcpp::Serialization serialization; + serialization.deserialize_message(&serialized_msg, &path); + + RCLCPP_INFO(node_->get_logger(), "n waypoints %zu", path.poses.size()); + + for (size_t i = 0; i < path.poses.size(); i++) { + geometry_msgs::msg::PoseStamped pos = path.poses[i]; + Ogre::Vector3 position; + position.x = pos.pose.position.x; + position.y = pos.pose.position.y; + position.z = pos.pose.position.z; + + Ogre::Quaternion quaternion; + quaternion.x = pos.pose.orientation.x; + quaternion.y = pos.pose.orientation.y; + quaternion.z = pos.pose.orientation.z; + quaternion.w = pos.pose.orientation.w; + + waypoint_tool_->makeItem(position, quaternion); + } + } + } + } + catch (const std::exception& e) { + RCLCPP_ERROR(node_->get_logger(), "Failed to load bag: %s", e.what()); + } +} + +void WaypointWidget::setPose(const Ogre::Vector3& position, const Ogre::Quaternion& quaternion) { + ui_->x_spinbox->blockSignals(true); + ui_->y_spinbox->blockSignals(true); + ui_->z_spinbox->blockSignals(true); + ui_->yaw_spinbox->blockSignals(true); + + ui_->x_spinbox->setValue(position.x); + ui_->y_spinbox->setValue(position.y); + ui_->z_spinbox->setValue(position.z); + + tf2::Quaternion qt(quaternion.x, quaternion.y, quaternion.z, quaternion.w); + tf2::Matrix3x3 m(qt); + double roll, pitch, yaw; + m.getRPY(roll, pitch, yaw); + ui_->yaw_spinbox->setValue(yaw); + + ui_->x_spinbox->blockSignals(false); + ui_->y_spinbox->blockSignals(false); + ui_->z_spinbox->blockSignals(false); + ui_->yaw_spinbox->blockSignals(false); +} + +void WaypointWidget::setConfig(QString topic, QString frame, float height) { + { + std::lock_guard lock(frame_updates_mutex_); + ui_->topic_input->blockSignals(true); + ui_->frame_input->blockSignals(true); + ui_->height_spinbox->blockSignals(true); + + ui_->topic_input->setText(topic); + ui_->frame_input->setText(frame); + ui_->height_spinbox->setValue(height); + + ui_->topic_input->blockSignals(false); + ui_->frame_input->blockSignals(false); + ui_->height_spinbox->blockSignals(false); + } + + topicChange(); + frameChange(); + heightChange(height); +} + +void WaypointWidget::setWaypointLabel(Ogre::Vector3 position) { + std::ostringstream string_stream; + string_stream.precision(2); + string_stream << selected_marker_name_; + std::string label = string_stream.str(); + ui_->text_selected->setText(QString::fromStdString(label)); +} + +void WaypointWidget::setWaypointCount(int size) { + std::ostringstream string_stream; + string_stream << "Total waypoints: " << size; + std::lock_guard lock(frame_updates_mutex_); + ui_->text_total->setText( + QString::fromStdString(string_stream.str()) + ); +} + +void WaypointWidget::setSelectedMarkerName(std::string name) { + selected_marker_name_ = name; +} + +void WaypointWidget::getPose(Ogre::Vector3& position, Ogre::Quaternion& quaternion) { + std::lock_guard lock(frame_updates_mutex_); + position.x = ui_->x_spinbox->value(); + position.y = ui_->y_spinbox->value(); + position.z = ui_->z_spinbox->value(); + double yaw = ui_->yaw_spinbox->value(); + + tf2::Quaternion qt; + qt.setRPY(0.0, 0.0, yaw); + quaternion.x = qt.x(); + quaternion.y = qt.y(); + quaternion.z = qt.z(); + quaternion.w = qt.w(); +} + +double WaypointWidget::getDefaultHeight() { + std::lock_guard lock(frame_updates_mutex_); + return default_height_; +} + +QString WaypointWidget::getFrameId() { + std::lock_guard lock(frame_updates_mutex_); + return frame_id_; +} + +QString WaypointWidget::getOutputTopic() { + std::lock_guard lock(frame_updates_mutex_); + return output_topic_; +} + +} diff --git a/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/ui/waypoint_plugin.ui b/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/ui/waypoint_plugin.ui new file mode 100644 index 000000000..bce84c609 --- /dev/null +++ b/common/ros_packages/gui/rviz/3d_waypoint_rviz2_plugin/ui/waypoint_plugin.ui @@ -0,0 +1,253 @@ + + + PluginWidget + + + + 0 + 0 + 469 + 175 + + + + Quadrotor Steering + + + + + + + + Topic + + + + + + + /waypoints + + + + + + + Frame + + + + + + + map + + + + + + + 6D + + + + + + + + + + + Default Height + + + + + + + -5.000000000000000 + + + 0.100000000000000 + + + + + + + Total Waypoints: + + + + + + + Selected wp: + + + + + + + + + + + + + + + + Load Waypoints + + + + + + + + + X: + + + + + + + -1000.000000000000000 + + + 1000.000000000000000 + + + 0.500000000000000 + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + Y: + + + + + + + -1000.000000000000000 + + + 1000.000000000000000 + + + 0.500000000000000 + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + Z: + + + + + + + -1000.000000000000000 + + + 1000.000000000000000 + + + 0.500000000000000 + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + Yaw: + + + + + + + -3.150000000000000 + + + 3.150000000000000 + + + 0.100000000000000 + + + + + + + + + Clear All + + + + + + + Save Waypoints + + + + + + + Publish Waypoints + + + + + + + + diff --git a/common/ros_packages/gui/rviz_behavior_tree_panel/.clang-format b/common/ros_packages/gui/rviz/rviz_behavior_tree_panel/.clang-format similarity index 100% rename from common/ros_packages/gui/rviz_behavior_tree_panel/.clang-format rename to common/ros_packages/gui/rviz/rviz_behavior_tree_panel/.clang-format diff --git a/common/ros_packages/gui/rviz_behavior_tree_panel/.gitignore b/common/ros_packages/gui/rviz/rviz_behavior_tree_panel/.gitignore similarity index 100% rename from common/ros_packages/gui/rviz_behavior_tree_panel/.gitignore rename to common/ros_packages/gui/rviz/rviz_behavior_tree_panel/.gitignore diff --git a/common/ros_packages/gui/rviz_behavior_tree_panel/.pre-commit-config.yaml b/common/ros_packages/gui/rviz/rviz_behavior_tree_panel/.pre-commit-config.yaml similarity index 100% rename from common/ros_packages/gui/rviz_behavior_tree_panel/.pre-commit-config.yaml rename to common/ros_packages/gui/rviz/rviz_behavior_tree_panel/.pre-commit-config.yaml diff --git a/common/ros_packages/gui/rviz_behavior_tree_panel/CMakeLists.txt b/common/ros_packages/gui/rviz/rviz_behavior_tree_panel/CMakeLists.txt similarity index 100% rename from common/ros_packages/gui/rviz_behavior_tree_panel/CMakeLists.txt rename to common/ros_packages/gui/rviz/rviz_behavior_tree_panel/CMakeLists.txt diff --git a/common/ros_packages/gui/rviz_behavior_tree_panel/LICENSE b/common/ros_packages/gui/rviz/rviz_behavior_tree_panel/LICENSE similarity index 100% rename from common/ros_packages/gui/rviz_behavior_tree_panel/LICENSE rename to common/ros_packages/gui/rviz/rviz_behavior_tree_panel/LICENSE diff --git a/common/ros_packages/gui/rviz_behavior_tree_panel/README.md b/common/ros_packages/gui/rviz/rviz_behavior_tree_panel/README.md similarity index 100% rename from common/ros_packages/gui/rviz_behavior_tree_panel/README.md rename to common/ros_packages/gui/rviz/rviz_behavior_tree_panel/README.md diff --git a/common/ros_packages/gui/rviz_behavior_tree_panel/icons/classes/BehaviorTreePanel.png b/common/ros_packages/gui/rviz/rviz_behavior_tree_panel/icons/classes/BehaviorTreePanel.png similarity index 100% rename from common/ros_packages/gui/rviz_behavior_tree_panel/icons/classes/BehaviorTreePanel.png rename to common/ros_packages/gui/rviz/rviz_behavior_tree_panel/icons/classes/BehaviorTreePanel.png diff --git a/common/ros_packages/gui/rviz_behavior_tree_panel/icons/classes/DemoPanel.png b/common/ros_packages/gui/rviz/rviz_behavior_tree_panel/icons/classes/DemoPanel.png similarity index 100% rename from common/ros_packages/gui/rviz_behavior_tree_panel/icons/classes/DemoPanel.png rename to common/ros_packages/gui/rviz/rviz_behavior_tree_panel/icons/classes/DemoPanel.png diff --git a/common/ros_packages/gui/rviz_behavior_tree_panel/include/rviz_behavior_tree_panel/behavior_tree_panel.hpp b/common/ros_packages/gui/rviz/rviz_behavior_tree_panel/include/rviz_behavior_tree_panel/behavior_tree_panel.hpp similarity index 100% rename from common/ros_packages/gui/rviz_behavior_tree_panel/include/rviz_behavior_tree_panel/behavior_tree_panel.hpp rename to common/ros_packages/gui/rviz/rviz_behavior_tree_panel/include/rviz_behavior_tree_panel/behavior_tree_panel.hpp diff --git a/common/ros_packages/gui/rviz_behavior_tree_panel/package.xml b/common/ros_packages/gui/rviz/rviz_behavior_tree_panel/package.xml similarity index 100% rename from common/ros_packages/gui/rviz_behavior_tree_panel/package.xml rename to common/ros_packages/gui/rviz/rviz_behavior_tree_panel/package.xml diff --git a/common/ros_packages/gui/rviz_behavior_tree_panel/rviz_common_plugins.xml b/common/ros_packages/gui/rviz/rviz_behavior_tree_panel/rviz_common_plugins.xml similarity index 100% rename from common/ros_packages/gui/rviz_behavior_tree_panel/rviz_common_plugins.xml rename to common/ros_packages/gui/rviz/rviz_behavior_tree_panel/rviz_common_plugins.xml diff --git a/common/ros_packages/gui/rviz_behavior_tree_panel/scripts/simple_test_publisher.py b/common/ros_packages/gui/rviz/rviz_behavior_tree_panel/scripts/simple_test_publisher.py similarity index 100% rename from common/ros_packages/gui/rviz_behavior_tree_panel/scripts/simple_test_publisher.py rename to common/ros_packages/gui/rviz/rviz_behavior_tree_panel/scripts/simple_test_publisher.py diff --git a/common/ros_packages/gui/rviz_behavior_tree_panel/scripts/test_behavior_tree_publisher.py b/common/ros_packages/gui/rviz/rviz_behavior_tree_panel/scripts/test_behavior_tree_publisher.py similarity index 100% rename from common/ros_packages/gui/rviz_behavior_tree_panel/scripts/test_behavior_tree_publisher.py rename to common/ros_packages/gui/rviz/rviz_behavior_tree_panel/scripts/test_behavior_tree_publisher.py diff --git a/common/ros_packages/gui/rviz_behavior_tree_panel/src/behavior_tree_panel.cpp b/common/ros_packages/gui/rviz/rviz_behavior_tree_panel/src/behavior_tree_panel.cpp similarity index 100% rename from common/ros_packages/gui/rviz_behavior_tree_panel/src/behavior_tree_panel.cpp rename to common/ros_packages/gui/rviz/rviz_behavior_tree_panel/src/behavior_tree_panel.cpp diff --git a/common/ros_packages/gui/rviz_behavior_tree_panel/xdot_cpp b/common/ros_packages/gui/rviz/rviz_behavior_tree_panel/xdot_cpp similarity index 100% rename from common/ros_packages/gui/rviz_behavior_tree_panel/xdot_cpp rename to common/ros_packages/gui/rviz/rviz_behavior_tree_panel/xdot_cpp diff --git a/common/ros_packages/gui/rviz/rviz_polygon_selection_tool b/common/ros_packages/gui/rviz/rviz_polygon_selection_tool new file mode 160000 index 000000000..018c74644 --- /dev/null +++ b/common/ros_packages/gui/rviz/rviz_polygon_selection_tool @@ -0,0 +1 @@ +Subproject commit 018c74644f1c1b96dde48b4bf118c3bcf0d70877 diff --git a/common/ros_packages/gui/rviz/rviz_tasks_panel/CMakeLists.txt b/common/ros_packages/gui/rviz/rviz_tasks_panel/CMakeLists.txt new file mode 100644 index 000000000..e6f81b9cc --- /dev/null +++ b/common/ros_packages/gui/rviz/rviz_tasks_panel/CMakeLists.txt @@ -0,0 +1,83 @@ +cmake_minimum_required(VERSION 3.8) +project(rviz_tasks_panel) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_AUTOMOC ON) + +find_package(ament_cmake REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rviz_common REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(Qt5 REQUIRED COMPONENTS Core Widgets Gui) +find_package(task_msgs REQUIRED) +find_package(airstack_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(diagnostic_msgs REQUIRED) +find_package(action_msgs REQUIRED) +find_package(rviz_polygon_selection_tool REQUIRED) +find_package(std_msgs REQUIRED) +find_package(waypoint_rviz2_plugin REQUIRED) + +qt5_wrap_cpp(MOC_FILES + include/rviz_tasks_panel/tasks_panel.hpp +) + +add_library(tasks_panel SHARED + src/tasks_panel.cpp + ${MOC_FILES} +) + +target_include_directories(tasks_panel PUBLIC + $ + $ +) + +ament_target_dependencies(tasks_panel + pluginlib + rviz_common + rclcpp + rclcpp_action + task_msgs + airstack_msgs + geometry_msgs + nav_msgs + std_msgs + diagnostic_msgs + action_msgs + rviz_polygon_selection_tool + waypoint_rviz2_plugin +) + +target_link_libraries(tasks_panel + Qt5::Core + Qt5::Widgets + Qt5::Gui +) + +install(TARGETS tasks_panel + EXPORT export_rviz_tasks_panel + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(DIRECTORY include/ + DESTINATION include +) + +install(FILES plugin_description.xml + DESTINATION share/${PROJECT_NAME} +) + +ament_export_include_directories(include) +ament_export_targets(export_rviz_tasks_panel) +pluginlib_export_plugin_description_file(rviz_common plugin_description.xml) + +ament_package() diff --git a/common/ros_packages/gui/rviz/rviz_tasks_panel/README.md b/common/ros_packages/gui/rviz/rviz_tasks_panel/README.md new file mode 100644 index 000000000..1822cb492 --- /dev/null +++ b/common/ros_packages/gui/rviz/rviz_tasks_panel/README.md @@ -0,0 +1,185 @@ +# RViz Tasks Panel + +RViz2 panel plugin for dispatching and monitoring ROS 2 task +action goals. Provides a tabbed GUI where operators can +parameterize, execute, and cancel tasks on any discovered robot, +with live feedback and result display. + +![Screenshot of the RViz Tasks Panel showing multiple tabs and parameter widgets](./media/rviz_tasks_panel.png) +![takeoff](./media/tasks_panel_takeoff.png) +![navigate](./media/tasks_panel_navigate.png) + +## Overview + +The Tasks Panel replaces CLI-based action goal dispatch with a +graphical interface for all 9 AirStack task types. Each task type +gets its own tab with auto-generated parameter widgets, an +executor selector, and a feedback/result view. + +The **Navigate** tab integrates directly with the +`waypoint_rviz2_plugin` package's shared `WaypointManager` +singleton, providing inline controls for placing, editing, +saving/loading, and executing waypoint-based navigation -- no +separate waypoint panel needed. + +```text ++-----------------------------------------------------+ +| Tasks Panel Robot: [robot_1] | ++-----------------------------------------------------+ +| [Takeoff] [Land] [Navigate] [Exploration] [Coverage] | ++-----------------------------------------------------+ +| +- Goal Parameters -----+ +- Feedback & Result -+ | +| | Executor: [/robot1/..] | | Feedback: | | +| | Height: [2.0] Wp: 3 | | [live stream] | | +| | X:[1] Y:[2] Z:[2] Y:[0]| | | | +| | [Clear] [Save] [Load] | | Result: | | +| | goal_tolerance_m: [1.0] | | [goal outcome] | | +| | [Cancel] [Execute] | | | | +| +-------------------------+ +---------------------+ | ++-----------------------------------------------------+ +``` + +## Features + +- **9 task tabs** with auto-generated goal parameter widgets +- **Executor discovery** -- scans ROS 2 topics every 5 seconds + to find running action servers +- **Robot namespace selector** -- auto-populated from discovered + action server namespaces +- **Polygon input** -- integrates with + `rviz_polygon_selection_tool` to capture 3D polygon selections +- **Integrated waypoint controls** -- the Navigate tab embeds + waypoint management (height, X/Y/Z/Yaw editing, Clear, Save, + Load) via the shared `WaypointManager` singleton from + `waypoint_rviz2_plugin` +- **Fixed Trajectory editor** -- type dropdown with + auto-populated default attributes in an editable key-value + table +- **Live feedback** -- timestamped feedback messages stream in + real time +- **Result display** -- color-coded status (green = succeeded, + red = aborted, orange = canceled) +- **Config persistence** -- robot and executor selections are + saved/restored with the RViz config +- **Single-task lock** -- only one action may execute at a time + to prevent conflicts +- **Airborne-only tasks** -- Land, Navigate, and all mission + tasks are disabled until the drone is airborne (subscribes to + `is_airborne` from `takeoff_landing_planner`); Takeoff is + always available while idle + +## Supported Task Types + +Tasks marked **airborne-only** are disabled until the drone is +in the air. + +| Tab | Action Type | Key Parameters | Airborne-only | +|-----|-------------|----------------|---------------| +| Takeoff | `TakeoffTask` | `target_altitude_m`, `velocity_m_s` | | +| Land | `LandTask` | `velocity_m_s` | ✓ | +| Navigate | `NavigateTask` | `global_plan` (Path), `goal_tolerance_m` | ✓ | +| Exploration | `ExplorationTask` | `search_bounds` (Polygon), altitude/speed, `time_limit_sec` | ✓ | +| Coverage | `CoverageTask` | `coverage_area` (Polygon), `line_spacing_m`, `heading_deg` | ✓ | +| Object Search | `ObjectSearchTask` | `object_class`, `search_area`, `target_count` | ✓ | +| Object Counting | `ObjectCountingTask` | `object_class`, `count_area` | ✓ | +| Semantic Search | `SemanticSearchTask` | `query`, `search_area`, `confidence_threshold` | ✓ | +| Fixed Trajectory | `FixedTrajectoryTask` | `trajectory_spec`, `loop` | ✓ | + +## Widget Type Mapping + +| ROS Type | Widget | Notes | +|----------|--------|-------| +| `float32`/`float64` | `QDoubleSpinBox` | Range from registry | +| `int32` | `QSpinBox` | Range from registry | +| `string` | `QLineEdit` | Free-text input | +| `bool` | `QCheckBox` | Toggle | +| `geometry_msgs/Polygon` | `QPushButton` | Calls `get_selection` service | +| `nav_msgs/Path` | Waypoint controls | Inline height/pose/buttons via WaypointManager | +| `airstack_msgs/FixedTrajectory` | `QComboBox` + `QTableWidget` | Type + attributes | + +## Dependencies + +- `rviz_common` -- RViz2 panel base class +- `pluginlib` -- plugin loading +- `rclcpp` / `rclcpp_action` -- ROS 2 node and action client +- `task_msgs` -- action definitions for all 9 task types +- `airstack_msgs` -- `FixedTrajectory` message +- `geometry_msgs` / `nav_msgs` / `std_msgs` -- standard message types +- `diagnostic_msgs` / `action_msgs` -- status introspection +- `rviz_polygon_selection_tool` -- polygon selection service +- `waypoint_rviz2_plugin` -- shared `WaypointManager` singleton +- Qt5 (Core, Widgets, Gui) + +## Build + +```bash +colcon build --packages-select waypoint_rviz2_plugin rviz_tasks_panel +``` + +## Usage + +1. Launch RViz2 +2. Go to **Panels > Add New Panel** +3. Select **rviz_tasks_panel / TasksPanel** +4. The panel auto-discovers running task action servers and + populates the Robot dropdown +5. Select a tab, configure goal parameters, and click **Execute** +6. Monitor feedback in the right pane; click **Cancel** to abort + +### Waypoint Navigation (Navigate tab) + +1. Activate the **Waypoint Tool** (key **1**) in the RViz toolbar +2. Left-click in the 3D viewport to place waypoints +3. In the Navigate tab, adjust **Default Height**, or edit the + selected waypoint's **X/Y/Z/Yaw** spinboxes +4. Use **Save** / **Load** to persist waypoints as `.db3` files +5. Use **Clear All** to remove all waypoints +6. Set **goal_tolerance_m** and click **Execute** to navigate + +### Polygon Selection + +For tasks requiring a polygon boundary: + +1. Activate the **Polygon Selection Tool** in the toolbar +2. Draw a polygon in the 3D viewport +3. Click **Get Polygon from RViz** in the relevant task tab + +### Fixed Trajectory + +1. Select a trajectory type from the dropdown +2. Edit default attributes in the key-value table +3. Click **Execute** + +## Executor Discovery + +The panel scans `get_topic_names_and_types()` for topics matching +`*//_action/status`. For each match, it +extracts the robot namespace and populates: + +- The top-level **Robot** dropdown +- Each tab's **Executor** dropdown + +Discovery runs every 5 seconds and can be triggered manually with +the **Refresh** button. + +## Architecture + +- **Compile-time task registry**: `getTaskDefs()` returns a + static vector of `TaskTypeDef` structs defining all task types +- **Shared waypoint state**: The Navigate tab holds a + `shared_ptr` (singleton from + `waypoint_rviz2_plugin`), the same instance used by + `WaypointTool` for 3D mouse interaction +- **Type-erased action clients**: `std::any` stores templated + `rclcpp_action::Client` instances per tab +- **Thread safety**: ROS 2 callbacks use + `QMetaObject::invokeMethod` with `Qt::QueuedConnection` to + marshal UI updates to the Qt main thread +- **Airborne gate**: subscribes to `{robot}/is_airborne` + (std_msgs/Bool from `takeoff_landing_planner`); per-tab Execute + buttons are enabled only when `task_idle && airborne_ok`, where + `airborne_ok = !requires_airborne || is_airborne_` + +## License + +MIT -- Carnegie Mellon University diff --git a/common/ros_packages/gui/rviz/rviz_tasks_panel/include/rviz_tasks_panel/tasks_panel.hpp b/common/ros_packages/gui/rviz/rviz_tasks_panel/include/rviz_tasks_panel/tasks_panel.hpp new file mode 100644 index 000000000..0c4f93809 --- /dev/null +++ b/common/ros_packages/gui/rviz/rviz_tasks_panel/include/rviz_tasks_panel/tasks_panel.hpp @@ -0,0 +1,189 @@ +// Copyright (c) 2024 Carnegie Mellon University +// MIT License + +#pragma once + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace rviz_tasks_panel +{ + +// Goal field definition for auto-generating widgets +struct GoalFieldDef +{ + std::string name; + std::string ros_type; // "float32", "int32", "string", "bool", + // "geometry_msgs/Polygon", "nav_msgs/Path", + // "airstack_msgs/FixedTrajectory" + double default_value{0.0}; + double min_value{0.0}; + double max_value{10000.0}; +}; + +// Task type definition +struct TaskTypeDef +{ + std::string display_name; + std::string action_topic_suffix; // e.g. "tasks/takeoff" + std::vector goal_fields; + bool requires_airborne{false}; // if true, Execute is disabled while drone is on the ground +}; + +// Per-tab runtime state +struct TaskTabState +{ + QComboBox * executor_combo{nullptr}; + std::map field_widgets; + std::map field_data; // complex types (Polygon, Path) + QPushButton * cancel_btn{nullptr}; + QPushButton * execute_btn{nullptr}; + QTextEdit * feedback_display{nullptr}; + QTextEdit * result_display{nullptr}; + QLabel * status_label{nullptr}; + std::any action_client; + std::any goal_handle; + bool goal_active{false}; +}; + +class TasksPanel : public rviz_common::Panel +{ + Q_OBJECT + +public: + explicit TasksPanel(QWidget * parent = nullptr); + ~TasksPanel() override = default; + void onInitialize() override; + void save(rviz_common::Config config) const override; + void load(const rviz_common::Config & config) override; + +private Q_SLOTS: + void onRefreshExecutors(); + void onExecuteClicked(); + void onCancelClicked(); + void onGetPolygon(int tab_index, const std::string & field_name); + void onTrajectoryTypeChanged(int tab_index, const QString & type); + + // Waypoint UI slots + void onWaypointCountChanged(int count); + void onSelectedMarkerChanged( + const QString & name, double x, double y, double z, double yaw); + void onClearWaypoints(); + void onSaveWaypoints(); + void onLoadWaypoints(); + void onWaypointPoseChanged(double value); + +private: + rclcpp::Node::SharedPtr raw_node_; + + // Top bar + QComboBox * robot_combo_{nullptr}; + + // Tabs + QTabWidget * tab_widget_{nullptr}; + std::vector task_defs_; + std::vector tab_states_; + + // Polygon selection service + rclcpp::Client::SharedPtr polygon_client_; + + // Shared waypoint manager (singleton, also held by WaypointTool) + std::shared_ptr waypoint_manager_; + + // Waypoint UI widgets (Navigate tab) + QLabel * wp_count_label_{nullptr}; + QLabel * wp_selected_label_{nullptr}; + QDoubleSpinBox * wp_height_spin_{nullptr}; + QDoubleSpinBox * wp_x_spin_{nullptr}; + QDoubleSpinBox * wp_y_spin_{nullptr}; + QDoubleSpinBox * wp_z_spin_{nullptr}; + QDoubleSpinBox * wp_yaw_spin_{nullptr}; + bool wp_updating_spinboxes_{false}; // guard against signal loops + + // Global task lock: only one task may run at a time + int active_task_tab_{-1}; // -1 = no task running, else = tab index of active task + + // Airborne state (fed by is_airborne topic from takeoff_landing_planner) + bool is_airborne_{false}; + QString is_airborne_robot_; // robot namespace currently subscribed to + rclcpp::Subscription::SharedPtr is_airborne_sub_; + + // Discovery timer + QTimer * refresh_timer_{nullptr}; + + static std::vector getTaskDefs(); + void buildTabs(); + QWidget * buildGoalFieldWidget(const GoalFieldDef & def, int tab_index); + void updateAvailableExecutors(); + + // Widget value extraction helpers + double getFloat(int tab_index, const std::string & field_name); + int32_t getInt(int tab_index, const std::string & field_name); + std::string getString(int tab_index, const std::string & field_name); + bool getBool(int tab_index, const std::string & field_name); + geometry_msgs::msg::Polygon getPolygon(int tab_index, const std::string & field_name); + nav_msgs::msg::Path getPath(int tab_index, const std::string & field_name); + airstack_msgs::msg::FixedTrajectory getFixedTrajectory(int tab_index, const std::string & field_name); + + // Template for sending goals with type-specific callbacks + template + void doSendGoal( + int tab_index, + const typename ActionT::Goal & goal, + std::function fmt_feedback, + std::function fmt_result); + + template + void doCancelGoal(int tab_index); + + void setGoalActive(int tab_index, bool active); + void updateExecuteButtons(); + void renewAirborneSubscription(); + void setTabStatus(int tab_index, const QString & icon, const QColor & text_color); + void clearTabStatus(int tab_index); + QString currentRobot() const; +}; + +} // namespace rviz_tasks_panel diff --git a/common/ros_packages/gui/rviz/rviz_tasks_panel/media/rviz_tasks_panel.png b/common/ros_packages/gui/rviz/rviz_tasks_panel/media/rviz_tasks_panel.png new file mode 100644 index 000000000..e87af772d Binary files /dev/null and b/common/ros_packages/gui/rviz/rviz_tasks_panel/media/rviz_tasks_panel.png differ diff --git a/common/ros_packages/gui/rviz/rviz_tasks_panel/media/tasks_panel_navigate.png b/common/ros_packages/gui/rviz/rviz_tasks_panel/media/tasks_panel_navigate.png new file mode 100644 index 000000000..46b03cbd0 Binary files /dev/null and b/common/ros_packages/gui/rviz/rviz_tasks_panel/media/tasks_panel_navigate.png differ diff --git a/common/ros_packages/gui/rviz/rviz_tasks_panel/media/tasks_panel_takeoff.png b/common/ros_packages/gui/rviz/rviz_tasks_panel/media/tasks_panel_takeoff.png new file mode 100644 index 000000000..01639d0bb Binary files /dev/null and b/common/ros_packages/gui/rviz/rviz_tasks_panel/media/tasks_panel_takeoff.png differ diff --git a/robot/ros_ws/src/behavior/behavior_executive/package.xml b/common/ros_packages/gui/rviz/rviz_tasks_panel/package.xml similarity index 50% rename from robot/ros_ws/src/behavior/behavior_executive/package.xml rename to common/ros_packages/gui/rviz/rviz_tasks_panel/package.xml index d1ae9fce8..8c0c91c21 100644 --- a/robot/ros_ws/src/behavior/behavior_executive/package.xml +++ b/common/ros_packages/gui/rviz/rviz_tasks_panel/package.xml @@ -1,27 +1,28 @@ - behavior_executive + rviz_tasks_panel 0.0.0 - TODO: Package description - uav - TODO: License declaration + RViz2 Panel for dispatching and monitoring task action goals + andrew + MIT ament_cmake + pluginlib + rviz_common rclcpp rclcpp_action - std_msgs - nav_msgs task_msgs - behavior_tree - behavior_tree_msgs airstack_msgs - airstack_common - mavros_msgs - - ament_lint_auto - ament_lint_common + geometry_msgs + nav_msgs + std_msgs + diagnostic_msgs + action_msgs + rviz_polygon_selection_tool + waypoint_rviz2_plugin + qtbase5-dev ament_cmake diff --git a/common/ros_packages/gui/rviz/rviz_tasks_panel/plugin_description.xml b/common/ros_packages/gui/rviz/rviz_tasks_panel/plugin_description.xml new file mode 100644 index 000000000..8298c7dc1 --- /dev/null +++ b/common/ros_packages/gui/rviz/rviz_tasks_panel/plugin_description.xml @@ -0,0 +1,5 @@ + + + Panel for dispatching and monitoring ROS 2 task action goals + + diff --git a/common/ros_packages/gui/rviz/rviz_tasks_panel/src/tasks_panel.cpp b/common/ros_packages/gui/rviz/rviz_tasks_panel/src/tasks_panel.cpp new file mode 100644 index 000000000..aed06bc47 --- /dev/null +++ b/common/ros_packages/gui/rviz/rviz_tasks_panel/src/tasks_panel.cpp @@ -0,0 +1,1163 @@ +// Copyright (c) 2024 Carnegie Mellon University +// MIT License + +#include "rviz_tasks_panel/tasks_panel.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace rviz_tasks_panel +{ + +// ─────────────────────────── static task registry ───────────────────────────── + +std::vector TasksPanel::getTaskDefs() +{ + // default, min, max + return { + {"Takeoff", "tasks/takeoff", { + {"target_altitude_m", "float32", 10.0, 0.0, 500.0}, + {"velocity_m_s", "float32", 1.0, 0.0, 50.0}, + }}, + {"Land", "tasks/land", { + {"velocity_m_s", "float32", 0.3, 0.0, 10.0}, + }, true}, + {"Navigate", "tasks/navigate", { + {"global_plan", "nav_msgs/Path", 0, 0, 0}, + {"goal_tolerance_m", "float32", 1.0, 0.0, 100.0}, + }, true}, + {"Exploration", "tasks/exploration", { + {"search_bounds", "geometry_msgs/Polygon", 0, 0, 0}, + {"min_altitude_agl", "float32", 3.0, 0.0, 500.0}, + {"max_altitude_agl", "float32", 10.0, 0.0, 500.0}, + {"min_flight_speed", "float32", 1.0, 0.0, 50.0}, + {"max_flight_speed", "float32", 3.0, 0.0, 50.0}, + {"time_limit_sec", "float32", 120.0, 0.0, 86400.0}, + }, true}, + {"Coverage", "tasks/coverage", { + {"coverage_area", "geometry_msgs/Polygon", 0, 0, 0}, + {"min_altitude_agl", "float32", 3.0, 0.0, 500.0}, + {"max_altitude_agl", "float32", 10.0, 0.0, 500.0}, + {"min_flight_speed", "float32", 1.0, 0.0, 50.0}, + {"max_flight_speed", "float32", 3.0, 0.0, 50.0}, + {"line_spacing_m", "float32", 5.0, 0.1, 1000.0}, + {"heading_deg", "float32", 0.0, 0.0, 360.0}, + }, true}, + {"Object Search", "tasks/object_search", { + {"object_class", "string", 0, 0, 0}, + {"search_area", "geometry_msgs/Polygon", 0, 0, 0}, + {"min_altitude_agl", "float32", 3.0, 0.0, 500.0}, + {"max_altitude_agl", "float32", 10.0, 0.0, 500.0}, + {"min_flight_speed", "float32", 1.0, 0.0, 50.0}, + {"max_flight_speed", "float32", 3.0, 0.0, 50.0}, + {"time_limit_sec", "float32", 120.0, 0.0, 86400.0}, + {"target_count", "int32", 1, 0, 10000}, + }, true}, + {"Object Counting", "tasks/object_counting", { + {"object_class", "string", 0, 0, 0}, + {"count_area", "geometry_msgs/Polygon", 0, 0, 0}, + {"min_altitude_agl", "float32", 3.0, 0.0, 500.0}, + {"max_altitude_agl", "float32", 10.0, 0.0, 500.0}, + {"min_flight_speed", "float32", 1.0, 0.0, 50.0}, + {"max_flight_speed", "float32", 3.0, 0.0, 50.0}, + }, true}, + {"Semantic Search", "tasks/semantic_search", { + {"query", "string", 0, 0, 0}, + {"search_area", "geometry_msgs/Polygon", 0, 0, 0}, + {"min_altitude_agl", "float32", 3.0, 0.0, 500.0}, + {"max_altitude_agl", "float32", 10.0, 0.0, 500.0}, + {"min_flight_speed", "float32", 1.0, 0.0, 50.0}, + {"max_flight_speed", "float32", 3.0, 0.0, 50.0}, + {"time_limit_sec", "float32", 120.0, 0.0, 86400.0}, + {"confidence_threshold", "float32", 0.5, 0.0, 1.0}, + }, true}, + {"Fixed Trajectory", "tasks/fixed_trajectory", { + {"trajectory_spec", "airstack_msgs/FixedTrajectory", 0, 0, 0}, + {"loop", "bool", 0, 0, 0}, + }, true}, + }; +} + +// ─────────────────────────── constructor / init ─────────────────────────────── + +TasksPanel::TasksPanel(QWidget * parent) +: rviz_common::Panel(parent) +{ + task_defs_ = getTaskDefs(); +} + +void TasksPanel::onInitialize() +{ + auto node_abs = getDisplayContext()->getRosNodeAbstraction().lock(); + raw_node_ = node_abs->get_raw_node(); + + // Polygon selection service client + polygon_client_ = + raw_node_->create_client("get_selection"); + + // Shared waypoint manager (singleton created by WaypointTool) + waypoint_manager_ = waypoint_rviz2_plugin::WaypointManager::instance(); + if (!waypoint_manager_->isInitialized()) { + // WaypointTool hasn't initialized yet; try again shortly + QTimer::singleShot(1000, [this]() { + if (waypoint_manager_ && !waypoint_manager_->isInitialized()) { + waypoint_manager_->initialize(getDisplayContext(), + getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node()); + } + }); + } + + // Connect waypoint manager signals + connect(waypoint_manager_.get(), + &waypoint_rviz2_plugin::WaypointManager::waypointCountChanged, + this, &TasksPanel::onWaypointCountChanged, Qt::QueuedConnection); + connect(waypoint_manager_.get(), + &waypoint_rviz2_plugin::WaypointManager::selectedMarkerChanged, + this, &TasksPanel::onSelectedMarkerChanged, Qt::QueuedConnection); + connect(waypoint_manager_.get(), + &waypoint_rviz2_plugin::WaypointManager::waypointsCleared, + this, [this]() { + onWaypointCountChanged(0); + }, Qt::QueuedConnection); + + // Build UI + auto * main_layout = new QVBoxLayout(); + + // Robot selector bar + auto * top_bar = new QHBoxLayout(); + top_bar->addWidget(new QLabel("Robot:")); + robot_combo_ = new QComboBox(); + robot_combo_->setEditable(true); + robot_combo_->setMinimumWidth(120); + top_bar->addWidget(robot_combo_); + auto * refresh_btn = new QPushButton("Refresh"); + connect(refresh_btn, &QPushButton::clicked, this, &TasksPanel::onRefreshExecutors); + top_bar->addWidget(refresh_btn); + top_bar->addStretch(); + main_layout->addLayout(top_bar); + + // Tab widget + tab_widget_ = new QTabWidget(); + tab_widget_->setFont(QFont("Noto Sans", 10)); + main_layout->addWidget(tab_widget_); + + setLayout(main_layout); + + buildTabs(); + + // Discovery timer + refresh_timer_ = new QTimer(this); + connect(refresh_timer_, &QTimer::timeout, this, &TasksPanel::onRefreshExecutors); + refresh_timer_->start(5000); + + // Initial discovery + QTimer::singleShot(500, this, &TasksPanel::onRefreshExecutors); +} + +// ─────────────────────────── build tabs ─────────────────────────────────────── + +void TasksPanel::buildTabs() +{ + tab_states_.resize(task_defs_.size()); + + for (size_t i = 0; i < task_defs_.size(); ++i) { + const auto & def = task_defs_[i]; + auto & state = tab_states_[i]; + + auto * tab = new QWidget(); + auto * splitter = new QSplitter(Qt::Horizontal); + + // ── LEFT: Goal Parameters ── + auto * left_widget = new QWidget(); + auto * left_layout = new QVBoxLayout(left_widget); + + // Executor selector + auto * exec_layout = new QHBoxLayout(); + exec_layout->addWidget(new QLabel("Executor:")); + state.executor_combo = new QComboBox(); + state.executor_combo->setEditable(true); + state.executor_combo->setMinimumWidth(200); + exec_layout->addWidget(state.executor_combo); + left_layout->addLayout(exec_layout); + + // Goal fields + auto * form = new QFormLayout(); + for (const auto & field : def.goal_fields) { + auto * widget = buildGoalFieldWidget(field, static_cast(i)); + state.field_widgets[field.name] = widget; + form->addRow(QString::fromStdString(field.name) + ":", widget); + } + left_layout->addLayout(form); + + left_layout->addStretch(); + + // Cancel / Execute buttons + auto * btn_layout = new QHBoxLayout(); + state.cancel_btn = new QPushButton("Cancel"); + state.cancel_btn->setEnabled(false); + state.execute_btn = new QPushButton("Execute"); + btn_layout->addWidget(state.cancel_btn); + btn_layout->addStretch(); + btn_layout->addWidget(state.execute_btn); + left_layout->addLayout(btn_layout); + + int tab_idx = static_cast(i); + connect(state.execute_btn, &QPushButton::clicked, [this, tab_idx]() { + // store which tab triggered it, then call handler + tab_widget_->setCurrentIndex(tab_idx); + onExecuteClicked(); + }); + connect(state.cancel_btn, &QPushButton::clicked, [this, tab_idx]() { + tab_widget_->setCurrentIndex(tab_idx); + onCancelClicked(); + }); + + // ── RIGHT: Feedback & Result ── + auto * right_widget = new QWidget(); + auto * right_layout = new QVBoxLayout(right_widget); + + right_layout->addWidget(new QLabel("Feedback:")); + state.feedback_display = new QTextEdit(); + state.feedback_display->setReadOnly(true); + state.feedback_display->setMaximumHeight(200); + right_layout->addWidget(state.feedback_display); + + right_layout->addWidget(new QLabel("Result:")); + state.result_display = new QTextEdit(); + state.result_display->setReadOnly(true); + state.result_display->setMaximumHeight(100); + right_layout->addWidget(state.result_display); + + state.status_label = new QLabel("Idle"); + right_layout->addWidget(state.status_label); + right_layout->addStretch(); + + splitter->addWidget(left_widget); + splitter->addWidget(right_widget); + splitter->setStretchFactor(0, 1); + splitter->setStretchFactor(1, 1); + + auto * tab_layout = new QVBoxLayout(tab); + tab_layout->addWidget(splitter); + + tab_widget_->addTab(tab, QString::fromStdString(def.display_name)); + } +} + +QWidget * TasksPanel::buildGoalFieldWidget(const GoalFieldDef & def, int tab_index) +{ + if (def.ros_type == "float32" || def.ros_type == "float64") { + auto * spin = new QDoubleSpinBox(); + spin->setRange(def.min_value, def.max_value); + spin->setValue(def.default_value); + spin->setDecimals(2); + spin->setSingleStep(0.1); + return spin; + } + if (def.ros_type == "int32") { + auto * spin = new QSpinBox(); + spin->setRange(static_cast(def.min_value), static_cast(def.max_value)); + spin->setValue(static_cast(def.default_value)); + return spin; + } + if (def.ros_type == "string") { + return new QLineEdit(); + } + if (def.ros_type == "bool") { + auto * cb = new QCheckBox(); + cb->setChecked(def.default_value != 0.0); + return cb; + } + if (def.ros_type == "geometry_msgs/Polygon") { + auto * container = new QWidget(); + auto * layout = new QVBoxLayout(container); + layout->setContentsMargins(0, 0, 0, 0); + auto * btn = new QPushButton("Get Polygon from RViz"); + auto * label = new QLabel("No polygon selected"); + layout->addWidget(btn); + layout->addWidget(label); + std::string field_name = def.name; + connect(btn, &QPushButton::clicked, [this, tab_index, field_name]() { + onGetPolygon(tab_index, field_name); + }); + return container; + } + if (def.ros_type == "nav_msgs/Path") { + auto * container = new QWidget(); + auto * layout = new QVBoxLayout(container); + layout->setContentsMargins(0, 0, 0, 0); + + // Instruction label + layout->addWidget(new QLabel("Activate Waypoint Tool (key: 1) to place waypoints")); + + // Height / count / selected row + auto * info_row = new QHBoxLayout(); + info_row->addWidget(new QLabel("Height:")); + wp_height_spin_ = new QDoubleSpinBox(); + wp_height_spin_->setRange(-100.0, 500.0); + wp_height_spin_->setSingleStep(0.1); + wp_height_spin_->setValue(waypoint_manager_ ? waypoint_manager_->defaultHeight() : 0.0); + wp_height_spin_->setMaximumWidth(80); + info_row->addWidget(wp_height_spin_); + + wp_count_label_ = new QLabel("Waypoints: 0"); + info_row->addWidget(wp_count_label_); + + wp_selected_label_ = new QLabel("Selected: none"); + info_row->addWidget(wp_selected_label_); + info_row->addStretch(); + layout->addLayout(info_row); + + // X / Y / Z / Yaw spinboxes for editing selected waypoint + auto * pose_row = new QHBoxLayout(); + + pose_row->addWidget(new QLabel("X:")); + wp_x_spin_ = new QDoubleSpinBox(); + wp_x_spin_->setRange(-1000.0, 1000.0); + wp_x_spin_->setSingleStep(0.5); + pose_row->addWidget(wp_x_spin_); + + pose_row->addWidget(new QLabel("Y:")); + wp_y_spin_ = new QDoubleSpinBox(); + wp_y_spin_->setRange(-1000.0, 1000.0); + wp_y_spin_->setSingleStep(0.5); + pose_row->addWidget(wp_y_spin_); + + pose_row->addWidget(new QLabel("Z:")); + wp_z_spin_ = new QDoubleSpinBox(); + wp_z_spin_->setRange(-1000.0, 1000.0); + wp_z_spin_->setSingleStep(0.5); + pose_row->addWidget(wp_z_spin_); + + pose_row->addWidget(new QLabel("Yaw:")); + wp_yaw_spin_ = new QDoubleSpinBox(); + wp_yaw_spin_->setRange(-3.15, 3.15); + wp_yaw_spin_->setSingleStep(0.1); + pose_row->addWidget(wp_yaw_spin_); + layout->addLayout(pose_row); + + // Action buttons + auto * btn_row = new QHBoxLayout(); + auto * clear_btn = new QPushButton("Clear All"); + auto * save_btn = new QPushButton("Save"); + auto * load_btn = new QPushButton("Load"); + btn_row->addWidget(clear_btn); + btn_row->addWidget(save_btn); + btn_row->addWidget(load_btn); + btn_row->addStretch(); + layout->addLayout(btn_row); + + // Connect height spinbox to manager + connect(wp_height_spin_, QOverload::of(&QDoubleSpinBox::valueChanged), + [this](double val) { + if (waypoint_manager_) { waypoint_manager_->setDefaultHeight(val); } + }); + + // Connect pose spinboxes to update selected waypoint + auto pose_changed = [this](double) { onWaypointPoseChanged(0.0); }; + connect(wp_x_spin_, QOverload::of(&QDoubleSpinBox::valueChanged), pose_changed); + connect(wp_y_spin_, QOverload::of(&QDoubleSpinBox::valueChanged), pose_changed); + connect(wp_z_spin_, QOverload::of(&QDoubleSpinBox::valueChanged), pose_changed); + connect(wp_yaw_spin_, QOverload::of(&QDoubleSpinBox::valueChanged), pose_changed); + + // Connect buttons + connect(clear_btn, &QPushButton::clicked, this, &TasksPanel::onClearWaypoints); + connect(save_btn, &QPushButton::clicked, this, &TasksPanel::onSaveWaypoints); + connect(load_btn, &QPushButton::clicked, this, &TasksPanel::onLoadWaypoints); + + return container; + } + if (def.ros_type == "airstack_msgs/FixedTrajectory") { + auto * container = new QWidget(); + auto * layout = new QVBoxLayout(container); + layout->setContentsMargins(0, 0, 0, 0); + + auto * type_combo = new QComboBox(); + type_combo->addItems({"Circle", "Figure8", "Racetrack", "Line", "Point", "Lawnmower"}); + layout->addWidget(type_combo); + + auto * table = new QTableWidget(0, 2); + table->setHorizontalHeaderLabels({"Key", "Value"}); + table->horizontalHeader()->setStretchLastSection(true); + table->setMaximumHeight(150); + layout->addWidget(table); + + auto * add_btn = new QPushButton("+ Add Attribute"); + layout->addWidget(add_btn); + + connect(add_btn, &QPushButton::clicked, [table]() { + int row = table->rowCount(); + table->insertRow(row); + table->setItem(row, 0, new QTableWidgetItem("")); + table->setItem(row, 1, new QTableWidgetItem("")); + }); + + // Store combo and table for later retrieval + tab_states_[tab_index].field_data[def.name + "_type_combo"] = type_combo; + tab_states_[tab_index].field_data[def.name + "_table"] = table; + + connect(type_combo, &QComboBox::currentTextChanged, + [this, tab_index, field_name = def.name](const QString & type) { + onTrajectoryTypeChanged(tab_index, type); + }); + + // Trigger initial population + QTimer::singleShot(0, [type_combo]() { + Q_EMIT type_combo->currentTextChanged(type_combo->currentText()); + }); + + return container; + } + + return new QLabel("(unsupported type: " + QString::fromStdString(def.ros_type) + ")"); +} + +// ─────────────────────────── executor discovery ────────────────────────────── + +void TasksPanel::onRefreshExecutors() +{ + if (!raw_node_) {return;} + + auto topic_map = raw_node_->get_topic_names_and_types(); + + // Collect robot namespaces and per-task executors + std::set robots; + + for (size_t i = 0; i < task_defs_.size(); ++i) { + const std::string suffix = task_defs_[i].action_topic_suffix + "/_action/status"; + QStringList executors; + + for (const auto & [topic, types] : topic_map) { + if (topic.size() > suffix.size() && + topic.substr(topic.size() - suffix.size()) == suffix) + { + // Extract action name (remove /_action/status) + std::string action_name = topic.substr(0, topic.size() - std::string("/_action/status").size()); + executors.append(QString::fromStdString(action_name)); + + // Extract robot namespace (everything before /tasks/...) + std::string task_suffix = task_defs_[i].action_topic_suffix; + size_t pos = action_name.rfind(task_suffix); + if (pos != std::string::npos && pos > 1) { + robots.insert(action_name.substr(0, pos - 1)); // -1 to remove trailing / + } + } + } + + // Update executor combo preserving current selection + auto & combo = tab_states_[i].executor_combo; + QString current = combo->currentText(); + combo->clear(); + combo->addItems(executors); + if (!current.isEmpty()) { + int idx = combo->findText(current); + if (idx >= 0) { + combo->setCurrentIndex(idx); + } else { + combo->setCurrentText(current); + } + } + } + + // Update robot combo + QString current_robot = robot_combo_->currentText(); + robot_combo_->clear(); + for (const auto & r : robots) { + robot_combo_->addItem(QString::fromStdString(r)); + } + if (!current_robot.isEmpty()) { + int idx = robot_combo_->findText(current_robot); + if (idx >= 0) { + robot_combo_->setCurrentIndex(idx); + } else { + robot_combo_->setCurrentText(current_robot); + } + } + + renewAirborneSubscription(); +} + +// ─────────────────────────── special widgets ────────────────────────────────── + +void TasksPanel::onGetPolygon(int tab_index, const std::string & field_name) +{ + if (!polygon_client_->wait_for_service(std::chrono::seconds(0))) { + tab_states_[tab_index].status_label->setText("Polygon tool not active - press 'p' first"); + return; + } + + auto request = std::make_shared(); + polygon_client_->async_send_request(request, + [this, tab_index, field_name]( + rclcpp::Client::SharedFuture future) + { + auto response = future.get(); + if (response->selection.empty()) { + QMetaObject::invokeMethod(this, [this, tab_index]() { + tab_states_[tab_index].status_label->setText("No polygons drawn"); + }, Qt::QueuedConnection); + return; + } + + // Take first polygon + geometry_msgs::msg::Polygon poly; + for (const auto & pt : response->selection[0].polygon.points) { + geometry_msgs::msg::Point32 p; + p.x = pt.x; + p.y = pt.y; + p.z = pt.z; + poly.points.push_back(p); + } + + QMetaObject::invokeMethod(this, [this, tab_index, field_name, poly]() { + tab_states_[tab_index].field_data[field_name] = poly; + // Update label in the polygon widget + auto * container = tab_states_[tab_index].field_widgets[field_name]; + auto * label = container->findChild(); + if (label) { + label->setText(QString("Polygon: %1 points").arg(poly.points.size())); + } + }, Qt::QueuedConnection); + }); +} + +void TasksPanel::onTrajectoryTypeChanged(int tab_index, const QString & type) +{ + std::string key = "trajectory_spec_table"; + auto it = tab_states_[tab_index].field_data.find(key); + if (it == tab_states_[tab_index].field_data.end()) {return;} + + auto * table = std::any_cast(it->second); + table->setRowCount(0); + + // Default attributes per trajectory type + std::vector> attrs; + if (type == "Circle") { + attrs = {{"frame_id", "base_link"}, {"radius", "5.0"}, {"velocity", "2.0"}}; + } else if (type == "Figure8") { + attrs = {{"frame_id", "base_link"}, {"length", "10.0"}, {"width", "5.0"}, + {"height", "0.0"}, {"velocity", "2.0"}, {"max_acceleration", "1.0"}}; + } else if (type == "Racetrack") { + attrs = {{"frame_id", "base_link"}, {"length", "20.0"}, {"width", "10.0"}, + {"height", "0.0"}, {"velocity", "2.0"}, {"turn_velocity", "1.0"}, + {"max_acceleration", "1.0"}}; + } else if (type == "Line") { + attrs = {{"frame_id", "base_link"}, {"length", "10.0"}, {"height", "0.0"}, + {"velocity", "2.0"}, {"max_acceleration", "1.0"}}; + } else if (type == "Point") { + attrs = {{"frame_id", "base_link"}, {"x", "5.0"}, {"y", "0.0"}, + {"height", "0.0"}, {"velocity", "2.0"}, {"max_acceleration", "1.0"}}; + } else if (type == "Lawnmower") { + attrs = {{"frame_id", "base_link"}, {"length", "20.0"}, {"width", "5.0"}, + {"height", "10.0"}, {"velocity", "2.0"}, {"vertical", "0"}}; + } + + for (const auto & [k, v] : attrs) { + int row = table->rowCount(); + table->insertRow(row); + table->setItem(row, 0, new QTableWidgetItem(QString::fromStdString(k))); + table->setItem(row, 1, new QTableWidgetItem(QString::fromStdString(v))); + } +} + +// ─────────────────────────── value extraction ───────────────────────────────── + +double TasksPanel::getFloat(int tab_index, const std::string & field_name) +{ + auto * w = tab_states_[tab_index].field_widgets[field_name]; + if (auto * spin = qobject_cast(w)) {return spin->value();} + return 0.0; +} + +int32_t TasksPanel::getInt(int tab_index, const std::string & field_name) +{ + auto * w = tab_states_[tab_index].field_widgets[field_name]; + if (auto * spin = qobject_cast(w)) {return spin->value();} + return 0; +} + +std::string TasksPanel::getString(int tab_index, const std::string & field_name) +{ + auto * w = tab_states_[tab_index].field_widgets[field_name]; + if (auto * edit = qobject_cast(w)) {return edit->text().toStdString();} + return ""; +} + +bool TasksPanel::getBool(int tab_index, const std::string & field_name) +{ + auto * w = tab_states_[tab_index].field_widgets[field_name]; + if (auto * cb = qobject_cast(w)) {return cb->isChecked();} + return false; +} + +geometry_msgs::msg::Polygon TasksPanel::getPolygon(int tab_index, const std::string & field_name) +{ + auto it = tab_states_[tab_index].field_data.find(field_name); + if (it != tab_states_[tab_index].field_data.end()) { + return std::any_cast(it->second); + } + return geometry_msgs::msg::Polygon(); +} + +nav_msgs::msg::Path TasksPanel::getPath(int, const std::string &) +{ + if (waypoint_manager_ && waypoint_manager_->isInitialized()) { + return waypoint_manager_->getPath(); + } + return nav_msgs::msg::Path(); +} + +airstack_msgs::msg::FixedTrajectory TasksPanel::getFixedTrajectory( + int tab_index, const std::string & field_name) +{ + airstack_msgs::msg::FixedTrajectory ft; + + auto combo_it = tab_states_[tab_index].field_data.find(field_name + "_type_combo"); + if (combo_it != tab_states_[tab_index].field_data.end()) { + auto * combo = std::any_cast(combo_it->second); + ft.type = combo->currentText().toStdString(); + } + + auto table_it = tab_states_[tab_index].field_data.find(field_name + "_table"); + if (table_it != tab_states_[tab_index].field_data.end()) { + auto * table = std::any_cast(table_it->second); + for (int row = 0; row < table->rowCount(); ++row) { + diagnostic_msgs::msg::KeyValue kv; + kv.key = table->item(row, 0)->text().toStdString(); + kv.value = table->item(row, 1)->text().toStdString(); + ft.attributes.push_back(kv); + } + } + + return ft; +} + +// ─────────────────────────── UI state helpers ───────────────────────────────── + +void TasksPanel::setGoalActive(int tab_index, bool active) +{ + auto & state = tab_states_[tab_index]; + state.goal_active = active; + state.cancel_btn->setEnabled(active); + + if (active) { + active_task_tab_ = tab_index; + state.feedback_display->clear(); + state.result_display->clear(); + state.status_label->setText("Running..."); + state.status_label->setStyleSheet("color: blue;"); + setTabStatus(tab_index, QString::fromUtf8(u8"⏳"), Qt::blue); + } else { + active_task_tab_ = -1; + } + + updateExecuteButtons(); +} + +void TasksPanel::updateExecuteButtons() +{ + for (size_t i = 0; i < tab_states_.size(); ++i) { + bool task_idle = (active_task_tab_ == -1); + bool airborne_ok = !task_defs_[i].requires_airborne || is_airborne_; + tab_states_[i].execute_btn->setEnabled(task_idle && airborne_ok); + } +} + +void TasksPanel::renewAirborneSubscription() +{ + QString robot = robot_combo_->currentText(); + if (robot == is_airborne_robot_) {return;} + is_airborne_robot_ = robot; + + is_airborne_sub_.reset(); + is_airborne_ = false; + updateExecuteButtons(); + + if (robot.isEmpty() || !raw_node_) {return;} + + std::string topic = robot.toStdString() + "/takeoff_landing_planner/is_airborne"; + is_airborne_sub_ = raw_node_->create_subscription( + topic, 1, + [this](std_msgs::msg::Bool::SharedPtr msg) { + bool airborne = msg->data; + QMetaObject::invokeMethod(this, [this, airborne]() { + is_airborne_ = airborne; + updateExecuteButtons(); + }, Qt::QueuedConnection); + }); +} + +void TasksPanel::setTabStatus(int tab_index, const QString & icon, const QColor & text_color) +{ + tab_widget_->tabBar()->setTabTextColor(tab_index, text_color); + const auto & def = task_defs_[tab_index]; + tab_widget_->setTabText(tab_index, icon + " " + QString::fromStdString(def.display_name)); +} + +void TasksPanel::clearTabStatus(int tab_index) +{ + tab_widget_->tabBar()->setTabTextColor(tab_index, QColor()); + const auto & def = task_defs_[tab_index]; + tab_widget_->setTabText(tab_index, QString::fromStdString(def.display_name)); +} + +QString TasksPanel::currentRobot() const +{ + return robot_combo_->currentText(); +} + +// ─────────────────────────── template: send/cancel ──────────────────────────── + +template +void TasksPanel::doSendGoal( + int tab_index, + const typename ActionT::Goal & goal, + std::function fmt_feedback, + std::function fmt_result) +{ + auto & state = tab_states_[tab_index]; + std::string action_name = state.executor_combo->currentText().toStdString(); + if (action_name.empty()) { + state.status_label->setText("No executor selected"); + state.status_label->setStyleSheet("color: red;"); + return; + } + + auto client = rclcpp_action::create_client(raw_node_, action_name); + state.action_client = client; + + if (!client->wait_for_action_server(std::chrono::seconds(2))) { + state.status_label->setText("Action server not available"); + state.status_label->setStyleSheet("color: red;"); + return; + } + + // Reset all tab colors from previous task results + for (size_t i = 0; i < tab_states_.size(); ++i) { + clearTabStatus(static_cast(i)); + } + setGoalActive(tab_index, true); + + auto send_goal_options = typename rclcpp_action::Client::SendGoalOptions(); + + // Feedback callback + send_goal_options.feedback_callback = + [this, tab_index, fmt_feedback]( + typename rclcpp_action::ClientGoalHandle::SharedPtr, + const std::shared_ptr feedback) + { + QString text = fmt_feedback(*feedback); + QMetaObject::invokeMethod(this, [this, tab_index, text]() { + auto & state = tab_states_[tab_index]; + state.feedback_display->append( + "[" + QTime::currentTime().toString("HH:mm:ss") + "] " + text); + // Auto-scroll to bottom + auto cursor = state.feedback_display->textCursor(); + cursor.movePosition(QTextCursor::End); + state.feedback_display->setTextCursor(cursor); + }, Qt::QueuedConnection); + }; + + // Result callback + send_goal_options.result_callback = + [this, tab_index, fmt_result]( + const typename rclcpp_action::ClientGoalHandle::WrappedResult & wrapped_result) + { + QString result_text; + QString status_text; + QString color; + QString tab_icon; + QColor tab_text_color; + + switch (wrapped_result.code) { + case rclcpp_action::ResultCode::SUCCEEDED: + result_text = fmt_result(wrapped_result.result); + status_text = "Succeeded"; + color = "color: darkGreen;"; + tab_icon = QString::fromUtf8(u8"✅"); + tab_text_color = Qt::darkGreen; + break; + case rclcpp_action::ResultCode::ABORTED: + result_text = fmt_result(wrapped_result.result); + status_text = "Aborted"; + color = "color: red;"; + tab_icon = QString::fromUtf8(u8"❌"); + tab_text_color = Qt::red; + break; + case rclcpp_action::ResultCode::CANCELED: + result_text = "Goal canceled"; + status_text = "Canceled"; + color = "color: orange;"; + tab_icon = QString::fromUtf8(u8"🚫"); + tab_text_color = QColor("orange"); + break; + default: + result_text = "Unknown result"; + status_text = "Unknown"; + color = "color: gray;"; + tab_icon = "?"; + tab_text_color = Qt::gray; + break; + } + + QMetaObject::invokeMethod(this, [this, tab_index, result_text, status_text, color, + tab_icon, tab_text_color]() { + auto & state = tab_states_[tab_index]; + state.result_display->setText(result_text); + state.status_label->setText(status_text); + state.status_label->setStyleSheet(color); + setTabStatus(tab_index, tab_icon, tab_text_color); + setGoalActive(tab_index, false); + }, Qt::QueuedConnection); + }; + + auto goal_future = client->async_send_goal(goal, send_goal_options); +} + +template +void TasksPanel::doCancelGoal(int tab_index) +{ + auto & state = tab_states_[tab_index]; + try { + auto client = std::any_cast::SharedPtr>( + state.action_client); + client->async_cancel_all_goals(); + state.status_label->setText("Cancelling..."); + state.status_label->setStyleSheet("color: orange;"); + } catch (const std::bad_any_cast &) { + state.status_label->setText("No active goal to cancel"); + } +} + +// ─────────────────────────── execute dispatch ───────────────────────────────── + +void TasksPanel::onExecuteClicked() +{ + int idx = tab_widget_->currentIndex(); + if (idx < 0 || idx >= static_cast(tab_states_.size())) {return;} + if (active_task_tab_ >= 0) {return;} // only one task at a time + if (task_defs_[idx].requires_airborne && !is_airborne_) {return;} // airborne required + + switch (idx) { + case 0: { // Takeoff + task_msgs::action::TakeoffTask::Goal goal; + goal.target_altitude_m = getFloat(0, "target_altitude_m"); + goal.velocity_m_s = getFloat(0, "velocity_m_s"); + doSendGoal(0, goal, + [](const auto & fb) { + return QString("status: %1 | alt: %2 / %3 m") + .arg(QString::fromStdString(fb.status)) + .arg(fb.current_altitude_m, 0, 'f', 1) + .arg(fb.target_altitude_m, 0, 'f', 1); + }, + [](const auto & r) { + return QString("success: %1\nmessage: %2") + .arg(r->success ? "true" : "false") + .arg(QString::fromStdString(r->message)); + }); + break; + } + case 1: { // Land + task_msgs::action::LandTask::Goal goal; + goal.velocity_m_s = getFloat(1, "velocity_m_s"); + doSendGoal(1, goal, + [](const auto & fb) { + return QString("status: %1 | alt: %2 m") + .arg(QString::fromStdString(fb.status)) + .arg(fb.current_altitude_m, 0, 'f', 1); + }, + [](const auto & r) { + return QString("success: %1\nmessage: %2") + .arg(r->success ? "true" : "false") + .arg(QString::fromStdString(r->message)); + }); + break; + } + case 2: { // Navigate + task_msgs::action::NavigateTask::Goal goal; + goal.global_plan = getPath(2, "global_plan"); + goal.goal_tolerance_m = getFloat(2, "goal_tolerance_m"); + doSendGoal(2, goal, + [](const auto & fb) { + return QString("status: %1 | dist: %2 m | pos: (%3, %4, %5)") + .arg(QString::fromStdString(fb.status)) + .arg(fb.distance_to_goal, 0, 'f', 1) + .arg(fb.current_position.x, 0, 'f', 1) + .arg(fb.current_position.y, 0, 'f', 1) + .arg(fb.current_position.z, 0, 'f', 1); + }, + [](const auto & r) { + return QString("success: %1\nmessage: %2") + .arg(r->success ? "true" : "false") + .arg(QString::fromStdString(r->message)); + }); + break; + } + case 3: { // Exploration + task_msgs::action::ExplorationTask::Goal goal; + goal.search_bounds = getPolygon(3, "search_bounds"); + goal.min_altitude_agl = getFloat(3, "min_altitude_agl"); + goal.max_altitude_agl = getFloat(3, "max_altitude_agl"); + goal.min_flight_speed = getFloat(3, "min_flight_speed"); + goal.max_flight_speed = getFloat(3, "max_flight_speed"); + goal.time_limit_sec = getFloat(3, "time_limit_sec"); + doSendGoal(3, goal, + [](const auto & fb) { + return QString("status: %1 | progress: %2 | pos: (%3, %4, %5)") + .arg(QString::fromStdString(fb.status)) + .arg(fb.progress, 0, 'f', 2) + .arg(fb.current_position.x, 0, 'f', 1) + .arg(fb.current_position.y, 0, 'f', 1) + .arg(fb.current_position.z, 0, 'f', 1); + }, + [](const auto & r) { + return QString("success: %1\nmessage: %2") + .arg(r->success ? "true" : "false") + .arg(QString::fromStdString(r->message)); + }); + break; + } + case 4: { // Coverage + task_msgs::action::CoverageTask::Goal goal; + goal.coverage_area = getPolygon(4, "coverage_area"); + goal.min_altitude_agl = getFloat(4, "min_altitude_agl"); + goal.max_altitude_agl = getFloat(4, "max_altitude_agl"); + goal.min_flight_speed = getFloat(4, "min_flight_speed"); + goal.max_flight_speed = getFloat(4, "max_flight_speed"); + goal.line_spacing_m = getFloat(4, "line_spacing_m"); + goal.heading_deg = getFloat(4, "heading_deg"); + doSendGoal(4, goal, + [](const auto & fb) { + return QString("status: %1 | progress: %2 | coverage: %3% | pos: (%4, %5, %6)") + .arg(QString::fromStdString(fb.status)) + .arg(fb.progress, 0, 'f', 2) + .arg(fb.coverage_percentage, 0, 'f', 1) + .arg(fb.current_position.x, 0, 'f', 1) + .arg(fb.current_position.y, 0, 'f', 1) + .arg(fb.current_position.z, 0, 'f', 1); + }, + [](const auto & r) { + return QString("success: %1\nmessage: %2\ncoverage: %3%") + .arg(r->success ? "true" : "false") + .arg(QString::fromStdString(r->message)) + .arg(r->coverage_percentage, 0, 'f', 1); + }); + break; + } + case 5: { // Object Search + task_msgs::action::ObjectSearchTask::Goal goal; + goal.object_class = getString(5, "object_class"); + goal.search_area = getPolygon(5, "search_area"); + goal.min_altitude_agl = getFloat(5, "min_altitude_agl"); + goal.max_altitude_agl = getFloat(5, "max_altitude_agl"); + goal.min_flight_speed = getFloat(5, "min_flight_speed"); + goal.max_flight_speed = getFloat(5, "max_flight_speed"); + goal.time_limit_sec = getFloat(5, "time_limit_sec"); + goal.target_count = getInt(5, "target_count"); + doSendGoal(5, goal, + [](const auto & fb) { + return QString("status: %1 | progress: %2 | found: %3 | pos: (%4, %5, %6)") + .arg(QString::fromStdString(fb.status)) + .arg(fb.progress, 0, 'f', 2) + .arg(fb.objects_found_so_far) + .arg(fb.current_position.x, 0, 'f', 1) + .arg(fb.current_position.y, 0, 'f', 1) + .arg(fb.current_position.z, 0, 'f', 1); + }, + [](const auto & r) { + return QString("success: %1\nmessage: %2\nobjects_found: %3") + .arg(r->success ? "true" : "false") + .arg(QString::fromStdString(r->message)) + .arg(r->objects_found); + }); + break; + } + case 6: { // Object Counting + task_msgs::action::ObjectCountingTask::Goal goal; + goal.object_class = getString(6, "object_class"); + goal.count_area = getPolygon(6, "count_area"); + goal.min_altitude_agl = getFloat(6, "min_altitude_agl"); + goal.max_altitude_agl = getFloat(6, "max_altitude_agl"); + goal.min_flight_speed = getFloat(6, "min_flight_speed"); + goal.max_flight_speed = getFloat(6, "max_flight_speed"); + doSendGoal(6, goal, + [](const auto & fb) { + return QString("status: %1 | progress: %2 | count: %3 | pos: (%4, %5, %6)") + .arg(QString::fromStdString(fb.status)) + .arg(fb.progress, 0, 'f', 2) + .arg(fb.current_count) + .arg(fb.current_position.x, 0, 'f', 1) + .arg(fb.current_position.y, 0, 'f', 1) + .arg(fb.current_position.z, 0, 'f', 1); + }, + [](const auto & r) { + return QString("success: %1\nmessage: %2\ncount: %3") + .arg(r->success ? "true" : "false") + .arg(QString::fromStdString(r->message)) + .arg(r->count); + }); + break; + } + case 7: { // Semantic Search + task_msgs::action::SemanticSearchTask::Goal goal; + goal.query = getString(7, "query"); + goal.search_area = getPolygon(7, "search_area"); + goal.min_altitude_agl = getFloat(7, "min_altitude_agl"); + goal.max_altitude_agl = getFloat(7, "max_altitude_agl"); + goal.min_flight_speed = getFloat(7, "min_flight_speed"); + goal.max_flight_speed = getFloat(7, "max_flight_speed"); + goal.time_limit_sec = getFloat(7, "time_limit_sec"); + goal.confidence_threshold = getFloat(7, "confidence_threshold"); + doSendGoal(7, goal, + [](const auto & fb) { + return QString("status: %1 | progress: %2 | best_conf: %3 | pos: (%4, %5, %6)") + .arg(QString::fromStdString(fb.status)) + .arg(fb.progress, 0, 'f', 2) + .arg(fb.best_confidence_so_far, 0, 'f', 3) + .arg(fb.current_position.x, 0, 'f', 1) + .arg(fb.current_position.y, 0, 'f', 1) + .arg(fb.current_position.z, 0, 'f', 1); + }, + [](const auto & r) { + return QString("success: %1\nmessage: %2\nconfidence: %3") + .arg(r->success ? "true" : "false") + .arg(QString::fromStdString(r->message)) + .arg(r->confidence, 0, 'f', 3); + }); + break; + } + case 8: { // Fixed Trajectory + task_msgs::action::FixedTrajectoryTask::Goal goal; + goal.trajectory_spec = getFixedTrajectory(8, "trajectory_spec"); + goal.loop = getBool(8, "loop"); + doSendGoal(8, goal, + [](const auto & fb) { + return QString("status: %1 | progress: %2 | pos: (%3, %4, %5)") + .arg(QString::fromStdString(fb.status)) + .arg(fb.progress, 0, 'f', 2) + .arg(fb.current_position.x, 0, 'f', 1) + .arg(fb.current_position.y, 0, 'f', 1) + .arg(fb.current_position.z, 0, 'f', 1); + }, + [](const auto & r) { + return QString("success: %1\nmessage: %2") + .arg(r->success ? "true" : "false") + .arg(QString::fromStdString(r->message)); + }); + break; + } + } +} + +// ─────────────────────────── cancel dispatch ────────────────────────────────── + +void TasksPanel::onCancelClicked() +{ + int idx = tab_widget_->currentIndex(); + if (idx < 0 || idx >= static_cast(tab_states_.size())) {return;} + if (!tab_states_[idx].goal_active) {return;} + + switch (idx) { + case 0: doCancelGoal(0); break; + case 1: doCancelGoal(1); break; + case 2: doCancelGoal(2); break; + case 3: doCancelGoal(3); break; + case 4: doCancelGoal(4); break; + case 5: doCancelGoal(5); break; + case 6: doCancelGoal(6); break; + case 7: doCancelGoal(7); break; + case 8: doCancelGoal(8); break; + } +} + +// ─────────────────────────── config persistence ────────────────────────────── + +void TasksPanel::save(rviz_common::Config config) const +{ + rviz_common::Panel::save(config); + config.mapSetValue("robot", robot_combo_->currentText()); + for (size_t i = 0; i < tab_states_.size(); ++i) { + config.mapSetValue( + QString("executor_%1").arg(i), + tab_states_[i].executor_combo->currentText()); + } +} + +void TasksPanel::load(const rviz_common::Config & config) +{ + rviz_common::Panel::load(config); + QString robot; + if (config.mapGetString("robot", &robot)) { + robot_combo_->setCurrentText(robot); + } + for (size_t i = 0; i < tab_states_.size(); ++i) { + QString executor; + if (config.mapGetString(QString("executor_%1").arg(i), &executor)) { + tab_states_[i].executor_combo->setCurrentText(executor); + } + } +} + +// ─────────────────────────── waypoint slots ────────────────────────────────── + +void TasksPanel::onWaypointCountChanged(int count) +{ + if (wp_count_label_) { + wp_count_label_->setText(QString("Waypoints: %1").arg(count)); + } +} + +void TasksPanel::onSelectedMarkerChanged( + const QString & name, double x, double y, double z, double yaw) +{ + if (wp_selected_label_) { + wp_selected_label_->setText(QString("Selected: %1").arg(name)); + } + + // Update spinboxes without triggering poseChanged + wp_updating_spinboxes_ = true; + if (wp_x_spin_) { wp_x_spin_->setValue(x); } + if (wp_y_spin_) { wp_y_spin_->setValue(y); } + if (wp_z_spin_) { wp_z_spin_->setValue(z); } + if (wp_yaw_spin_) { wp_yaw_spin_->setValue(yaw); } + wp_updating_spinboxes_ = false; +} + +void TasksPanel::onClearWaypoints() +{ + if (waypoint_manager_) { waypoint_manager_->clearAll(); } +} + +void TasksPanel::onSaveWaypoints() +{ + QString filename = QFileDialog::getSaveFileName( + this, tr("Save Waypoints"), "waypoints", tr("Bag Files (*.db3)")); + if (filename.isEmpty()) { return; } + if (waypoint_manager_) { waypoint_manager_->saveBag(filename.toStdString()); } +} + +void TasksPanel::onLoadWaypoints() +{ + QString filename = QFileDialog::getOpenFileName( + this, tr("Load Waypoints"), "~/", tr("Bag Files (*.db3)")); + if (filename.isEmpty()) { return; } + if (waypoint_manager_) { waypoint_manager_->loadBag(filename.toStdString()); } +} + +void TasksPanel::onWaypointPoseChanged(double) +{ + if (wp_updating_spinboxes_ || !waypoint_manager_) { return; } + waypoint_manager_->updateSelectedPose( + wp_x_spin_->value(), wp_y_spin_->value(), + wp_z_spin_->value(), wp_yaw_spin_->value()); +} + +} // namespace rviz_tasks_panel + +PLUGINLIB_EXPORT_CLASS(rviz_tasks_panel::TasksPanel, rviz_common::Panel) diff --git a/common/ros_packages/msgs/task_msgs/CMakeLists.txt b/common/ros_packages/msgs/task_msgs/CMakeLists.txt index add09d32e..852f3e2b7 100644 --- a/common/ros_packages/msgs/task_msgs/CMakeLists.txt +++ b/common/ros_packages/msgs/task_msgs/CMakeLists.txt @@ -16,10 +16,12 @@ rosidl_generate_interfaces(${PROJECT_NAME} "action/CoverageTask.action" "action/ExplorationTask.action" "action/FixedTrajectoryTask.action" + "action/LandTask.action" "action/NavigateTask.action" "action/ObjectCountingTask.action" "action/ObjectSearchTask.action" "action/SemanticSearchTask.action" + "action/TakeoffTask.action" DEPENDENCIES action_msgs geometry_msgs nav_msgs airstack_msgs ) diff --git a/common/ros_packages/msgs/task_msgs/action/FixedTrajectoryTask.action b/common/ros_packages/msgs/task_msgs/action/FixedTrajectoryTask.action index d8abff82b..72dfce8cb 100644 --- a/common/ros_packages/msgs/task_msgs/action/FixedTrajectoryTask.action +++ b/common/ros_packages/msgs/task_msgs/action/FixedTrajectoryTask.action @@ -1,8 +1,8 @@ -# Follow a pre-defined trajectory. +# Follow a fixed trajectory specified by type and parameters. # loop: if true, repeat the trajectory indefinitely until cancelled # Goal -airstack_msgs/TrajectoryXYZVYaw trajectory +airstack_msgs/FixedTrajectory trajectory_spec bool loop --- # Result @@ -12,6 +12,4 @@ string message # Feedback string status float32 progress -int32 current_waypoint_index -int32 total_waypoints geometry_msgs/Point current_position diff --git a/common/ros_packages/msgs/task_msgs/action/LandTask.action b/common/ros_packages/msgs/task_msgs/action/LandTask.action new file mode 100644 index 000000000..4e81d5fcd --- /dev/null +++ b/common/ros_packages/msgs/task_msgs/action/LandTask.action @@ -0,0 +1,12 @@ +# Land the robot. + +# Goal +float32 velocity_m_s # 0.0 = use default from config +--- +# Result +bool success +string message +--- +# Feedback +string status +float32 current_altitude_m diff --git a/common/ros_packages/msgs/task_msgs/action/TakeoffTask.action b/common/ros_packages/msgs/task_msgs/action/TakeoffTask.action new file mode 100644 index 000000000..6d78d4daf --- /dev/null +++ b/common/ros_packages/msgs/task_msgs/action/TakeoffTask.action @@ -0,0 +1,15 @@ +# Takeoff to a target altitude. +# Rejects if not armed, no offboard control, or state estimate timed out. + +# Goal +float32 target_altitude_m +float32 velocity_m_s +--- +# Result +bool success +string message +--- +# Feedback +string status +float32 current_altitude_m +float32 target_altitude_m diff --git a/robot/ros_ws/src/behavior/behavior_bringup/launch/behavior.launch.xml b/robot/ros_ws/src/behavior/behavior_bringup/launch/behavior.launch.xml index 7e14ec054..cef093702 100644 --- a/robot/ros_ws/src/behavior/behavior_bringup/launch/behavior.launch.xml +++ b/robot/ros_ws/src/behavior/behavior_bringup/launch/behavior.launch.xml @@ -2,35 +2,14 @@ - - - - + + + + + - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/robot/ros_ws/src/behavior/behavior_executive/CMakeLists.txt b/robot/ros_ws/src/behavior/behavior_executive/CMakeLists.txt deleted file mode 100644 index b84f62f77..000000000 --- a/robot/ros_ws/src/behavior/behavior_executive/CMakeLists.txt +++ /dev/null @@ -1,54 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(behavior_executive) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# find dependencies -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_action REQUIRED) -find_package(std_msgs REQUIRED) -find_package(nav_msgs REQUIRED) -find_package(task_msgs REQUIRED) -find_package(behavior_tree REQUIRED) -find_package(behavior_tree_msgs REQUIRED) -find_package(airstack_msgs REQUIRED) -find_package(airstack_common REQUIRED) -find_package(mavros_msgs REQUIRED) - -add_executable(behavior_executive src/behavior_executive.cpp) -target_include_directories(behavior_executive PUBLIC - $ - $) -target_compile_features(behavior_executive PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 -ament_target_dependencies(behavior_executive - rclcpp - rclcpp_action - std_msgs - nav_msgs - task_msgs - behavior_tree - behavior_tree_msgs - airstack_msgs - airstack_common - mavros_msgs -) - -install(TARGETS behavior_executive - DESTINATION lib/${PROJECT_NAME}) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() - -ament_package() diff --git a/robot/ros_ws/src/behavior/behavior_executive/include/behavior_executive/behavior_executive.hpp b/robot/ros_ws/src/behavior/behavior_executive/include/behavior_executive/behavior_executive.hpp deleted file mode 100644 index b71896cf7..000000000 --- a/robot/ros_ws/src/behavior/behavior_executive/include/behavior_executive/behavior_executive.hpp +++ /dev/null @@ -1,120 +0,0 @@ -// Copyright (c) 2024 Carnegie Mellon University -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "rclcpp_action/rclcpp_action.hpp" -#include "task_msgs/action/exploration_task.hpp" - -class BehaviorExecutive : public rclcpp::Node { - private: - // parameters - bool ascent_takeoff; - - // variables - std::string takeoff_state, landing_state; - - // Condition variables - bt::Condition* auto_takeoff_commanded_condition; - bt::Condition* takeoff_commanded_condition; - bt::Condition* armed_condition; - bt::Condition* offboard_mode_condition; - bt::Condition* stationary_condition; - bt::Condition* land_commanded_condition; - bt::Condition* pause_commanded_condition; - bt::Condition* rewind_commanded_condition; - bt::Condition* fixed_trajectory_condition; - bt::Condition* global_plan_condition; - bt::Condition* offboard_commanded_condition; - bt::Condition* arm_commanded_condition; - bt::Condition* disarm_commanded_condition; - bt::Condition* takeoff_complete_condition; - bt::Condition* landing_complete_condition; - bt::Condition* in_air_condition; - bt::Condition* state_estimate_timed_out_condition; - bt::Condition* stuck_condition; - bt::Condition* autonomously_explore_condition; - std::vector conditions; - - // Action variables - bt::Action* arm_action; - bt::Action* takeoff_action; - bt::Action* land_action; - bt::Action* pause_action; - bt::Action* rewind_action; - bt::Action* follow_fixed_trajectory_action; - bt::Action* global_plan_action; - bt::Action* request_control_action; - bt::Action* disarm_action; - std::vector actions; - - // subscribers - rclcpp::Subscription::SharedPtr - behavior_tree_commands_sub; - rclcpp::Subscription::SharedPtr is_armed_sub; - rclcpp::Subscription::SharedPtr has_control_sub; - rclcpp::Subscription::SharedPtr takeoff_state_sub; - rclcpp::Subscription::SharedPtr landing_state_sub; - rclcpp::Subscription::SharedPtr state_estimate_timed_out_sub; - rclcpp::Subscription::SharedPtr stuck_sub; - - // publishers - rclcpp::Publisher::SharedPtr recording_pub; - rclcpp::Publisher::SharedPtr reset_stuck_pub; - rclcpp::Publisher::SharedPtr clear_map_pub; - - // services - rclcpp::CallbackGroup::SharedPtr service_callback_group; - rclcpp::Client::SharedPtr robot_command_client; - rclcpp::Client::SharedPtr trajectory_mode_client; - rclcpp::Client::SharedPtr - takeoff_landing_command_client; - - // action clients - using ExplorationTask = task_msgs::action::ExplorationTask; - rclcpp_action::Client::SharedPtr exploration_client_; - rclcpp_action::ClientGoalHandle::SharedPtr exploration_goal_handle_; - bool exploration_goal_done_ = true; - bool exploration_goal_succeeded_ = false; - - // timers - rclcpp::TimerBase::SharedPtr timer; - - // callbacks - void timer_callback(); - void bt_commands_callback(behavior_tree_msgs::msg::BehaviorTreeCommands msg); - void is_armed_callback(const std_msgs::msg::Bool::SharedPtr msg); - void has_control_callback(const std_msgs::msg::Bool::SharedPtr msg); - void takeoff_state_callback(const std_msgs::msg::String::SharedPtr msg); - void landing_state_callback(const std_msgs::msg::String::SharedPtr msg); - void state_estimate_timed_out_callback(const std_msgs::msg::Bool::SharedPtr msg); - void stuck_callback(const std_msgs::msg::Bool::SharedPtr msg); - - public: - BehaviorExecutive(); -}; diff --git a/robot/ros_ws/src/behavior/behavior_executive/src/behavior_executive.cpp b/robot/ros_ws/src/behavior/behavior_executive/src/behavior_executive.cpp deleted file mode 100644 index 9dc0900ba..000000000 --- a/robot/ros_ws/src/behavior/behavior_executive/src/behavior_executive.cpp +++ /dev/null @@ -1,467 +0,0 @@ -// Copyright (c) 2024 Carnegie Mellon University -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include - -BehaviorExecutive::BehaviorExecutive() : Node("behavior_executive") -{ - // conditions - auto_takeoff_commanded_condition = new bt::Condition("Auto Takeoff Commanded", this); - takeoff_commanded_condition = new bt::Condition("Takeoff Commanded", this); - armed_condition = new bt::Condition("Armed", this); - offboard_mode_condition = new bt::Condition("Offboard Mode", this); - stationary_condition = new bt::Condition("Stationary", this); - land_commanded_condition = new bt::Condition("Land Commanded", this); - pause_commanded_condition = new bt::Condition("Pause Commanded", this); - rewind_commanded_condition = new bt::Condition("Rewind Commanded", this); - fixed_trajectory_condition = new bt::Condition("Fixed Trajectory Commanded", this); - global_plan_condition = new bt::Condition("Global Plan Commanded", this); - offboard_commanded_condition = new bt::Condition("Offboard Commanded", this); - arm_commanded_condition = new bt::Condition("Arm Commanded", this); - disarm_commanded_condition = new bt::Condition("Disarm Commanded", this); - takeoff_complete_condition = new bt::Condition("Takeoff Complete", this); - landing_complete_condition = new bt::Condition("Landing Complete", this); - in_air_condition = new bt::Condition("In Air", this); - state_estimate_timed_out_condition = new bt::Condition("State Estimate Timed Out", this); - stuck_condition = new bt::Condition("Stuck", this); - autonomously_explore_condition = new bt::Condition("Autonomously Explore Commanded", this); - conditions.push_back(auto_takeoff_commanded_condition); - conditions.push_back(takeoff_commanded_condition); - conditions.push_back(armed_condition); - conditions.push_back(offboard_mode_condition); - conditions.push_back(stationary_condition); - conditions.push_back(land_commanded_condition); - conditions.push_back(pause_commanded_condition); - conditions.push_back(rewind_commanded_condition); - conditions.push_back(fixed_trajectory_condition); - conditions.push_back(global_plan_condition); - conditions.push_back(offboard_commanded_condition); - conditions.push_back(arm_commanded_condition); - conditions.push_back(disarm_commanded_condition); - conditions.push_back(takeoff_complete_condition); - conditions.push_back(landing_complete_condition); - conditions.push_back(in_air_condition); - conditions.push_back(state_estimate_timed_out_condition); - conditions.push_back(stuck_condition); - conditions.push_back(autonomously_explore_condition); - - // actions - arm_action = new bt::Action("Arm", this); - takeoff_action = new bt::Action("Takeoff", this); - land_action = new bt::Action("Land", this); - pause_action = new bt::Action("Pause", this); - rewind_action = new bt::Action("Rewind", this); - follow_fixed_trajectory_action = new bt::Action("Follow Fixed Trajectory", this); - global_plan_action = new bt::Action("Follow Global Plan", this); - request_control_action = new bt::Action("Request Control", this); - disarm_action = new bt::Action("Disarm", this); - actions.push_back(arm_action); - actions.push_back(takeoff_action); - actions.push_back(land_action); - actions.push_back(pause_action); - actions.push_back(rewind_action); - actions.push_back(follow_fixed_trajectory_action); - actions.push_back(global_plan_action); - actions.push_back(request_control_action); - actions.push_back(disarm_action); - - // subscribers - behavior_tree_commands_sub = - this->create_subscription( - "behavior_tree_commands", 1, - std::bind(&BehaviorExecutive::bt_commands_callback, this, std::placeholders::_1)); - is_armed_sub = this->create_subscription( - "is_armed", 1, - std::bind(&BehaviorExecutive::is_armed_callback, this, std::placeholders::_1)); - has_control_sub = this->create_subscription( - "has_control", 1, - std::bind(&BehaviorExecutive::has_control_callback, this, std::placeholders::_1)); - takeoff_state_sub = this->create_subscription("takeoff_state", 1, - std::bind(&BehaviorExecutive::takeoff_state_callback, - this, std::placeholders::_1)); - landing_state_sub = this->create_subscription("landing_state", 1, - std::bind(&BehaviorExecutive::landing_state_callback, - this, std::placeholders::_1)); - state_estimate_timed_out_sub = - this->create_subscription("state_estimate_timed_out", 1, - std::bind(&BehaviorExecutive::state_estimate_timed_out_callback, - this, std::placeholders::_1)); - stuck_sub = - this->create_subscription("stuck", 1, - std::bind(&BehaviorExecutive::stuck_callback, - this, std::placeholders::_1)); - - // publishers - recording_pub = this->create_publisher("set_recording_status", 1); - reset_stuck_pub = this->create_publisher("reset_stuck", 1); - clear_map_pub = this->create_publisher("clear_map", 1); - - // services - service_callback_group = - this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - robot_command_client = this->create_client( - "robot_command", rmw_qos_profile_services_default, service_callback_group); - trajectory_mode_client = this->create_client( - "set_trajectory_mode", rmw_qos_profile_services_default, service_callback_group); - takeoff_landing_command_client = this->create_client( - "set_takeoff_landing_command", rmw_qos_profile_services_default, service_callback_group); - - // action clients - exploration_client_ = rclcpp_action::create_client(this, "exploration_task"); - - // timers - timer = rclcpp::create_timer(this, this->get_clock(), rclcpp::Duration::from_seconds(1. / 20.), - std::bind(&BehaviorExecutive::timer_callback, this)); -} - -void BehaviorExecutive::timer_callback() -{ - if (request_control_action->is_active()) - { - if (request_control_action->active_has_changed()) - { - airstack_msgs::srv::RobotCommand::Request::SharedPtr request = - std::make_shared(); - request->command = airstack_msgs::srv::RobotCommand::Request::REQUEST_CONTROL; - - auto result = robot_command_client->async_send_request(request); - std::cout << "waiting be rc" << std::endl; - result.wait(); - std::cout << "done be rc" << std::endl; - if (result.get()->success) - request_control_action->set_success(); - else - request_control_action->set_failure(); - } - } - - if (arm_action->is_active()) - { - if (arm_action->active_has_changed()) - { - std_msgs::msg::Bool start_msg; - start_msg.data = true; - recording_pub->publish(start_msg); - - airstack_msgs::srv::RobotCommand::Request::SharedPtr request = - std::make_shared(); - request->command = airstack_msgs::srv::RobotCommand::Request::ARM; - - auto result = robot_command_client->async_send_request(request); - std::cout << "waiting be arm" << std::endl; - result.wait(); - std::cout << "done be arm" << std::endl; - if (result.get()->success) - arm_action->set_success(); - else - arm_action->set_failure(); - } - } - - if (disarm_action->is_active()) - { - if (disarm_action->active_has_changed()) - { - std_msgs::msg::Bool stop_msg; - stop_msg.data = false; - recording_pub->publish(stop_msg); - - in_air_condition->set(false); - airstack_msgs::srv::RobotCommand::Request::SharedPtr request = - std::make_shared(); - request->command = airstack_msgs::srv::RobotCommand::Request::DISARM; - - auto result = robot_command_client->async_send_request(request); - std::cout << "waiting be arm" << std::endl; - result.wait(); - std::cout << "done be arm" << std::endl; - if (result.get()->success) - disarm_action->set_success(); - else - disarm_action->set_failure(); - } - } - - if (takeoff_action->is_active()) - { - // std::cout << "takeoff" << std::endl; - takeoff_action->set_running(); - in_air_condition->set(true); - reset_stuck_pub->publish(std_msgs::msg::Empty()); - - if (takeoff_action->active_has_changed()) - { - // clear the local planner's map - clear_map_pub->publish(std_msgs::msg::Empty()); - - // put trajectory controller in track mode - airstack_msgs::srv::TrajectoryMode::Request::SharedPtr mode_request = - std::make_shared(); - mode_request->mode = airstack_msgs::srv::TrajectoryMode::Request::TRACK; - auto mode_result = trajectory_mode_client->async_send_request(mode_request); - std::cout << "trajectory mode request sent" << std::endl; - mode_result.wait(); - std::cout << "trajectory mode request confirmed received" << std::endl; - - if (mode_result.get()->success) - { - // send a takeoff command to ardupilot - // TODO clean up variable names - airstack_msgs::srv::RobotCommand::Request::SharedPtr request = - std::make_shared(); - request->command = airstack_msgs::srv::RobotCommand::Request::TAKEOFF; - - auto result = robot_command_client->async_send_request(request); - std::cout << "waiting robot command takeoff" << std::endl; - result.wait(); - std::cout << "done robot command takeoff" << std::endl; - - // send the takeoff trajectory - if (result.get()->success) - { - airstack_msgs::srv::TakeoffLandingCommand::Request::SharedPtr takeoff_request = - std::make_shared(); - takeoff_request->command = - airstack_msgs::srv::TakeoffLandingCommand::Request::TAKEOFF; - auto takeoff_result = - takeoff_landing_command_client->async_send_request(takeoff_request); - std::cout << "takeoff request sent" << std::endl; - takeoff_result.wait(); - std::cout << "takeoff request confirmed received" << std::endl; - if (!takeoff_result.get()->accepted) - takeoff_action->set_failure(); - } - else - takeoff_action->set_failure(); - } - else - takeoff_action->set_failure(); - } - - if (takeoff_state == "COMPLETE") - { - takeoff_complete_condition->set(true); - takeoff_action->set_success(); - } - } - - if (land_action->is_active()) - { - // std::cout << "land" << std::endl; - land_action->set_running(); - if (land_action->active_has_changed()) - { - // put trajectory controller in track mode - airstack_msgs::srv::TrajectoryMode::Request::SharedPtr mode_request = - std::make_shared(); - mode_request->mode = airstack_msgs::srv::TrajectoryMode::Request::TRACK; - auto mode_result = trajectory_mode_client->async_send_request(mode_request); - std::cout << "trajectory mode request sent" << std::endl; - mode_result.wait(); - std::cout << "trajectory mode request confirmed received" << std::endl; - - if (mode_result.get()->success) - { - airstack_msgs::srv::TakeoffLandingCommand::Request::SharedPtr land_request = - std::make_shared(); - land_request->command = airstack_msgs::srv::TakeoffLandingCommand::Request::LAND; - auto land_result = takeoff_landing_command_client->async_send_request(land_request); - std::cout << "land 1" << std::endl; - land_result.wait(); - std::cout << "land 2" << std::endl; - if (land_result.get()->accepted) - land_action->set_success(); - else - land_action->set_failure(); - } - else - land_action->set_failure(); - } - - if (landing_state == "COMPLETE") - { - landing_complete_condition->set(true); - land_action->set_success(); - } - } - - if (pause_action->is_active()) - { - pause_action->set_running(); - if (pause_action->active_has_changed()) - { - airstack_msgs::srv::TrajectoryMode::Request::SharedPtr mode_request = - std::make_shared(); - mode_request->mode = airstack_msgs::srv::TrajectoryMode::Request::PAUSE; - auto mode_result = trajectory_mode_client->async_send_request(mode_request); - mode_result.wait(); - } - } - - if (rewind_action->is_active()) - { - rewind_action->set_running(); - if (rewind_action->active_has_changed()) - { - airstack_msgs::srv::TrajectoryMode::Request::SharedPtr mode_request = - std::make_shared(); - mode_request->mode = airstack_msgs::srv::TrajectoryMode::Request::REWIND; - auto mode_result = trajectory_mode_client->async_send_request(mode_request); - mode_result.wait(); - } - } - - // follow fixed trajectory action - if (follow_fixed_trajectory_action->is_active()) - { - follow_fixed_trajectory_action->set_running(); - - if (follow_fixed_trajectory_action->active_has_changed()) - { - // put trajectory controller in track mode - airstack_msgs::srv::TrajectoryMode::Request::SharedPtr mode_request = - std::make_shared(); - mode_request->mode = airstack_msgs::srv::TrajectoryMode::Request::TRACK; - auto mode_result = trajectory_mode_client->async_send_request(mode_request); - std::cout << "trajectory mode request sent" << std::endl; - mode_result.wait(); - std::cout << "trajectory mode request confirmed received" << std::endl; - } - } - - if (global_plan_action->is_active()) - { - global_plan_action->set_running(); - if (global_plan_action->active_has_changed()) - { - auto goal = ExplorationTask::Goal(); - goal.min_altitude_agl = 2.0f; - goal.max_altitude_agl = 20.0f; - goal.min_flight_speed = 1.0f; - goal.max_flight_speed = 5.0f; - goal.time_limit_sec = 0.0f; // no limit - - exploration_goal_done_ = false; - exploration_goal_succeeded_ = false; - - auto options = rclcpp_action::Client::SendGoalOptions(); - options.goal_response_callback = - [this](const rclcpp_action::ClientGoalHandle::SharedPtr& gh) { - exploration_goal_handle_ = gh; - if (!gh) { - RCLCPP_WARN(this->get_logger(), "ExplorationTask goal rejected"); - exploration_goal_done_ = true; - } - }; - options.result_callback = - [this](const rclcpp_action::ClientGoalHandle::WrappedResult& result) { - exploration_goal_succeeded_ = - (result.code == rclcpp_action::ResultCode::SUCCEEDED) && result.result->success; - exploration_goal_done_ = true; - }; - exploration_client_->async_send_goal(goal, options); - RCLCPP_INFO(this->get_logger(), "Sent ExplorationTask goal"); - } - - if (exploration_goal_done_) - { - if (exploration_goal_succeeded_) - global_plan_action->set_success(); - else - global_plan_action->set_failure(); - } - } - - for (bt::Condition *condition : conditions) - condition->publish(); - for (bt::Action *action : actions) - action->publish(); -} - -// callbacks - -/** - * @brief Parses behavior tree commands and updates conditions accordingly - * - * @param msg - */ -void BehaviorExecutive::bt_commands_callback(behavior_tree_msgs::msg::BehaviorTreeCommands msg) -{ - for (int i = 0; i < msg.commands.size(); i++) - { - std::string condition_name = msg.commands[i].condition_name; - int status = msg.commands[i].status; - - for (int j = 0; j < conditions.size(); j++) - { - bt::Condition *condition = conditions[j]; - if (condition_name == condition->get_label()) - { - if (status == behavior_tree_msgs::msg::Status::SUCCESS) - condition->set(true); - else if (status == behavior_tree_msgs::msg::Status::FAILURE) - condition->set(false); - } - } - } -} - -void BehaviorExecutive::is_armed_callback(const std_msgs::msg::Bool::SharedPtr msg) -{ - armed_condition->set(msg->data); -} - -void BehaviorExecutive::has_control_callback(const std_msgs::msg::Bool::SharedPtr msg) -{ - offboard_mode_condition->set(msg->data); -} - -void BehaviorExecutive::takeoff_state_callback(const std_msgs::msg::String::SharedPtr msg) -{ - takeoff_state = msg->data; -} - -void BehaviorExecutive::landing_state_callback(const std_msgs::msg::String::SharedPtr msg) -{ - landing_state = msg->data; -} - -void BehaviorExecutive::state_estimate_timed_out_callback(const std_msgs::msg::Bool::SharedPtr msg) -{ - state_estimate_timed_out_condition->set(msg->data); -} - -void BehaviorExecutive::stuck_callback(const std_msgs::msg::Bool::SharedPtr msg) -{ - stuck_condition->set(msg->data); -} - -int main(int argc, char **argv) -{ - rclcpp::init(argc, argv); - std::shared_ptr node = std::make_shared(); - - rclcpp::executors::MultiThreadedExecutor executor; - executor.add_node(node); - executor.spin(); - rclcpp::shutdown(); - - return 0; -} diff --git a/robot/ros_ws/src/behavior/behavior_tree/.gitignore b/robot/ros_ws/src/behavior/behavior_tree/.gitignore deleted file mode 100644 index e6458330d..000000000 --- a/robot/ros_ws/src/behavior/behavior_tree/.gitignore +++ /dev/null @@ -1,2 +0,0 @@ -*~ -*.pyc \ No newline at end of file diff --git a/robot/ros_ws/src/behavior/behavior_tree/CMakeLists.txt b/robot/ros_ws/src/behavior/behavior_tree/CMakeLists.txt deleted file mode 100644 index 00372fad8..000000000 --- a/robot/ros_ws/src/behavior/behavior_tree/CMakeLists.txt +++ /dev/null @@ -1,75 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(behavior_tree) - -# Default to C99 -if(NOT CMAKE_C_STANDARD) - set(CMAKE_C_STANDARD 99) -endif() - -# Default to C++14 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# find dependencies -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(std_msgs REQUIRED) -find_package(behavior_tree_msgs REQUIRED) -find_package(Boost REQUIRED COMPONENTS iostreams) - -include_directories(include ${rclcpp_INCLUDE_DIRS} ${behavior_tree_msgs_INCLUDE_DIRS}) - -add_executable(behavior_tree_implementation src/behavior_tree_implementation.cpp) -target_compile_features(behavior_tree_implementation PUBLIC c_std_99 cxx_std_17) -target_link_libraries(behavior_tree_implementation - ${Boost_LIBRARIES} - ${rclcpp_LIBRARIES} - ${std_msgs_LIBRARIES} - ${behavior_tree_msgs_LIBRARIES}) -#ament_target_dependencies(behavior_tree_implementation rclcpp std_msgs behavior_tree_msgs) - -add_library(behavior_tree src/behavior_tree.cpp) -ament_target_dependencies(behavior_tree rclcpp std_msgs behavior_tree_msgs) -ament_export_targets(behavior_tree HAS_LIBRARY_TARGET) - -install( - DIRECTORY include/behavior_tree - DESTINATION include - ) - -install( - TARGETS behavior_tree - EXPORT behavior_tree - LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib - RUNTIME DESTINATION bin - INCLUDES DESTINATION include -) - -install(TARGETS - behavior_tree_implementation - DESTINATION lib/${PROJECT_NAME}) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # uncomment the line when a copyright and license is not present in all source files - #set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # uncomment the line when this package is not in a git repo - #set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() - -# Install files. -# install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) -# install(DIRECTORY rviz DESTINATION share/${PROJECT_NAME}) -install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) -# install(DIRECTORY params DESTINATION share/${PROJECT_NAME}) - -ament_package() diff --git a/robot/ros_ws/src/behavior/behavior_tree/config/drone.tree b/robot/ros_ws/src/behavior/behavior_tree/config/drone.tree deleted file mode 100644 index bf28299ed..000000000 --- a/robot/ros_ws/src/behavior/behavior_tree/config/drone.tree +++ /dev/null @@ -1,82 +0,0 @@ -? - -> - (Auto Takeoff Commanded) - -> - - (State Estimate Timed Out) - ? - (In Air) - (Armed) - (Offboard Mode) - [Request Control] - ? - (In Air) - (Armed) - [Arm] - ? - (Takeoff Complete) - [Takeoff] - -> - (Takeoff Commanded) - ? - (Takeoff Complete) - [Takeoff] - -> - (Land Commanded) - ? - (Landing Complete) - [Land] - -> - (Pause Commanded) - ? - [Pause] - -> - (Rewind Commanded) - ? - [Rewind] - -> - (Fixed Trajectory Commanded) - ? - [Follow Fixed Trajectory] - -> - (Global Plan Commanded) - ? - [Follow Global Plan] - -> - (Offboard Commanded) - ? - (Offboard Mode) - [Request Control] - -> - (Arm Commanded) - ? - (Armed) - [Arm] - -> - (Disarm Commanded) - ? - - (Armed) - [Disarm] - -> - (Autonomously Explore Commanded) - -> - - (State Estimate Timed Out) - ? - (In Air) - (Armed) - (Offboard Mode) - [Request Control] - ? - (In Air) - (Armed) - [Arm] - ? - (Takeoff Complete) - [Takeoff] - ? - -> - (Stuck) - [Rewind] - [Follow Global Plan] \ No newline at end of file diff --git a/robot/ros_ws/src/behavior/behavior_tree/include/behavior_tree/behavior_tree.hpp b/robot/ros_ws/src/behavior/behavior_tree/include/behavior_tree/behavior_tree.hpp deleted file mode 100644 index 2c1b9a6ed..000000000 --- a/robot/ros_ws/src/behavior/behavior_tree/include/behavior_tree/behavior_tree.hpp +++ /dev/null @@ -1,139 +0,0 @@ -// Copyright (c) 2024 Carnegie Mellon University -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace bt { - -class Condition { - private: - std_msgs::msg::Bool success; - std::string label; - - rclcpp::Publisher::SharedPtr success_pub; - - public: - Condition(std::string label, rclcpp::Node* node); - void set(bool b); - void publish(); - bool get(); - std::string get_label(); - - static std::string get_published_topic_name(std::string label); -}; - -class Action { - private: - bool active, prev_active, active_changed; - uint64_t current_id; - behavior_tree_msgs::msg::Status status; - std::string label; - - rclcpp::Subscription::SharedPtr active_sub; - rclcpp::Publisher::SharedPtr status_pub; - - void active_callback(const behavior_tree_msgs::msg::Active::SharedPtr msg); - - std::function active_callback_func; - std::function inactive_callback_func; - - void init(std::string label, rclcpp::Node* node); - - public: - Action(std::string label, rclcpp::Node* node); - - // constructor for active and inactive callback that are class members - template - Action(std::string label, rclcpp::Node* node, void (ActiveClassType::*active_callback)(), - ActiveClassType* active_class_object, void (InactiveClassType::*inactive_callback)(), - InactiveClassType* inactive_class_object) { - init(label, node); - active_callback_func = std::bind(active_callback, active_class_object); - inactive_callback_func = std::bind(inactive_callback, inactive_class_object); - } - - // constructor for an active callback that is a class member - template - Action(std::string label, rclcpp::Node* node, void (ActiveClassType::*active_callback)(), - ActiveClassType* active_class_object) { - init(label, node); - active_callback_func = std::bind(active_callback, active_class_object); - } - - // constructor for active and inactive callbacks that are not class members. The inactive - // callback is optional - Action(std::string label, rclcpp::Node* node, void (*active_callback)(), - void (*inactive_callback)() = NULL) { - init(label, node); - active_callback_func = active_callback; - inactive_callback_func = inactive_callback; - } - - // constructor for an active callback that is not a class member and inactive callback that is a - // class member - template - Action(std::string label, rclcpp::Node* node, void (*active_callback)(), - void (InactiveClassType::*inactive_callback)(), - InactiveClassType* inactive_class_object) { - init(label, node); - active_callback_func = active_callback; - inactive_callback_func = std::bind(inactive_callback, inactive_class_object); - } - - // constructor for an active callback that is a class member and inactive callback that is not a - // class member - template - Action(std::string label, rclcpp::Node* node, void (ActiveClassType::*active_callback)(), - ActiveClassType* active_class_object, void (*inactive_callback)()) { - init(label, node); - active_callback_func = std::bind(active_callback, active_class_object); - inactive_callback_func = inactive_callback; - } - - static std::string get_published_topic_name(std::string label); - static std::string get_subscribed_topic_name(std::string label); - - void set_success(); - void set_running(); - void set_failure(); - - bool is_active(); - bool active_has_changed(); - - bool is_success(); - bool is_running(); - bool is_failure(); - - std::string get_label(); - - void publish(); -}; - -}; // namespace bt diff --git a/robot/ros_ws/src/behavior/behavior_tree/include/behavior_tree/behavior_tree_implementation.hpp b/robot/ros_ws/src/behavior/behavior_tree/include/behavior_tree/behavior_tree_implementation.hpp deleted file mode 100644 index 8d142abe2..000000000 --- a/robot/ros_ws/src/behavior/behavior_tree/include/behavior_tree/behavior_tree_implementation.hpp +++ /dev/null @@ -1,194 +0,0 @@ -// Copyright (c) 2024 Carnegie Mellon University -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#pragma once - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -//=============================================================================================== -// ------------------------------------- Base Node Class ---------------------------------------- -//=============================================================================================== - -class Node { - private: - public: - static double max_wait_time; - - std::string label; - bool is_active; - uint8_t status; - std::vector children; - - virtual void add_child(Node*); - virtual bool tick(bool active, int traversal_count) = 0; -}; - -//=============================================================================================== -// ------------------------------------ Control Flow Nodes ------------------------------------- -//=============================================================================================== - -class ControlFlowNode : public Node { - private: - public: -}; - -class FallbackNode : public ControlFlowNode { - private: - public: - FallbackNode(); - bool tick(bool active, int traversal_count) override; -}; - -class SequenceNode : public ControlFlowNode { - private: - public: - SequenceNode(); - bool tick(bool active, int traversal_count) override; -}; - -class ParallelNode : public ControlFlowNode { - private: - public: - int child_success_threshold; - ParallelNode(int child_success_threshold); - bool tick(bool active, int traversal_count) override; -}; - -//=============================================================================================== -// -------------------------------------- Execution Nodes -------------------------------------- -//=============================================================================================== - -class ExecutionNode : public Node { - private: - public: - rclcpp::Time status_modification_time; - int current_traversal_count; - rclcpp::Node* node; - // rclcpp::Subscription subscriber; - // rclcpp::Publisher publisher; - - ExecutionNode(rclcpp::Node* node); - void init_ros(); - virtual std::string get_publisher_name() = 0; - virtual void init_publisher() = 0; - virtual std::string get_subscriber_name() = 0; - virtual void init_subscriber() = 0; - void set_status(uint8_t status); - double get_status_age(); -}; - -class ConditionNode : public ExecutionNode { - private: - bool callback_status_updated; - // rclcpp::Node* node; - - public: - rclcpp::Subscription::SharedPtr subscriber; - // rclcpp::Publisher<>::SharedPtr publisher; - - ConditionNode(std::string label, rclcpp::Node* node); - bool tick(bool active, int traversal_count) override; - std::string get_publisher_name() override; - void init_publisher() override; - std::string get_subscriber_name() override; - void init_subscriber() override; - void callback(const std_msgs::msg::Bool::SharedPtr msg); -}; - -class ActionNode : public ExecutionNode { - private: - int current_id; - bool publisher_initialized; - bool callback_status_updated; - - public: - bool is_newly_active; - - // rclcpp::Node* node; - - rclcpp::Subscription::SharedPtr subscriber; - rclcpp::Publisher::SharedPtr publisher; - - ActionNode(std::string label, rclcpp::Node* node); - bool tick(bool active, int traversal_count) override; - std::string get_publisher_name() override; - void init_publisher() override; - std::string get_subscriber_name() override; - void init_subscriber() override; - void callback(const behavior_tree_msgs::msg::Status::SharedPtr msg); - void publish_active_msg(int active_id); -}; - -//=============================================================================================== -// -------------------------------------- Decorator Nodes -------------------------------------- -//=============================================================================================== - -class DecoratorNode : public Node { - private: - public: - void add_child(Node* node) override; -}; - -class NotNode : public DecoratorNode { - private: - public: - NotNode(); - void add_child(Node* node) override; - bool tick(bool active, int traversal_count) override; -}; - -//=============================================================================================== -// --------------------------------------- Behavior Tree --------------------------------------- -//=============================================================================================== - -class BehaviorTree { - public: - rclcpp::Node* ros2_node; - std::string config_filename; - std::vector nodes; - Node* root; - int traversal_count; - std::unordered_map active_ids; - rclcpp::Publisher::SharedPtr active_actions_pub; - rclcpp::Publisher::SharedPtr graphviz_pub; - rclcpp::Publisher::SharedPtr compressed_pub; - bool first_tick; - - void parse_config(); - int count_tabs(std::string str); - std::string strip_space(std::string str); - std::string strip_brackets(std::string str); - std::vector get_arguments(std::string str); - std::string get_graphviz(); - // public: - BehaviorTree(std::string config_filename, rclcpp::Node* node); - bool tick(); -}; diff --git a/robot/ros_ws/src/behavior/behavior_tree/package.xml b/robot/ros_ws/src/behavior/behavior_tree/package.xml deleted file mode 100644 index a99a5382d..000000000 --- a/robot/ros_ws/src/behavior/behavior_tree/package.xml +++ /dev/null @@ -1,22 +0,0 @@ - - - - behavior_tree - 0.0.0 - TODO: Package description - john - TODO: License declaration - - ament_cmake - - behavior_tree_msgs - std_msgs - rclcpp - - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/robot/ros_ws/src/behavior/behavior_tree/src/behavior_tree.cpp b/robot/ros_ws/src/behavior/behavior_tree/src/behavior_tree.cpp deleted file mode 100644 index 0661b90ae..000000000 --- a/robot/ros_ws/src/behavior/behavior_tree/src/behavior_tree.cpp +++ /dev/null @@ -1,136 +0,0 @@ -// Copyright (c) 2024 Carnegie Mellon University -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include - -namespace bt { - -// ============================================================================== -// -------------------------------- Condition ----------------------------------- -// ============================================================================== - -std::string Condition::get_published_topic_name(std::string label) { - std::string topic_name = label; - std::transform(topic_name.begin(), topic_name.end(), topic_name.begin(), ::tolower); - std::replace(topic_name.begin(), topic_name.end(), ' ', '_'); - topic_name += "_success"; - return topic_name; -} - -Condition::Condition(std::string label, rclcpp::Node* node) { - // ros::NodeHandle nh; - this->label = label; - // success_pub = nh.advertise(Condition::get_published_topic_name(label), 10); - success_pub = - node->create_publisher(Condition::get_published_topic_name(label), 10); - success.data = false; -} - -std::string Condition::get_label() { return label; } - -void Condition::set(bool b) { success.data = b; } - -bool Condition::get() { return success.data; } - -void Condition::publish() { success_pub->publish(success); } - -// ============================================================================== -// --------------------------------- Action ------------------------------------- -// ============================================================================== - -std::string Action::get_published_topic_name(std::string label) { - std::string topic_name = label; - std::transform(topic_name.begin(), topic_name.end(), topic_name.begin(), ::tolower); - std::replace(topic_name.begin(), topic_name.end(), ' ', '_'); - topic_name += "_status"; - return topic_name; -} - -std::string Action::get_subscribed_topic_name(std::string label) { - std::string topic_name = label; - std::transform(topic_name.begin(), topic_name.end(), topic_name.begin(), ::tolower); - std::replace(topic_name.begin(), topic_name.end(), ' ', '_'); - topic_name += "_active"; - return topic_name; -} - -Action::Action(std::string label, rclcpp::Node* node) { init(label, node); } - -void Action::init(std::string label, rclcpp::Node* node) { - // ros::NodeHandle nh; - active_sub = node->create_subscription( - Action::get_subscribed_topic_name(label), 10, - std::bind(&Action::active_callback, this, std::placeholders::_1)); - status_pub = node->create_publisher( - Action::get_published_topic_name(label), 1); - - active = false; - prev_active = false; - active_changed = false; - status.status = behavior_tree_msgs::msg::Status::FAILURE; - - active_callback_func = NULL; - inactive_callback_func = NULL; - - this->label = label; -} - -void Action::active_callback(const behavior_tree_msgs::msg::Active::SharedPtr msg) { - bool active_changed = active != msg->active; - - active = msg->active; - current_id = msg->id; - - if (active_changed && active_callback_func != NULL) { - if (active) - active_callback_func(); - else - inactive_callback_func(); - } -} - -void Action::set_success() { status.status = behavior_tree_msgs::msg::Status::SUCCESS; } - -void Action::set_running() { status.status = behavior_tree_msgs::msg::Status::RUNNING; } - -void Action::set_failure() { status.status = behavior_tree_msgs::msg::Status::FAILURE; } - -bool Action::is_active() { - active_changed = prev_active != active; - prev_active = active; - return active; -} - -bool Action::active_has_changed() { return active_changed; } - -bool Action::is_success() { return status.status == behavior_tree_msgs::msg::Status::SUCCESS; } - -bool Action::is_running() { return status.status == behavior_tree_msgs::msg::Status::RUNNING; } - -bool Action::is_failure() { return status.status == behavior_tree_msgs::msg::Status::FAILURE; } - -std::string Action::get_label() { return label; } - -void Action::publish() { - status.id = current_id; - status_pub->publish(status); -} - -}; // namespace bt diff --git a/robot/ros_ws/src/behavior/behavior_tree/src/behavior_tree_implementation.cpp b/robot/ros_ws/src/behavior/behavior_tree/src/behavior_tree_implementation.cpp deleted file mode 100644 index 2d0f2c0ba..000000000 --- a/robot/ros_ws/src/behavior/behavior_tree/src/behavior_tree_implementation.cpp +++ /dev/null @@ -1,755 +0,0 @@ -// Copyright (c) 2024 Carnegie Mellon University -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -//=============================================================================================== -// ------------------------------------- Base Node Class ---------------------------------------- -//=============================================================================================== - -double Node::max_wait_time = 1.0; - -void Node::add_child(Node* node) { children.push_back(node); } - -//=============================================================================================== -// ------------------------------------ Control Flow Nodes ------------------------------------- -//=============================================================================================== - -FallbackNode::FallbackNode() { - label = "?"; - is_active = false; - status = behavior_tree_msgs::msg::Status::FAILURE; -} - -bool FallbackNode::tick(bool active, int traversal_count) { - uint8_t prev_status = status; - bool child_changed = false; - - if (!active) { - // TODO: is ticking children necessary if inactive? - for (int i = 0; i < children.size(); i++) - child_changed |= children[i]->tick(false, traversal_count); - } else { - status = behavior_tree_msgs::msg::Status::FAILURE; - for (int i = 0; i < children.size(); i++) { - Node* child = children[i]; - if (status == behavior_tree_msgs::msg::Status::FAILURE) { - child_changed |= child->tick(true, traversal_count); - status = child->status; - } else - child_changed |= child->tick(false, traversal_count); - } - } - - bool status_changed = (status != prev_status) || child_changed; - bool active_changed = (is_active != active); - - is_active = active; - - return status_changed || active_changed; -} - -SequenceNode::SequenceNode() { - label = "->"; //"\u2192"; // unicode arrow character - is_active = false; - status = behavior_tree_msgs::msg::Status::FAILURE; -} - -bool SequenceNode::tick(bool active, int traversal_count) { - uint8_t prev_status = status; - bool child_changed = false; - - if (!active) { - for (int i = 0; i < children.size(); i++) - child_changed |= children[i]->tick(false, traversal_count); - - } else { - status = behavior_tree_msgs::msg::Status::SUCCESS; - for (int i = 0; i < children.size(); i++) { - Node* child = children[i]; - if (status == behavior_tree_msgs::msg::Status::SUCCESS) { - child_changed |= child->tick(true, traversal_count); - status = child->status; - } else - child_changed |= child->tick(false, traversal_count); - } - } - - bool status_changed = (status != prev_status) || child_changed; - bool active_changed = (is_active != active); - - is_active = active; - - return status_changed || active_changed; -} - -ParallelNode::ParallelNode(int child_success_threshold) { - label = "||"; - is_active = false; - status = behavior_tree_msgs::msg::Status::FAILURE; - this->child_success_threshold = child_success_threshold; -} - -bool ParallelNode::tick(bool active, int traversal_count) { - uint8_t prev_status = status; - bool child_changed = false; - - if (!active) { - for (int i = 0; i < children.size(); i++) - child_changed |= children[i]->tick(false, traversal_count); - } else { - int child_success_count = 0; - int child_failure_count = 0; - for (int i = 0; i < children.size(); i++) { - Node* child = children[i]; - child_changed |= child->tick(true, traversal_count); - - if (child->status == behavior_tree_msgs::msg::Status::SUCCESS) - child_success_count++; - else if (child->status == behavior_tree_msgs::msg::Status::FAILURE) - child_failure_count++; - } - - if (child_success_count >= child_success_threshold) - status = behavior_tree_msgs::msg::Status::SUCCESS; - else if (child_failure_count >= (children.size() - child_success_threshold + 1)) - status = behavior_tree_msgs::msg::Status::FAILURE; - else - status = behavior_tree_msgs::msg::Status::RUNNING; - } - - bool status_changed = (status != prev_status) || child_changed; - bool active_changed = (is_active != active); - - is_active = active; - - return status_changed || active_changed; -} - -//=============================================================================================== -// -------------------------------------- Execution Nodes -------------------------------------- -//=============================================================================================== - -ExecutionNode::ExecutionNode(rclcpp::Node* node) : node(node) {} - -void ExecutionNode::init_ros() { - init_publisher(); - init_subscriber(); -} - -void ExecutionNode::set_status(uint8_t status) { - this->status = status; - status_modification_time = node->get_clock()->now(); // rclcpp::Time::now(); -} - -double ExecutionNode::get_status_age() { - // return (rclcpp::Time::now() - status_modification_time).toSec(); - return (node->get_clock()->now() - status_modification_time).seconds(); -} - -ConditionNode::ConditionNode(std::string label, rclcpp::Node* node) : ExecutionNode(node) { - this->label = label; - status_modification_time = node->get_clock()->now(); // rclcpp::Time::now(); - is_active = false; - status = behavior_tree_msgs::msg::Status::FAILURE; - current_traversal_count = -1; - callback_status_updated = false; -} - -bool ConditionNode::tick(bool active, int traversal_count) { - // if(active) - // ROS_INFO_STREAM("Condition " << label << " active"); - - uint8_t prev_status = status; - bool child_changed = false; - - if (current_traversal_count != traversal_count) - current_traversal_count = traversal_count; - else if (is_active) - return false; // TODO: is this correct? - - if (!is_active && active && status == behavior_tree_msgs::msg::Status::FAILURE) - set_status(behavior_tree_msgs::msg::Status::FAILURE); - - if (get_status_age() > Node::max_wait_time) - set_status(behavior_tree_msgs::msg::Status::FAILURE); - - bool status_changed = (status != prev_status) || callback_status_updated; - bool active_changed = (is_active != active); - - is_active = active; - callback_status_updated = false; - - return status_changed || active_changed; -} - -std::string ConditionNode::get_publisher_name() { return ""; } - -void ConditionNode::init_publisher() {} - -std::string ConditionNode::get_subscriber_name() { - std::string name = label; - std::transform(name.begin(), name.end(), name.begin(), - [](unsigned char c) { return std::tolower(c); }); - std::replace(name.begin(), name.end(), ' ', '_'); - name = name + "_success"; - // ROS_INFO_STREAM("condition get subscriber name: " << name << " " << label); - return name; -} - -void ConditionNode::init_subscriber() { - // ros::NodeHandle nh; - subscriber = node->create_subscription( - get_subscriber_name(), 1, std::bind(&ConditionNode::callback, this, std::placeholders::_1)); -} - -void ConditionNode::callback(const std_msgs::msg::Bool::SharedPtr msg) { - // ROS_INFO_STREAM(label << ": " << (int)msg.data); - // TODO: bug where if status is set here the changed flag won't be set correctly in tick - uint8_t prev_status = status; - - if (msg->data) - set_status(behavior_tree_msgs::msg::Status::SUCCESS); - else - set_status(behavior_tree_msgs::msg::Status::FAILURE); - - if (status != prev_status) callback_status_updated = true; -} - -ActionNode::ActionNode(std::string label, rclcpp::Node* node) : ExecutionNode(node) { - this->label = label; - status_modification_time = node->get_clock()->now(); // rclcpp::Time::now(); - is_active = false; - status = behavior_tree_msgs::msg::Status::FAILURE; - current_traversal_count = -1; - is_newly_active = false; - current_id = 0; - publisher_initialized = false; - callback_status_updated = false; -} - -bool ActionNode::tick(bool active, int traversal_count) { - // if(active) - // ROS_INFO_STREAM("Action " << label << " active"); - - uint8_t prev_status = status; - - if (current_traversal_count != traversal_count) - current_traversal_count = traversal_count; - else if (is_active) - return false; // TOOD: is this correct? - - if (!is_active and active) { - set_status(behavior_tree_msgs::msg::Status::RUNNING); - is_newly_active = true; - } else - is_newly_active = false; - - if (get_status_age() > Node::max_wait_time) - set_status(behavior_tree_msgs::msg::Status::FAILURE); - - bool status_changed = (status != prev_status) || callback_status_updated; - bool active_changed = (is_active != active); - - is_active = active; - callback_status_updated = false; - - return status_changed || active_changed; -} - -std::string ActionNode::get_publisher_name() { - std::string name = label; - std::transform(name.begin(), name.end(), name.begin(), - [](unsigned char c) { return std::tolower(c); }); - std::replace(name.begin(), name.end(), ' ', '_'); - name = name + "_active"; - // ROS_INFO_STREAM("action get publisher name: " << name << " " << label); - return name; -} - -void ActionNode::init_publisher() { - publisher = node->create_publisher(get_publisher_name(), 1); - publisher_initialized = true; -} - -std::string ActionNode::get_subscriber_name() { - std::string name = label; - std::transform(name.begin(), name.end(), name.begin(), - [](unsigned char c) { return std::tolower(c); }); - std::replace(name.begin(), name.end(), ' ', '_'); - name = name + "_status"; - // ROS_INFO_STREAM("action get subscriber name: " << name << " " << label); - return name; -} - -void ActionNode::init_subscriber() { - subscriber = node->create_subscription( - get_subscriber_name(), 1, std::bind(&ActionNode::callback, this, std::placeholders::_1)); -} - -void ActionNode::callback(const behavior_tree_msgs::msg::Status::SharedPtr msg) { - uint8_t prev_status = status; - if (is_active) { - if (msg->id != current_id) { - // ROS_ERROR_STREAM(label << " Incorrect ID " << msg->id << " " << current_id << " " << - // is_active); - std::cout << label << " Incorrect ID " << msg->id << " " << current_id << " " - << is_active << std::endl; - return; - } - set_status(msg->status); - } - - if (status != prev_status) callback_status_updated = true; -} - -void ActionNode::publish_active_msg(int active_id) { - if (publisher_initialized) { - behavior_tree_msgs::msg::Active active_msg; - active_msg.active = is_active; - active_msg.id = active_id; - current_id = active_id; - publisher->publish(active_msg); - } -} - -//=============================================================================================== -// -------------------------------------- Decorator Nodes -------------------------------------- -//=============================================================================================== - -void DecoratorNode::add_child(Node* node) { - if (children.size() == 0) - children.push_back(node); - else { - // ROS_ERROR_STREAM("A decorator node can only have one child."); - std::cout << "A decorator node can only have one child." << std::endl; - exit(1); - } -} - -NotNode::NotNode() { - label = "!"; - is_active = false; - status = behavior_tree_msgs::msg::Status::FAILURE; -} - -void NotNode::add_child(Node* node) { - ConditionNode* condition_node = dynamic_cast(node); - if (condition_node != NULL) - DecoratorNode::add_child(condition_node); - else { - // ROS_ERROR_STREAM("A not decorator node can only have a condition node as a child."); - std::cout << "A not decorator node can only have a condition node as a child." << std::endl; - exit(1); - } -} - -bool NotNode::tick(bool active, int traversal_count) { - uint8_t prev_status = status; - bool child_changed = false; - - if (children.size() > 0) { - Node* child = children[0]; - child_changed |= child->tick(active, traversal_count); - - if (child->status == behavior_tree_msgs::msg::Status::SUCCESS) - status = behavior_tree_msgs::msg::Status::FAILURE; - else if (child->status == behavior_tree_msgs::msg::Status::FAILURE) - status = behavior_tree_msgs::msg::Status::SUCCESS; - } - - bool status_changed = (status != prev_status) || child_changed; - bool active_changed = (is_active != active); - - is_active = active; - - return status_changed || active_changed; -} - -//=============================================================================================== -// --------------------------------------- Behavior Tree --------------------------------------- -//=============================================================================================== - -BehaviorTree::BehaviorTree(std::string config_filename, rclcpp::Node* node) : ros2_node(node) { - this->config_filename = config_filename; - root = NULL; - traversal_count = 0; - first_tick = true; - - active_actions_pub = node->create_publisher("active_actions", 1); - graphviz_pub = node->create_publisher("behavior_tree_graphviz", 1); - compressed_pub = - node->create_publisher("behavior_tree_graphviz_compressed", 1); - - parse_config(); - for (int i = 0; i < nodes.size(); i++) { - ActionNode* action_node = dynamic_cast(nodes[i]); - if (action_node != NULL) action_node->init_ros(); - ConditionNode* condition_node = dynamic_cast(nodes[i]); - if (condition_node != NULL) condition_node->init_ros(); - } -} - -void BehaviorTree::parse_config() { - std::ifstream in(config_filename); - - // ROS_INFO_STREAM("Config file: " << config_filename); - - if (in.is_open()) { - std::vector nodes_worklist; - int prev_tabs = 0; - - std::string line; - while (getline(in, line)) { - // skip empty lines - if (line.size() == 0) continue; - - // ROS_INFO_STREAM("Line: " << line); - - // initialization - int curr_tabs = count_tabs(line); - std::string label = strip_space(line); - Node* node = NULL; - - // ROS_INFO_STREAM("curr tabs: " << curr_tabs << " label: " << label); - - // create a node of the right type - if (label.compare(0, 2, "->") == 0) - node = new SequenceNode(); - else if (label.compare(0, 1, "?") == 0) - node = new FallbackNode(); - else if (label.compare(0, 2, "||") == 0) { - std::vector arguments = get_arguments(label); - int child_success_threshold = 0; - if (arguments.size() > 0) - child_success_threshold = arguments[0]; - else { - // ROS_ERROR_STREAM("Arguments not provided to parallel node"); - std::cout << "Arguments not provided to parallel node" << std::endl; - exit(1); - } - node = new ParallelNode(child_success_threshold); - } else if (label.compare(0, 1, "(") == 0) - node = new ConditionNode(strip_brackets(label), ros2_node); - else if (label.compare(0, 1, "[") == 0) { - node = new ActionNode(strip_brackets(label), ros2_node); - active_ids[node->label] = 0; - } else if (label.compare(0, 1, "<") == 0) { - std::string decorator_label = strip_brackets(label); - if (decorator_label.compare(0, 1, "!") == 0) node = new NotNode(); - } - - if (node != NULL) { - nodes.push_back(node); - - if (root == NULL) { - root = node; - nodes_worklist.push_back(node); - continue; - } - - if (curr_tabs == prev_tabs + 1) { - Node* parent = nodes_worklist[nodes_worklist.size() - 1]; - parent->add_child(node); - } else { - for (int i = 0; i < prev_tabs - curr_tabs + 1; i++) nodes_worklist.pop_back(); - Node* parent = nodes_worklist[nodes_worklist.size() - 1]; - parent->add_child(node); - } - - nodes_worklist.push_back(node); - prev_tabs = curr_tabs; - } - } - in.close(); - } else - std::cout << "Failed to open behavior tree config file: " << config_filename << std::endl; - // ROS_ERROR_STREAM("Failed to open behavior tree config file: " << config_filename); -} - -int BehaviorTree::count_tabs(std::string str) { - int count = 0; - for (int i = 0; i < str.size(); i++) - if (str[i] == '\t') - count++; - else - break; - - return count; -} - -std::string BehaviorTree::strip_space(std::string str) { - std::string result = str; - while (result.size() > 0) - if (std::isspace(result[0])) - result.erase(0, 1); - else - break; - - while (result.size() > 0) - if (std::isspace(result[result.size() - 1])) - result.erase(result.size() - 1, 1); - else - break; - - return result; -} - -std::string BehaviorTree::strip_brackets(std::string str) { - std::string result = str; - while (result.size() > 0) - if (result[0] == '(' || result[0] == '[' || result[0] == '<') - result.erase(0, 1); - else - break; - - while (result.size() > 0) - if (result[result.size() - 1] == ')' || result[result.size() - 1] == ']' || - result[result.size() - 1] == '>') - result.erase(result.size() - 1, 1); - else - break; - - return result; -} - -std::vector BehaviorTree::get_arguments(std::string str) { - std::vector arguments; - - std::istringstream ss(str); - std::string first; - ss >> first; - - int i; - while (ss >> i) arguments.push_back(i); - - return arguments; -} - -bool BehaviorTree::tick() { - bool changed = false || first_tick; - first_tick = false; - - if (root != NULL) { - changed = root->tick(true, traversal_count); - traversal_count++; - - std_msgs::msg::String active_actions; - - std::unordered_map unique_action_nodes; - for (int i = 0; i < nodes.size(); i++) { - ActionNode* action_node = dynamic_cast(nodes[i]); - if (action_node != NULL) { - if (unique_action_nodes.count(action_node->label) == 0 || action_node->is_active) { - unique_action_nodes[action_node->label] = action_node; - if (action_node->is_newly_active) active_ids[action_node->label]++; - } - } - } - - for (auto it = unique_action_nodes.begin(); it != unique_action_nodes.end(); it++) { - ActionNode* action_node = dynamic_cast(it->second); - if (action_node != NULL) { - action_node->publish_active_msg(active_ids[it->first]); - - if (it->second->is_active) active_actions.data += action_node->label + ", "; - } else { - std::cout << "action node is NULL, something went wrong." << std::endl; - // ROS_ERROR_STREAM("action node is NULL, something went wrong."); - } - } - if (active_actions.data.length() >= 2) { - active_actions.data.pop_back(); - active_actions.data.pop_back(); - } - active_actions_pub->publish(active_actions); - } - - return changed; -} - -std::string BehaviorTree::get_graphviz() { - std::string gv = "digraph G {\n"; - - std::vector nodes_worklist; - nodes_worklist.push_back(root); - int count = 0; - std::unordered_map node_names; - - while (!nodes_worklist.empty()) { - Node* node = nodes_worklist.back(); - nodes_worklist.pop_back(); - std::string name = "node_" + std::to_string(count); - count++; - - std::string style = ""; - if (node->is_active) { - style += "penwidth=2 color=black style=filled "; - if (node->status == behavior_tree_msgs::msg::Status::SUCCESS) - style += "fillcolor=chartreuse"; // dark green - else if (node->status == behavior_tree_msgs::msg::Status::RUNNING) - style += "fillcolor=skyblue"; // dark blue - else if (node->status == behavior_tree_msgs::msg::Status::FAILURE) - style += "fillcolor=tomato"; // dark red - } else { - style += "penwidth=2 "; - if (node->status == behavior_tree_msgs::msg::Status::SUCCESS) - style += "color=chartreuse"; // light green - else if (node->status == behavior_tree_msgs::msg::Status::RUNNING) - style += "color=skyblue"; // light blue - else if (node->status == behavior_tree_msgs::msg::Status::FAILURE) - style += "color=tomato"; // light red - } - - if (dynamic_cast(node) != NULL) { - gv += "\t" + name + " [label=\"" + node->label + "\" " + style + "]\n"; - } else if (dynamic_cast(node) != NULL) { - gv += "\t" + name + " [label=\"" + node->label + "\" shape=square " + style + "]\n"; - } else if (dynamic_cast(node) != NULL) { - gv += "\t" + name + " [label=\"" + node->label + "\" shape=square " + style + "]\n"; - } else if (dynamic_cast(node) != NULL) { - gv += "\t" + name + " [label=\"" + node->label + "\" shape=square " + style + "]\n"; - } else if (dynamic_cast(node) != NULL) { - gv += "\t" + name + " [label=\"" + node->label + "\" shape=parallelogram " + style + - "]\n"; - } else if (dynamic_cast(node) != NULL) { - gv += "\t" + name + " [label=\"" + node->label + "\" shape=diamond " + style + "]\n"; - } - - node_names[node] = name; - - for (int i = 0; i < node->children.size(); i++) nodes_worklist.push_back(node->children[i]); - } - - gv += "\n\tordering=out;\n\n"; - - nodes_worklist.push_back(root); - while (!nodes_worklist.empty()) { - Node* node = nodes_worklist.back(); - nodes_worklist.pop_back(); - - for (int i = 0; i < node->children.size(); i++) { - gv += "\t" + node_names[node] + " -> " + node_names[node->children[i]] + ";\n"; - nodes_worklist.push_back(node->children[i]); - } - } - - gv += "}\n"; - - return gv; -} - -std::string string_compress_encode(const std::string& data) { - std::stringstream compressed; - std::stringstream original; - original << data; - boost::iostreams::filtering_streambuf out; - out.push(boost::iostreams::zlib_compressor()); - out.push(original); - boost::iostreams::copy(out, compressed); - - /**need to encode here **/ - // std::string compressed_encoded = base64_encode(reinterpret_cast(compressed.c_str()), compressed.length()); - - return compressed.str(); // compressed_encoded; -} - -BehaviorTree* bt; - -behavior_tree_msgs::msg::GraphVizXdot graphviz_msg; -behavior_tree_msgs::msg::GraphVizXdotCompressed compressed_msg; -bool only_publish_on_change; -void timer_callback() { // const rclcpp::TimerEvent& te){ - bool changed = bt->tick(); - // ROS_INFO_STREAM("Changed: " << changed); - - if (changed) { - graphviz_msg.header.stamp = bt->ros2_node->get_clock()->now(); // rclcpp::Time::now(); - graphviz_msg.xdot.data = bt->get_graphviz(); - compressed_msg.header.stamp = graphviz_msg.header.stamp; - compressed_msg.xdot_compressed.data = string_compress_encode(graphviz_msg.xdot.data); - - if (only_publish_on_change) { - bt->graphviz_pub->publish(graphviz_msg); - bt->compressed_pub->publish(compressed_msg); - } - } - - if (!only_publish_on_change) { - bt->graphviz_pub->publish(graphviz_msg); - bt->compressed_pub->publish(compressed_msg); - } -} - -class BehaviorTreeNode : public rclcpp::Node { - private: - rclcpp::TimerBase::SharedPtr timer; - - public: - BehaviorTreeNode() : Node("behavior_tree_node") { - // timer = this->create_timer(std::chrono::milliseconds(50), - // std::bind(&MinimalPublisher::timer_callback, this)); - - this->declare_parameter("config", ""); - std::string config_filename = this->get_parameter("config").as_string(); - - this->declare_parameter("timeout", 1.0); - ::Node::max_wait_time = this->get_parameter("timeout").as_double(); - - bt = new BehaviorTree(config_filename, this); - - timer = rclcpp::create_timer(this, this->get_clock(), std::chrono::milliseconds(50), - &timer_callback); - } -}; - -int main(int argc, char** argv) { - /* - ros::init(argc, argv, "behavior_tree"); - - ros::NodeHandle nh; - ros::NodeHandle pnh("~"); - - std::string config_filename = pnh.param("config", std::string("")); - only_publish_on_change = pnh.param("only_publish_on_change", false); - Node::max_wait_time = pnh.param("timeout", 1.0); - - rclcpp::Timer timer = nh.createTimer(ros::Duration(0.05), timer_callback); - - bt = new BehaviorTree(config_filename); - - ros::spin(); - */ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - - return 0; -} diff --git a/robot/ros_ws/src/behavior/behavior_tree_example/.gitignore b/robot/ros_ws/src/behavior/behavior_tree_example/.gitignore deleted file mode 100644 index e6458330d..000000000 --- a/robot/ros_ws/src/behavior/behavior_tree_example/.gitignore +++ /dev/null @@ -1,2 +0,0 @@ -*~ -*.pyc \ No newline at end of file diff --git a/robot/ros_ws/src/behavior/behavior_tree_example/CMakeLists.txt b/robot/ros_ws/src/behavior/behavior_tree_example/CMakeLists.txt deleted file mode 100644 index 21d815efb..000000000 --- a/robot/ros_ws/src/behavior/behavior_tree_example/CMakeLists.txt +++ /dev/null @@ -1,69 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(behavior_tree_example) - -# Default to C99 -if(NOT CMAKE_C_STANDARD) - set(CMAKE_C_STANDARD 99) -endif() - -# Default to C++14 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# find dependencies -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(std_msgs REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(visualization_msgs REQUIRED) -find_package(nav_msgs REQUIRED) -find_package(behavior_tree_msgs REQUIRED) -find_package(behavior_tree REQUIRED) -find_package(tf2 REQUIRED) - -#add_executable(behavior_tree_example src/behavior_tree_example.cpp) -#ament_target_dependencies(talker rclcpp std_msgs) - -add_executable(behavior_tree_example src/behavior_tree_example.cpp) -ament_target_dependencies(behavior_tree_example rclcpp std_msgs geometry_msgs visualization_msgs nav_msgs behavior_tree_msgs behavior_tree tf2) - -add_executable(robot_node src/robot_node.cpp) -ament_target_dependencies(robot_node rclcpp std_msgs geometry_msgs visualization_msgs nav_msgs behavior_tree_msgs tf2) - -install(DIRECTORY - launch - DESTINATION share/${PROJECT_NAME}/ - ) - -install(DIRECTORY - config - DESTINATION share/${PROJECT_NAME}/ - ) - -install(DIRECTORY - rviz - DESTINATION share/${PROJECT_NAME}/ - ) - -install(TARGETS - behavior_tree_example - robot_node - DESTINATION lib/${PROJECT_NAME}) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # uncomment the line when a copyright and license is not present in all source files - #set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # uncomment the line when this package is not in a git repo - #set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() - -ament_package() diff --git a/robot/ros_ws/src/behavior/behavior_tree_example/config/example.tree b/robot/ros_ws/src/behavior/behavior_tree_example/config/example.tree deleted file mode 100644 index 4903ecf67..000000000 --- a/robot/ros_ws/src/behavior/behavior_tree_example/config/example.tree +++ /dev/null @@ -1,22 +0,0 @@ -? - -> - (Visited Destination) - (At Home) - ? - (On Ground) - [Land] - -> - ? - (In No Fly Zone) - (At Flight Altitude) - [Takeoff] - ? - (In Fly Zone) - (On Ground) - [Land] - ? - (Visited Destination) - [Go To Destination] - ? - (At Home) - [Go Home] diff --git a/robot/ros_ws/src/behavior/behavior_tree_example/config/example2.tree b/robot/ros_ws/src/behavior/behavior_tree_example/config/example2.tree deleted file mode 100644 index 535704900..000000000 --- a/robot/ros_ws/src/behavior/behavior_tree_example/config/example2.tree +++ /dev/null @@ -1,27 +0,0 @@ -|| 2 - ? - (At Home) - [Spin] - ? - -> - (Visited Destination) - (At Home) - ? - (On Ground) - [Land] - -> - ? - (In No Fly Zone) - (At Flight Altitude) - [Takeoff] - ? - - (In No Fly Zone) - (On Ground) - [Land] - ? - (Visited Destination) - [Go To Destination] - ? - (At Home) - [Go Home] diff --git a/robot/ros_ws/src/behavior/behavior_tree_example/config/example3.tree b/robot/ros_ws/src/behavior/behavior_tree_example/config/example3.tree deleted file mode 100644 index 19229d0a4..000000000 --- a/robot/ros_ws/src/behavior/behavior_tree_example/config/example3.tree +++ /dev/null @@ -1,12 +0,0 @@ -|| 2 - ? - (At Home) - [Spin] - ? - -> - (Visited Destination) - (At Home) - ? - (On Ground) - [Land] - include $(find behavior_tree_example)/config/example3_subtree.tree diff --git a/robot/ros_ws/src/behavior/behavior_tree_example/config/example3_nested_subtree.tree b/robot/ros_ws/src/behavior/behavior_tree_example/config/example3_nested_subtree.tree deleted file mode 100644 index 923908277..000000000 --- a/robot/ros_ws/src/behavior/behavior_tree_example/config/example3_nested_subtree.tree +++ /dev/null @@ -1,3 +0,0 @@ -? - (Visited Destination) - [Go To Destination] \ No newline at end of file diff --git a/robot/ros_ws/src/behavior/behavior_tree_example/config/example3_subtree.tree b/robot/ros_ws/src/behavior/behavior_tree_example/config/example3_subtree.tree deleted file mode 100644 index a311d1d93..000000000 --- a/robot/ros_ws/src/behavior/behavior_tree_example/config/example3_subtree.tree +++ /dev/null @@ -1,14 +0,0 @@ --> - ? - (In No Fly Zone) - (At Flight Altitude) - [Takeoff] - ? - - (In No Fly Zone) - (On Ground) - [Land] - include $(find behavior_tree_example)/config/example3_nested_subtree.tree - ? - (At Home) - [Go Home] \ No newline at end of file diff --git a/robot/ros_ws/src/behavior/behavior_tree_example/config/ugv_tree.tree b/robot/ros_ws/src/behavior/behavior_tree_example/config/ugv_tree.tree deleted file mode 100644 index 84e185046..000000000 --- a/robot/ros_ws/src/behavior/behavior_tree_example/config/ugv_tree.tree +++ /dev/null @@ -1,37 +0,0 @@ -? - -> - (User Pressed Estop) - ? - (Not Stopped) - [Stop] - -> - (User Pressed Return Home Button) - ? - (At Home) - [Go To Home] - -> - (User Pressed Start Button) - ? - (Not Stopped) - [Initiate] - ? - (Not Stuck) - [Recovery 1] - [Recovery 2] - [Recovery 3] - ? - (Low Battery) - -> - ? - (Good Comms) - [Explore] - ? - (In Comms) - [Drop Comms] - [Return to Comms] - ? - (At Home) - [Go To Home] - ? - (Not Stopped) - [Stop] \ No newline at end of file diff --git a/robot/ros_ws/src/behavior/behavior_tree_example/config/ugv_tree_basic.tree b/robot/ros_ws/src/behavior/behavior_tree_example/config/ugv_tree_basic.tree deleted file mode 100644 index 5d93cae6c..000000000 --- a/robot/ros_ws/src/behavior/behavior_tree_example/config/ugv_tree_basic.tree +++ /dev/null @@ -1,25 +0,0 @@ -? - -> - (User Pressed Estop) - ? - (Not Stopped) - [Stop] - -> - (User Pressed Return Home Button) - ? - (At Home) - [Go To Home] - -> - (User Pressed Resume) - ? - (Stopped) - [Initiate] - ? - (Low Battery) - [Explore] - ? - (At Home) - [Go To Home] - ? - (Not Stopped) - [Stop] \ No newline at end of file diff --git a/robot/ros_ws/src/behavior/behavior_tree_example/launch/behavior_tree_example.xml b/robot/ros_ws/src/behavior/behavior_tree_example/launch/behavior_tree_example.xml deleted file mode 100644 index fb5ba46ab..000000000 --- a/robot/ros_ws/src/behavior/behavior_tree_example/launch/behavior_tree_example.xml +++ /dev/null @@ -1,23 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/robot/ros_ws/src/behavior/behavior_tree_example/package.xml b/robot/ros_ws/src/behavior/behavior_tree_example/package.xml deleted file mode 100644 index 56aa8345d..000000000 --- a/robot/ros_ws/src/behavior/behavior_tree_example/package.xml +++ /dev/null @@ -1,27 +0,0 @@ - - - - behavior_tree_example - 0.0.0 - TODO: Package description - john - TODO: License declaration - - ament_cmake - - rclcpp - behavior_tree_msgs - behavior_tree - std_msgs - visualization_msgs - geometry_msgs - nav_msgs - tf2 - - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/robot/ros_ws/src/behavior/behavior_tree_example/rviz/behavior_tree.perspective b/robot/ros_ws/src/behavior/behavior_tree_example/rviz/behavior_tree.perspective deleted file mode 100644 index ea05a4467..000000000 --- a/robot/ros_ws/src/behavior/behavior_tree_example/rviz/behavior_tree.perspective +++ /dev/null @@ -1,62 +0,0 @@ -{ - "keys": {}, - "groups": { - "mainwindow": { - "keys": { - "geometry": { - "repr(QByteArray.hex)": "QtCore.QByteArray(b'01d9d0cb000300000000006400000064000005600000038e0000006400000089000005600000038e00000000000000000a000000006400000089000005600000038e')", - "type": "repr(QByteArray.hex)", - "pretty-print": " d d ` d ` d ` " - }, - "state": { - "repr(QByteArray.hex)": "QtCore.QByteArray(b'000000ff00000000fd0000000100000003000004fd000002dafc0100000001fb00000042007200710074005f006200650068006100760069006f0072005f0074007200650065005f005f005000790043006f006e0073006f006c0065005f005f0031005f005f0100000000000004fd0000017300ffffff000004fd0000000000000004000000040000000800000008fc00000001000000030000000100000036004d0069006e0069006d0069007a006500640044006f0063006b00570069006400670065007400730054006f006f006c0062006100720000000000ffffffff0000000000000000')", - "type": "repr(QByteArray.hex)", - "pretty-print": " Brqt_behavior_tree__PyConsole__1__ 6MinimizedDockWidgetsToolbar " - } - }, - "groups": { - "toolbar_areas": { - "keys": { - "MinimizedDockWidgetsToolbar": { - "repr": "8", - "type": "repr" - } - }, - "groups": {} - } - } - }, - "pluginmanager": { - "keys": { - "running-plugins": { - "repr": "{'rqt_behavior_tree/PyConsole': [1]}", - "type": "repr" - } - }, - "groups": { - "plugin__rqt_behavior_tree__PyConsole__1": { - "keys": {}, - "groups": { - "dock_widget__": { - "keys": { - "dock_widget_title": { - "repr": "''", - "type": "repr" - }, - "dockable": { - "repr": "True", - "type": "repr" - }, - "parent": { - "repr": "None", - "type": "repr" - } - }, - "groups": {} - } - } - } - } - } - } -} \ No newline at end of file diff --git a/robot/ros_ws/src/behavior/behavior_tree_example/rviz/behavior_tree_example.rviz b/robot/ros_ws/src/behavior/behavior_tree_example/rviz/behavior_tree_example.rviz deleted file mode 100644 index 1159cebd2..000000000 --- a/robot/ros_ws/src/behavior/behavior_tree_example/rviz/behavior_tree_example.rviz +++ /dev/null @@ -1,210 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - Splitter Ratio: 0.5 - Tree Height: 365 - - Class: rviz_common/Selection - Name: Selection - - Class: rviz_common/Tool Properties - Expanded: - - /2D Goal Pose1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz_common/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz_default_plugins/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: 100 - Reference Frame: - Value: true - - Alpha: 1 - Class: rviz_default_plugins/PointStamped - Color: 0; 0; 255 - Enabled: true - History Length: 1 - Name: Destination - Radius: 0.4000000059604645 - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /destination - Value: true - - Alpha: 1 - Class: rviz_default_plugins/PointStamped - Color: 0; 255; 0 - Enabled: true - History Length: 1 - Name: Home - Radius: 0.4000000059604645 - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /home - Value: true - - Angle Tolerance: 0 - Class: rviz_default_plugins/Odometry - Covariance: - Orientation: - Alpha: 0.5 - Color: 255; 255; 127 - Color Style: Unique - Frame: Local - Offset: 1 - Scale: 1 - Value: true - Position: - Alpha: 0.30000001192092896 - Color: 204; 51; 204 - Scale: 1 - Value: true - Value: true - Enabled: true - Keep: 1 - Name: Robot Odometry - Position Tolerance: 0 - Shape: - Alpha: 1 - Axes Length: 1 - Axes Radius: 0.10000000149011612 - Color: 255; 25; 0 - Head Length: 0.30000001192092896 - Head Radius: 0.10000000149011612 - Shaft Length: 1 - Shaft Radius: 0.05000000074505806 - Value: Axes - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: perception/state_estimation/odometry - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: Markers - Namespaces: - destination_label: true - home_label: true - no_fly_zone: true - no_fly_zone_label: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /no_fly_zone_vis - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: world - Frame Rate: 30 - Name: root - Tools: - - Class: rviz_default_plugins/Interact - Hide Inactive Objects: true - - Class: rviz_default_plugins/MoveCamera - - Class: rviz_default_plugins/Select - - Class: rviz_default_plugins/FocusCamera - - Class: rviz_default_plugins/Measure - Line color: 128; 128; 0 - - Class: rviz_default_plugins/SetInitialPose - Covariance x: 0.25 - Covariance y: 0.25 - Covariance yaw: 0.06853891909122467 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /initialpose - - Class: rviz_default_plugins/SetGoal - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /goal_pose - - Class: rviz_default_plugins/PublishPoint - Single click: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /clicked_point - Transformation: - Current: - Class: rviz_default_plugins/TF - Value: true - Views: - Current: - Class: rviz_default_plugins/Orbit - Distance: 13.15915298461914 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: 5.043847560882568 - Y: -0.7309801578521729 - Z: 1.8234843015670776 - Focal Shape Fixed Size: false - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.06039784476161003 - Target Frame: - Value: Orbit (rviz) - Yaw: 1.57039475440979 - Saved: ~ -Window Geometry: - Displays: - collapsed: true - Height: 846 - Hide Left Dock: true - Hide Right Dock: true - QMainWindow State: 000000ff00000000fd000000040000000000000156000002f4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d000002f4000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c000002610000000100000100000002f4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002f4000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004b0000002f800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: true - Width: 1200 - X: 1182 - Y: 227 diff --git a/robot/ros_ws/src/behavior/behavior_tree_example/src/behavior_tree_example.cpp b/robot/ros_ws/src/behavior/behavior_tree_example/src/behavior_tree_example.cpp deleted file mode 100644 index 9f7ff0607..000000000 --- a/robot/ros_ws/src/behavior/behavior_tree_example/src/behavior_tree_example.cpp +++ /dev/null @@ -1,312 +0,0 @@ -// Copyright (c) 2024 Carnegie Mellon University -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include // Include the behavior tree library containing the Action and Condition classes. -#include -#include -#include -#include -#include -#include -#include -#include - -#include "tf2/LinearMath/Matrix3x3.h" -#include "tf2/LinearMath/Quaternion.h" -#include "tf2/LinearMath/Vector3.h" - -class BehaviorTreeExample : public rclcpp::Node { - private: - bool got_robot_odom, got_home, got_destination, got_no_fly_zone, got_no_fly_zone_radius; - nav_msgs::msg::Odometry robot_odom; - float flight_z, ground_z; - geometry_msgs::msg::PointStamped home, destination; - float acceptance_radius; - geometry_msgs::msg::PointStamped no_fly_zone; - float no_fly_zone_radius; - - // Condition variables - bt::Condition* at_flight_altitude_condition; - bt::Condition* on_ground_condition; - bt::Condition* visited_destination_condition; - bt::Condition* at_home_condition; - bt::Condition* in_fly_zone_condition; - bt::Condition* in_no_fly_zone_condition; - - // Action variables - bt::Action* land_action; - bt::Action* takeoff_action; - bt::Action* go_to_destination_action; - bt::Action* go_home_action; - - // subscribers - rclcpp::Subscription::SharedPtr robot_odom_sub; - rclcpp::Subscription::SharedPtr home_sub; - rclcpp::Subscription::SharedPtr destination_sub; - rclcpp::Subscription::SharedPtr no_fly_zone_sub; - rclcpp::Subscription::SharedPtr no_fly_zone_radius_sub; - - // publishers - rclcpp::Publisher::SharedPtr vel_cmd_pub; - - // timers - rclcpp::TimerBase::SharedPtr timer; - - public: - BehaviorTreeExample() : Node("behavior_tree_example") { - on_ground_condition = new bt::Condition("On Ground", this); - at_flight_altitude_condition = new bt::Condition("At Flight Altitude", this); - visited_destination_condition = new bt::Condition("Visited Destination", this); - at_home_condition = new bt::Condition("At Home", this); - in_fly_zone_condition = new bt::Condition("In Fly Zone", this); - in_no_fly_zone_condition = new bt::Condition("In No Fly Zone", this); - - land_action = new bt::Action("Land", this); - // Do the same for the "Takeoff" action. - takeoff_action = - new bt::Action("Takeoff", this, &BehaviorTreeExample::takeoff_active_callback, this, - &BehaviorTreeExample::takeoff_inactive_callback, this); - go_to_destination_action = new bt::Action("Go To Destination", this); - go_home_action = new bt::Action("Go Home", this); - - // init subscribers - robot_odom_sub = this->create_subscription( - "odometry", 10, - std::bind(&BehaviorTreeExample::robot_odom_callback, this, std::placeholders::_1)); - home_sub = this->create_subscription( - "home", 10, - std::bind(&BehaviorTreeExample::home_callback, this, std::placeholders::_1)); - destination_sub = this->create_subscription( - "destination", 10, - std::bind(&BehaviorTreeExample::destination_callback, this, std::placeholders::_1)); - - no_fly_zone_sub = this->create_subscription( - "no_fly_zone", 10, - std::bind(&BehaviorTreeExample::no_fly_zone_callback, this, std::placeholders::_1)); - no_fly_zone_radius_sub = this->create_subscription( - "no_fly_zone_radius", 10, - std::bind(&BehaviorTreeExample::no_fly_zone_radius_callback, this, - std::placeholders::_1)); - - // init publishers - vel_cmd_pub = this->create_publisher("/vel_cmd", 10); - - // initialization - got_robot_odom = false; - got_home = false; - got_destination = false; - got_no_fly_zone = false; - got_no_fly_zone_radius = false; - flight_z = 0.99; - ground_z = 0.01; - acceptance_radius = 0.01; - - // init timer - timer = this->create_wall_timer(std::chrono::milliseconds(50), - std::bind(&BehaviorTreeExample::timer_callback, this)); - } - - void timer_callback() { - if (!got_robot_odom || !got_home || !got_destination) return; - - // The condition should be success if the robot is above the flying height's z level, - // and failure otherwise. - at_flight_altitude_condition->set(robot_odom.pose.pose.position.z > flight_z); - at_flight_altitude_condition->publish(); - - // Set the condition to success if the robot is below the ground's z level, - // or to failure if the robot is above the ground's z level. - on_ground_condition->set(robot_odom.pose.pose.position.z < ground_z); - on_ground_condition->publish(); - - // The at home condition should be set to success if the robot is within - // a threshold distance of the home location and failure otherwise. - at_home_condition->set(distance(robot_odom, home) < acceptance_radius); - at_home_condition->publish(); - - if (got_no_fly_zone && got_no_fly_zone_radius) { - float distance = sqrt(pow(robot_odom.pose.pose.position.x - no_fly_zone.point.x, 2) + - pow(robot_odom.pose.pose.position.y - no_fly_zone.point.y, 2)); - - // Check if the robot is with the radius of the no fly zone point. - bool in_no_fly_zone = distance <= no_fly_zone_radius; - - in_no_fly_zone_condition->set(in_no_fly_zone); // Set the condition. - in_no_fly_zone_condition->publish(); // Publish. - - in_fly_zone_condition->set(!in_no_fly_zone); // Set the condition. - in_fly_zone_condition->publish(); // Publish. - } - - // The visited destination condition should be set to success (true) when we first - // get close enough to the destination point and remain true forever. - float destination_distance = distance(robot_odom, destination); - if (!visited_destination_condition - ->get()) // Only changed the condition value if it hasn't been set to true yet. - visited_destination_condition->set(destination_distance < acceptance_radius); - visited_destination_condition->publish(); // Publish the condition. - - if (land_action - ->is_active()) { // Make sure the robot only lands if the land action is active. - geometry_msgs::msg::TwistStamped vel_cmd; - vel_cmd.header.stamp = this->get_clock()->now(); - vel_cmd.header.frame_id = "world"; - - if (on_ground_condition->get()) { - // Set the status to success if the robot has landed. - land_action->set_success(); - // Command zero velocity when the robot is done landing. - vel_cmd.twist.linear.x = vel_cmd.twist.linear.y = vel_cmd.twist.linear.z = 0; - } else { - // Set the status to running while the robot is landing. - land_action->set_running(); - // Command downward velocity to land. - vel_cmd.twist.linear.z = -0.3; - } - - vel_cmd_pub->publish(vel_cmd); - - // Publish the land action's status. - land_action->publish(); - } - - if (go_home_action->is_active()) { // Make sure the robot tries to go home when the go home - // action is active. - geometry_msgs::msg::TwistStamped vel_cmd; - vel_cmd.header.stamp = this->get_clock()->now(); - vel_cmd.header.frame_id = "world"; - - if (at_home_condition->get()) { - // Set the status to "SUCCESS" if the robot has reached the home location - go_home_action->set_success(); - // Command zero velocity when the robot is done traveling home. - vel_cmd.twist.linear.x = vel_cmd.twist.linear.y = vel_cmd.twist.linear.z = 0; - } else { - // Set the status to "RUNNING" while the robot is moving toward the home location. - go_home_action->set_running(); - // Command a velocity towards the home location. - tf2::Vector3 vel(home.point.x - robot_odom.pose.pose.position.x, - home.point.y - robot_odom.pose.pose.position.y, 0); - vel = 0.3 * vel.normalized(); - vel_cmd.twist.linear.x = vel.x(); - vel_cmd.twist.linear.y = vel.y(); - vel_cmd.twist.linear.z = vel.z(); - } - - vel_cmd_pub->publish(vel_cmd); - - // Publish the go home action's status - go_home_action->publish(); - } - - // Make sure the robot only tries to go to the destination while the go to destination - // action is active - if (go_to_destination_action->is_active()) { - geometry_msgs::msg::TwistStamped vel_cmd; - vel_cmd.header.stamp = this->get_clock()->now(); - vel_cmd.header.frame_id = "world"; - - if (destination_distance < acceptance_radius) { - // Set the status to success if the robot has reached the destination location. - go_to_destination_action->set_success(); - // Command zero velocity when the robot is done traveling to the destination. - vel_cmd.twist.linear.x = vel_cmd.twist.linear.y = vel_cmd.twist.linear.z = 0; - } else { - // Set the status to running while the robot is moving towards the destination - // location. - go_to_destination_action->set_running(); - // Command a velocity towards the destination location. - tf2::Vector3 vel(destination.point.x - robot_odom.pose.pose.position.x, - destination.point.y - robot_odom.pose.pose.position.y, 0); - vel = 0.3 * vel.normalized(); - vel_cmd.twist.linear.x = vel.x(); - vel_cmd.twist.linear.y = vel.y(); - vel_cmd.twist.linear.z = vel.z(); - } - - vel_cmd_pub->publish(vel_cmd); - - // Publish the go to destination action's status. - go_to_destination_action->publish(); - } - } - - void takeoff_active_callback() { - geometry_msgs::msg::TwistStamped vel_cmd; - vel_cmd.header.stamp = this->get_clock()->now(); - vel_cmd.header.frame_id = "world"; - // Set the status to "RUNNING" while the take off is in progress. - takeoff_action->set_running(); - // Command upward velocity to take off. - vel_cmd.twist.linear.z = 0.3; - - vel_cmd_pub->publish(vel_cmd); - } - - void takeoff_inactive_callback() { - geometry_msgs::msg::TwistStamped vel_cmd; - vel_cmd.header.stamp = this->get_clock()->now(); - vel_cmd.header.frame_id = "world"; - // Set the status to "RUNNING" while the take off is in progress. - takeoff_action->set_running(); - // Command upward velocity to take off. - vel_cmd.twist.linear.z = 0.0; - - vel_cmd_pub->publish(vel_cmd); - } - - void robot_odom_callback(const nav_msgs::msg::Odometry::SharedPtr robot_odom) { - got_robot_odom = true; - this->robot_odom = *robot_odom; - } - - void home_callback(const geometry_msgs::msg::PointStamped::SharedPtr home) { - got_home = true; - this->home = *home; - } - - void destination_callback(const geometry_msgs::msg::PointStamped::SharedPtr destination) { - got_destination = true; - this->destination = *destination; - } - - void no_fly_zone_callback(const geometry_msgs::msg::PointStamped::SharedPtr no_fly_zone) { - got_no_fly_zone = true; - this->no_fly_zone = *no_fly_zone; - } - - void no_fly_zone_radius_callback(const std_msgs::msg::Float32::SharedPtr no_fly_zone_radius) { - got_no_fly_zone_radius = true; - this->no_fly_zone_radius = no_fly_zone_radius->data; - } - - float distance(nav_msgs::msg::Odometry odom, geometry_msgs::msg::PointStamped point) { - float distance = sqrt(pow(odom.pose.pose.position.x - point.point.x, 2) + - pow(odom.pose.pose.position.y - point.point.y, 2)); - return distance; - } -}; - -int main(int argc, char* argv[]) { - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/robot/ros_ws/src/behavior/behavior_tree_example/src/robot_node.cpp b/robot/ros_ws/src/behavior/behavior_tree_example/src/robot_node.cpp deleted file mode 100644 index 4d711bc2a..000000000 --- a/robot/ros_ws/src/behavior/behavior_tree_example/src/robot_node.cpp +++ /dev/null @@ -1,229 +0,0 @@ -// Copyright (c) 2024 Carnegie Mellon University -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include - -#include -#include -#include -#include -#include -#include -#include "tf2/LinearMath/Quaternion.h" -#include "tf2/LinearMath/Matrix3x3.h" - -class RobotNode : public rclcpp::Node -{ -private: - nav_msgs::msg::Odometry odom; - geometry_msgs::msg::TwistStamped vel_cmd, ang_vel_cmd; - rclcpp::Time last_execute_time; - geometry_msgs::msg::PointStamped home, destination, no_fly_zone; - std_msgs::msg::Float32 no_fly_zone_radius; - visualization_msgs::msg::Marker no_fly_zone_marker, no_fly_zone_label, home_label, destination_label; - visualization_msgs::msg::MarkerArray marker_array; - - // subscribers - rclcpp::Subscription::SharedPtr vel_cmd_sub; - rclcpp::Subscription::SharedPtr ang_vel_cmd_sub; - - // publishers - rclcpp::Publisher::SharedPtr robot_odom_pub; - rclcpp::Publisher::SharedPtr home_pub; - rclcpp::Publisher::SharedPtr destination_pub; - rclcpp::Publisher::SharedPtr no_fly_zone_pub; - rclcpp::Publisher::SharedPtr no_fly_zone_radius_pub; - rclcpp::Publisher::SharedPtr no_fly_zone_vis_pub; - - rclcpp::TimerBase::SharedPtr timer; - -public: - RobotNode() - : Node("robot_node"){ - // initialization - home.header.frame_id = "world"; - home.point.x = 0; - home.point.y = 0; - home.point.z = 0; - - destination.header.frame_id = "world"; - destination.point.x = 10; - destination.point.y = 0; - destination.point.z = 0; - - no_fly_zone.header.frame_id = "world"; - no_fly_zone.point.x = 5; - no_fly_zone.point.y = 0; - no_fly_zone.point.z = 0; - - no_fly_zone_radius.data = 2.f; - - float marker_height = 10; - no_fly_zone_marker.header.frame_id = "world"; - no_fly_zone_marker.ns = "no_fly_zone"; - no_fly_zone_marker.id = 0; - no_fly_zone_marker.type = visualization_msgs::msg::Marker::CYLINDER; - no_fly_zone_marker.action = visualization_msgs::msg::Marker::ADD; - no_fly_zone_marker.pose.orientation.w = 1; - no_fly_zone_marker.pose.position.x = no_fly_zone.point.x; - no_fly_zone_marker.pose.position.y = no_fly_zone.point.y; - no_fly_zone_marker.pose.position.z = marker_height/2; - no_fly_zone_marker.scale.x = 2*no_fly_zone_radius.data; - no_fly_zone_marker.scale.y = 2*no_fly_zone_radius.data; - no_fly_zone_marker.scale.z = marker_height; - no_fly_zone_marker.color.r = 1; - no_fly_zone_marker.color.g = 0; - no_fly_zone_marker.color.b = 0; - no_fly_zone_marker.color.a = 0.3; - - no_fly_zone_label.header.frame_id = "world"; - no_fly_zone_label.ns = "no_fly_zone_label"; - no_fly_zone_label.id = 0; - no_fly_zone_label.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; - no_fly_zone_label.action = visualization_msgs::msg::Marker::ADD; - no_fly_zone_label.pose.orientation.w = 1; - no_fly_zone_label.pose.position.x = no_fly_zone.point.x; - no_fly_zone_label.pose.position.y = no_fly_zone.point.y; - no_fly_zone_label.pose.position.z = marker_height/2; - no_fly_zone_label.scale.x = 1;//2*no_fly_zone_radius.data; - no_fly_zone_label.scale.y = 1;//2*no_fly_zone_radius.data; - no_fly_zone_label.scale.z = 1;//marker_height; - no_fly_zone_label.color.r = 1; - no_fly_zone_label.color.g = 1; - no_fly_zone_label.color.b = 1; - no_fly_zone_label.color.a = 1; - no_fly_zone_label.text = "No Fly Zone"; - - home_label.header.frame_id = "world"; - home_label.ns = "home_label"; - home_label.id = 0; - home_label.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; - home_label.action = visualization_msgs::msg::Marker::ADD; - home_label.pose.orientation.w = 1; - home_label.pose.position.x = home.point.x; - home_label.pose.position.y = home.point.y; - home_label.pose.position.z = home.point.z; - home_label.scale.x = 0.5; - home_label.scale.y = 0.5; - home_label.scale.z = 0.5; - home_label.color.r = 1; - home_label.color.g = 1; - home_label.color.b = 1; - home_label.color.a = 1; - home_label.text = "Home"; - - destination_label.header.frame_id = "world"; - destination_label.ns = "destination_label"; - destination_label.id = 0; - destination_label.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; - destination_label.action = visualization_msgs::msg::Marker::ADD; - destination_label.pose.orientation.w = 1; - destination_label.pose.position.x = destination.point.x; - destination_label.pose.position.y = destination.point.y; - destination_label.pose.position.z = destination.point.z; - destination_label.scale.x = 0.5; - destination_label.scale.y = 0.5; - destination_label.scale.z = 0.5; - destination_label.color.r = 1; - destination_label.color.g = 1; - destination_label.color.b = 1; - destination_label.color.a = 1; - destination_label.text = "Destination"; - - marker_array.markers.push_back(no_fly_zone_marker); - marker_array.markers.push_back(no_fly_zone_label); - marker_array.markers.push_back(home_label); - marker_array.markers.push_back(destination_label); - - odom.header.frame_id = "world"; - odom.child_frame_id = "world"; - odom.pose.pose.orientation.w = 1; - - last_execute_time = this->get_clock()->now(); - - - // init subscribers - vel_cmd_sub = this->create_subscription("vel_cmd", 10, std::bind(&RobotNode::vel_cmd_callback, this, std::placeholders::_1)); - ang_vel_cmd_sub = this->create_subscription("ang_vel_cmd", 10, std::bind(&RobotNode::ang_vel_cmd_callback, this, std::placeholders::_1)); - - // init publishers - robot_odom_pub = this->create_publisher("odometry", 10); - home_pub = this->create_publisher("home", 10); - destination_pub = this->create_publisher("destination", 10); - no_fly_zone_pub = this->create_publisher("no_fly_zone", 10); - no_fly_zone_radius_pub = this->create_publisher("no_fly_zone_radius", 10); - no_fly_zone_vis_pub = this->create_publisher("no_fly_zone_vis", 10); - - // init timer - timer = this->create_wall_timer(std::chrono::milliseconds(50), std::bind(&RobotNode::timer_callback, this)); - } - - void timer_callback(){ - // keep track of time - rclcpp::Time now = this->get_clock()->now(); - double dt = (now - last_execute_time).seconds(); - last_execute_time = now; - - // update the robot's position based on the velocity command - odom.pose.pose.position.x += vel_cmd.twist.linear.x*dt; - odom.pose.pose.position.y += vel_cmd.twist.linear.y*dt; - odom.pose.pose.position.z += vel_cmd.twist.linear.z*dt; - - - // update the robot's orientation based on the velocity command - double roll, pitch, yaw; - tf2::Quaternion q(odom.pose.pose.orientation.x, odom.pose.pose.orientation.y, odom.pose.pose.orientation.z, odom.pose.pose.orientation.w); - tf2::Matrix3x3 m(q); - m.getRPY(roll, pitch, yaw); - roll += ang_vel_cmd.twist.angular.x*dt; - pitch += ang_vel_cmd.twist.angular.y*dt; - yaw += ang_vel_cmd.twist.angular.z*dt; - q.setRPY(roll, pitch, yaw); - odom.pose.pose.orientation.x = q.x(); - odom.pose.pose.orientation.y = q.y(); - odom.pose.pose.orientation.z = q.z(); - odom.pose.pose.orientation.w = q.w(); - - // publish - odom.header.stamp = home.header.stamp = destination.header.stamp = no_fly_zone.header.stamp = no_fly_zone_marker.header.stamp = this->get_clock()->now(); - robot_odom_pub->publish(odom); - home_pub->publish(home); - destination_pub->publish(destination); - no_fly_zone_pub->publish(no_fly_zone); - no_fly_zone_radius_pub->publish(no_fly_zone_radius); - no_fly_zone_vis_pub->publish(marker_array);//no_fly_zone_marker); - } - - void vel_cmd_callback(const geometry_msgs::msg::TwistStamped::SharedPtr twist){ - vel_cmd = *twist; - } - - void ang_vel_cmd_callback(const geometry_msgs::msg::TwistStamped::SharedPtr twist){ - ang_vel_cmd = *twist; - } -}; - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/robot/ros_ws/src/interface/drone_safety_monitor/CMakeLists.txt b/robot/ros_ws/src/behavior/drone_safety_monitor/CMakeLists.txt similarity index 60% rename from robot/ros_ws/src/interface/drone_safety_monitor/CMakeLists.txt rename to robot/ros_ws/src/behavior/drone_safety_monitor/CMakeLists.txt index 503f0e46d..d9350e958 100644 --- a/robot/ros_ws/src/interface/drone_safety_monitor/CMakeLists.txt +++ b/robot/ros_ws/src/behavior/drone_safety_monitor/CMakeLists.txt @@ -5,51 +5,33 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -# find dependencies find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) +find_package(nav_msgs REQUIRED) find_package(airstack_msgs REQUIRED) find_package(airstack_common REQUIRED) -find_package(trajectory_library REQUIRED) -find_package(mavros_msgs REQUIRED) -find_package(std_srvs REQUIRED) add_executable(drone_safety_monitor src/drone_safety_monitor.cpp) target_include_directories(drone_safety_monitor PUBLIC $ $) -target_compile_features(drone_safety_monitor PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +target_compile_features(drone_safety_monitor PUBLIC c_std_99 cxx_std_17) ament_target_dependencies( drone_safety_monitor + rclcpp std_msgs + nav_msgs airstack_msgs airstack_common - trajectory_library - mavros_msgs - std_srvs ) install(TARGETS drone_safety_monitor DESTINATION lib/${PROJECT_NAME}) -install(DIRECTORY - launch - DESTINATION share/${PROJECT_NAME}/ - ) - -install(DIRECTORY - config - DESTINATION share/${PROJECT_NAME}) - if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files set(ament_cmake_cpplint_FOUND TRUE) ament_lint_auto_find_test_dependencies() endif() diff --git a/robot/ros_ws/src/interface/drone_safety_monitor/include/drone_safety_monitor/drone_safety_monitor.hpp b/robot/ros_ws/src/behavior/drone_safety_monitor/include/drone_safety_monitor/drone_safety_monitor.hpp similarity index 83% rename from robot/ros_ws/src/interface/drone_safety_monitor/include/drone_safety_monitor/drone_safety_monitor.hpp rename to robot/ros_ws/src/behavior/drone_safety_monitor/include/drone_safety_monitor/drone_safety_monitor.hpp index 48e11fc1c..4b25a4abd 100644 --- a/robot/ros_ws/src/interface/drone_safety_monitor/include/drone_safety_monitor/drone_safety_monitor.hpp +++ b/robot/ros_ws/src/behavior/drone_safety_monitor/include/drone_safety_monitor/drone_safety_monitor.hpp @@ -20,17 +20,17 @@ #pragma once -#include -#include -#include -#include #include -// ===================================================================================== -// ---------------------------------- TimeChecker -------------------------------------- -// ===================================================================================== +#include +#include +#include +#include +#include +#include -class TimeChecker{ +class TimeChecker +{ private: rclcpp::Time time; bool time_initialized; @@ -39,4 +39,4 @@ class TimeChecker{ TimeChecker(); void update(rclcpp::Time time); double elapsed_since_last_update(rclcpp::Time time); -}; +}; diff --git a/robot/ros_ws/src/interface/drone_safety_monitor/package.xml b/robot/ros_ws/src/behavior/drone_safety_monitor/package.xml similarity index 75% rename from robot/ros_ws/src/interface/drone_safety_monitor/package.xml rename to robot/ros_ws/src/behavior/drone_safety_monitor/package.xml index 54c32c903..2ac348759 100644 --- a/robot/ros_ws/src/interface/drone_safety_monitor/package.xml +++ b/robot/ros_ws/src/behavior/drone_safety_monitor/package.xml @@ -3,20 +3,18 @@ drone_safety_monitor 0.0.0 - TODO: Package description + Safety monitor: state estimate watchdog and pause/resume/rewind command handling uav - TODO: License declaration + MIT ament_cmake rclcpp std_msgs + nav_msgs airstack_msgs airstack_common - trajectory_library - mavros_msgs - std_srvs - + ament_lint_auto ament_lint_common diff --git a/robot/ros_ws/src/behavior/drone_safety_monitor/src/drone_safety_monitor.cpp b/robot/ros_ws/src/behavior/drone_safety_monitor/src/drone_safety_monitor.cpp new file mode 100644 index 000000000..595724523 --- /dev/null +++ b/robot/ros_ws/src/behavior/drone_safety_monitor/src/drone_safety_monitor.cpp @@ -0,0 +1,147 @@ +// Copyright (c) 2024 Carnegie Mellon University +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#include + +TimeChecker::TimeChecker() +: time_initialized(false) {} + +void TimeChecker::update(rclcpp::Time time) +{ + this->time = time; + time_initialized = true; +} + +double TimeChecker::elapsed_since_last_update(rclcpp::Time time) +{ + if (!time_initialized) { + return std::numeric_limits::infinity(); + } + return (time - this->time).seconds(); +} + + +class DroneSafetyMonitorNode : public rclcpp::Node +{ +public: + DroneSafetyMonitorNode() + : Node("drone_safety_monitor") + { + state_estimate_timeout_ = airstack::get_param(this, "state_estimate_timeout", 1.0); + + // subscribers + state_estimate_sub_ = this->create_subscription( + "state_estimate", 10, + std::bind(&DroneSafetyMonitorNode::state_estimate_callback, this, std::placeholders::_1)); + + command_sub_ = this->create_subscription( + "command", 10, + std::bind(&DroneSafetyMonitorNode::command_callback, this, std::placeholders::_1)); + + // publishers + state_estimate_timed_out_pub_ = + this->create_publisher("state_estimate_timed_out", 10); + + // service client + traj_mode_client_ = + this->create_client("set_trajectory_mode"); + + // timer at 1 Hz + timer_ = rclcpp::create_timer( + this, this->get_clock(), std::chrono::milliseconds(1000), + std::bind(&DroneSafetyMonitorNode::timer_callback, this)); + + RCLCPP_INFO(this->get_logger(), "DroneSafetyMonitorNode started. " + "Publish to 'command' topic with: pause | resume | rewind"); + } + +private: + float state_estimate_timeout_; + bool was_timed_out_{false}; + + TimeChecker state_estimate_time_checker_; + + rclcpp::Subscription::SharedPtr state_estimate_sub_; + rclcpp::Subscription::SharedPtr command_sub_; + rclcpp::Publisher::SharedPtr state_estimate_timed_out_pub_; + rclcpp::Client::SharedPtr traj_mode_client_; + rclcpp::TimerBase::SharedPtr timer_; + + void set_trajectory_mode(int32_t mode) + { + if (!traj_mode_client_->wait_for_service(std::chrono::seconds(1))) { + RCLCPP_WARN(this->get_logger(), "set_trajectory_mode service not available"); + return; + } + auto req = std::make_shared(); + req->mode = mode; + traj_mode_client_->async_send_request(req); + } + + void state_estimate_callback(const nav_msgs::msg::Odometry::SharedPtr) + { + state_estimate_time_checker_.update(this->get_clock()->now()); + } + + void command_callback(const std_msgs::msg::String::SharedPtr msg) + { + const std::string & cmd = msg->data; + if (cmd == "pause") { + RCLCPP_INFO(this->get_logger(), "Safety command: PAUSE"); + set_trajectory_mode(airstack_msgs::srv::TrajectoryMode::Request::PAUSE); + } else if (cmd == "resume") { + RCLCPP_INFO(this->get_logger(), "Safety command: RESUME"); + set_trajectory_mode(airstack_msgs::srv::TrajectoryMode::Request::ROBOT_POSE); + } else if (cmd == "rewind") { + RCLCPP_INFO(this->get_logger(), "Safety command: REWIND"); + set_trajectory_mode(airstack_msgs::srv::TrajectoryMode::Request::REWIND); + } else { + RCLCPP_WARN(this->get_logger(), + "Unknown safety command: '%s'. Valid commands: pause | resume | rewind", cmd.c_str()); + } + } + + void timer_callback() + { + bool timed_out = + state_estimate_time_checker_.elapsed_since_last_update(this->get_clock()->now()) > + state_estimate_timeout_; + + std_msgs::msg::Bool msg; + msg.data = timed_out; + state_estimate_timed_out_pub_->publish(msg); + + // auto-pause if state estimate just timed out + if (timed_out && !was_timed_out_) { + RCLCPP_WARN(this->get_logger(), + "State estimate timed out! Auto-pausing trajectory controller."); + set_trajectory_mode(airstack_msgs::srv::TrajectoryMode::Request::PAUSE); + } + was_timed_out_ = timed_out; + } +}; + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/CHANGELOG.rst b/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/CHANGELOG.rst deleted file mode 100644 index 0a789b786..000000000 --- a/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/CHANGELOG.rst +++ /dev/null @@ -1,149 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package rqt_py_console -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -1.0.2 (2021-08-31) ------------------- -* Fix modern setuptools warning about dashes instead of underscores (`#11 `_) -* Contributors: Chris Lalancette - -1.0.1 (2021-04-27) ------------------- -* Changed the build type to ament_python and fixed package to run with ros2 run (`#8 `_) -* Contributors: Alejandro Hernández Cordero - -1.0.0 (2018-12-11) ------------------- -* spyderlib -> spyder (`#5 `_) -* ros2 port (`#3 `_) -* autopep8 (`#2 `_) -* Contributors: Mike Lautman - -0.4.8 (2017-04-28) ------------------- - -0.4.7 (2017-03-02) ------------------- - -0.4.6 (2017-02-27) ------------------- - -0.4.5 (2017-02-03) ------------------- - -0.4.4 (2017-01-24) ------------------- -* use Python 3 compatible syntax (`#421 `_) - -0.4.3 (2016-11-02) ------------------- - -0.4.2 (2016-09-19) ------------------- - -0.4.1 (2016-05-16) ------------------- - -0.4.0 (2016-04-27) ------------------- -* Support Qt 5 (in Kinetic and higher) as well as Qt 4 (in Jade and earlier) (`#359 `_) - -0.3.13 (2016-03-08) -------------------- - -0.3.12 (2015-07-24) -------------------- - -0.3.11 (2015-04-30) -------------------- - -0.3.10 (2014-10-01) -------------------- -* update plugin scripts to use full name to avoid future naming collisions - -0.3.9 (2014-08-18) ------------------- - -0.3.8 (2014-07-15) ------------------- - -0.3.7 (2014-07-11) ------------------- -* export architecture_independent flag in package.xml (`#254 `_) - -0.3.6 (2014-06-02) ------------------- - -0.3.5 (2014-05-07) ------------------- - -0.3.4 (2014-01-28) ------------------- - -0.3.3 (2014-01-08) ------------------- -* add groups for rqt plugins, renamed some plugins (`#167 `_) - -0.3.2 (2013-10-14) ------------------- - -0.3.1 (2013-10-09) ------------------- - -0.3.0 (2013-08-28) ------------------- - -0.2.17 (2013-07-04) -------------------- - -0.2.16 (2013-04-09 13:33) -------------------------- - -0.2.15 (2013-04-09 00:02) -------------------------- - -0.2.14 (2013-03-14) -------------------- - -0.2.13 (2013-03-11 22:14) -------------------------- - -0.2.12 (2013-03-11 13:56) -------------------------- - -0.2.11 (2013-03-08) -------------------- - -0.2.10 (2013-01-22) -------------------- - -0.2.9 (2013-01-17) ------------------- - -0.2.8 (2013-01-11) ------------------- - -0.2.7 (2012-12-24) ------------------- - -0.2.6 (2012-12-23) ------------------- - -0.2.5 (2012-12-21 19:11) ------------------------- - -0.2.4 (2012-12-21 01:13) ------------------------- - -0.2.3 (2012-12-21 00:24) ------------------------- - -0.2.2 (2012-12-20 18:29) ------------------------- - -0.2.1 (2012-12-20 17:47) ------------------------- - -0.2.0 (2012-12-20 17:39) ------------------------- -* first release of this package into groovy diff --git a/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/README.md b/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/README.md deleted file mode 100644 index 82f4ee4c9..000000000 --- a/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/README.md +++ /dev/null @@ -1,19 +0,0 @@ -# RQT Python FixedTrajectoryGenerator - -If you `colcon build` this package in a workspace and then run "rqt --force-discover" after sourcing the workspace, the plugin should show up as "Fixed Trajectory Generator" in "Miscellaneous Tools" in the "Plugins" menu. - -You can use the `generate_rqt_py_package.sh` script to generate a new package by doing the following from the rqt_fixed_trajectory_generator directory - -``` -./generate_rqt_py_package.sh [package name] [class name] [plugin title] -``` - -[package name] will be the name of the package and a directory with this name will be created above `rqt_fixed_trajectory_generator/`. [class name] is the name of the class in `src/[package name]/template.py`. [plugin title] is what the plugin will be called in the "Miscellaneous Tools" menu. - -For example, - -``` -cd rqt_fixed_trajectory_generator/ -./generate_rqt_py_package.sh new_rqt_package ClassName "Plugin Title" -``` - diff --git a/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/config/fixed_trajectories.yaml b/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/config/fixed_trajectories.yaml deleted file mode 100644 index 0fdeb0af2..000000000 --- a/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/config/fixed_trajectories.yaml +++ /dev/null @@ -1,47 +0,0 @@ -trajectories: - - Figure8: - attributes: - - frame_id - - velocity - - max_acceleration - - length - - width - - height - - Racetrack: - attributes: - - frame_id - - velocity - - turn_velocity - - max_acceleration - - length - - width - - height - - Circle: - attributes: - - frame_id - - velocity - - radius - - Line: - attributes: - - frame_id - - velocity - - max_acceleration - - length - - width - - height - - Point: - attributes: - - frame_id - - velocity - - max_acceleration - - x - - y - - height - - Lawnmower: - attributes: - - frame_id - - length - - width - - height - - velocity - - vertical diff --git a/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/package.xml b/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/package.xml deleted file mode 100644 index 8f12f2f8d..000000000 --- a/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/package.xml +++ /dev/null @@ -1,28 +0,0 @@ - - rqt_fixed_trajectory_generator - 1.0.2 - rqt_fixed_trajectory_generator is a Python GUI template. - John Keller - - BSD - - - - - - John Keller - - ament_index_python - python_qt_binding - qt_gui - qt_gui_py_common - rclpy - rqt_gui - rqt_gui_py - - - - - ament_python - - diff --git a/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/plugin.xml b/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/plugin.xml deleted file mode 100644 index ec1f2a2b4..000000000 --- a/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/plugin.xml +++ /dev/null @@ -1,17 +0,0 @@ - - - - A Python GUI plugin providing an interactive Python console. - - - - - folder - Plugins related to miscellaneous tools. - - - applications-python - A Python RQT GUI template. - - - diff --git a/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/resource/py_console_widget.ui b/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/resource/py_console_widget.ui deleted file mode 100644 index 12810f1c5..000000000 --- a/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/resource/py_console_widget.ui +++ /dev/null @@ -1,53 +0,0 @@ - - - PyConsole - - - - 0 - 0 - 276 - 212 - - - - PyConsole - - - - 0 - - - 0 - - - 0 - - - 3 - - - 0 - - - - - 0 - - - - - - - - - - - PyConsoleTextEdit - QTextEdit -
rqt_py_console.py_console_text_edit
-
-
- - -
diff --git a/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/resource/rqt_fixed_trajectory_generator b/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/resource/rqt_fixed_trajectory_generator deleted file mode 100644 index e69de29bb..000000000 diff --git a/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/setup.cfg b/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/setup.cfg deleted file mode 100644 index 2394a82ac..000000000 --- a/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/rqt_fixed_trajectory_generator -[install] -install_scripts=$base/lib/rqt_fixed_trajectory_generator diff --git a/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/setup.py b/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/setup.py deleted file mode 100644 index 12c8b1c23..000000000 --- a/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/setup.py +++ /dev/null @@ -1,41 +0,0 @@ -from setuptools import setup -from glob import glob - -package_name = 'rqt_fixed_trajectory_generator' - -setup( - name=package_name, - version='1.0.2', - packages=[package_name], - package_dir={'': 'src'}, - data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ('share/' + package_name + '/resource', - ['resource/py_console_widget.ui']), - ('share/' + package_name, ['package.xml']), - ('share/' + package_name, ['plugin.xml']), - ('share/' + package_name + '/config/', glob('config/*')), - ], - install_requires=['setuptools'], - zip_safe=True, - author='', - maintainer='', - maintainer_email='', - keywords=['ROS'], - classifiers=[ - '', - '', - '', - '', - ], - description=( - 'rqt_fixed_trajectory_generator' - ), - license='BSD', - entry_points={ - 'console_scripts': [ - 'rqt_fixed_trajectory_generator = ' + package_name + '.main:main', - ], - }, -) diff --git a/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/src/rqt_fixed_trajectory_generator/main.py b/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/src/rqt_fixed_trajectory_generator/main.py deleted file mode 100755 index 9a5c9376e..000000000 --- a/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/src/rqt_fixed_trajectory_generator/main.py +++ /dev/null @@ -1,12 +0,0 @@ -import sys - -from rqt_gui.main import Main - - -def main(): - main = Main() - sys.exit(main.main(sys.argv, standalone='rqt_py_console.py_console.PyConsole')) - - -if __name__ == '__main__': - main() diff --git a/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/src/rqt_fixed_trajectory_generator/py_console_text_edit.py b/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/src/rqt_fixed_trajectory_generator/py_console_text_edit.py deleted file mode 100644 index dc9ce1a0a..000000000 --- a/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/src/rqt_fixed_trajectory_generator/py_console_text_edit.py +++ /dev/null @@ -1,69 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Dorian Scholz -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import sys -from code import InteractiveInterpreter - -from python_qt_binding import QT_BINDING, QT_BINDING_VERSION -from python_qt_binding.QtCore import Qt, Signal - -from qt_gui_py_common.console_text_edit import ConsoleTextEdit - - -class PyConsoleTextEdit(ConsoleTextEdit): - _color_stdin = Qt.darkGreen - _multi_line_char = ':' - _multi_line_indent = ' ' - _prompt = ('>>> ', '... ') # prompt for single and multi line - exit = Signal() - - def __init__(self, parent=None): - super(PyConsoleTextEdit, self).__init__(parent) - - self._interpreter_locals = {} - self._interpreter = InteractiveInterpreter(self._interpreter_locals) - - self._comment_writer.write('Python %s on %s\n' % - (sys.version.replace('\n', ''), sys.platform)) - self._comment_writer.write( - 'Qt bindings: %s version %s\n' % (QT_BINDING, QT_BINDING_VERSION)) - - self._add_prompt() - - def update_interpreter_locals(self, newLocals): - self._interpreter_locals.update(newLocals) - - def _exec_code(self, code): - try: - self._interpreter.runsource(code) - except SystemExit: # catch sys.exit() calls, so they don't close the whole gui - self.exit.emit() diff --git a/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/src/rqt_fixed_trajectory_generator/py_console_widget.py b/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/src/rqt_fixed_trajectory_generator/py_console_widget.py deleted file mode 100644 index e69bde34c..000000000 --- a/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/src/rqt_fixed_trajectory_generator/py_console_widget.py +++ /dev/null @@ -1,59 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Dorian Scholz -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import os -from ament_index_python.resources import get_resource - -from python_qt_binding import loadUi -from python_qt_binding.QtWidgets import QWidget -from rqt_py_console.py_console_text_edit import PyConsoleTextEdit - - -class PyConsoleWidget(QWidget): - - def __init__(self, context=None): - super(PyConsoleWidget, self).__init__() - - _, package_path = get_resource('packages', 'rqt_py_console') - ui_file = os.path.join( - package_path, 'share', 'rqt_py_console', 'resource', 'py_console_widget.ui') - - loadUi(ui_file, self, {'PyConsoleTextEdit': PyConsoleTextEdit}) - self.setObjectName('PyConsoleWidget') - - my_locals = { - 'context': context - } - self.py_console.update_interpreter_locals(my_locals) - self.py_console.print_message( - 'The variable "context" is set to the PluginContext of this plugin.') - self.py_console.exit.connect(context.close_plugin) diff --git a/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/src/rqt_fixed_trajectory_generator/spyder_console_widget.py b/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/src/rqt_fixed_trajectory_generator/spyder_console_widget.py deleted file mode 100644 index 374ef7a5d..000000000 --- a/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/src/rqt_fixed_trajectory_generator/spyder_console_widget.py +++ /dev/null @@ -1,60 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Dorian Scholz -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from python_qt_binding.QtGui import QFont - -from spyder.widgets.internalshell import InternalShell -from spyder.utils.module_completion import moduleCompletion - -class SpyderConsoleWidget(InternalShell): - - def __init__(self, context=None): - my_locals = { - 'context': context - } - super(SpyderConsoleWidget, self).__init__(namespace=my_locals) - self.setObjectName('SpyderConsoleWidget') - self.set_pythonshell_font(QFont('Mono')) - self.interpreter.restore_stds() - - def get_module_completion(self, objtxt): - """Return module completion list associated to object name""" - return moduleCompletion(objtxt) - - def run_command(self, *args): - self.interpreter.redirect_stds() - super(SpyderConsoleWidget, self).run_command(*args) - self.flush() - self.interpreter.restore_stds() - - def shutdown(self): - self.exit_interpreter() diff --git a/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/src/rqt_fixed_trajectory_generator/template.py b/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/src/rqt_fixed_trajectory_generator/template.py deleted file mode 100644 index 76c60c770..000000000 --- a/robot/ros_ws/src/behavior/rqt_fixed_trajectory_generator/src/rqt_fixed_trajectory_generator/template.py +++ /dev/null @@ -1,269 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Dorian Scholz -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from python_qt_binding.QtWidgets import QVBoxLayout, QWidget -from rqt_gui_py.plugin import Plugin -from qt_gui_py_common.simple_settings_dialog import SimpleSettingsDialog -from rqt_py_console.py_console_widget import PyConsoleWidget - -import python_qt_binding.QtWidgets as qt -import python_qt_binding.QtWidgets as QtWidgets -import python_qt_binding.QtGui as QtGui -import python_qt_binding.QtCore as QtCore - -import os -import time -import numpy as np -import yaml -import collections -import pickle -from ament_index_python.packages import get_package_share_directory - -from std_msgs.msg import Bool -from airstack_msgs.msg import FixedTrajectory -from diagnostic_msgs.msg import KeyValue - -try: - from rqt_py_console.spyder_console_widget import SpyderConsoleWidget - _has_spyderlib = True -except ImportError: - _has_spyderlib = False - - -class FixedTrajectoryGenerator(Plugin): - """ - Plugin providing an interactive Python console - """ - - def __init__(self, context): - super(FixedTrajectoryGenerator, self).__init__(context) - self.setObjectName('FixedTrajectoryGenerator') - - self.context = context - # to access ros2 node use self.context.node - - self.config_filename = '' - - self.button_dct = {} - self.attribute_settings = collections.OrderedDict() - - self.timer = self.context.node.create_timer(1./10., self.timer_callback) - - self.fixed_trajectory_pub = self.context.node.create_publisher(FixedTrajectory, 'fixed_trajectory_command', 1) - self.global_plan_fixed_trajectory_pub = self.context.node.create_publisher(FixedTrajectory, 'global_plan_fixed_trajectory', 1) - - # main layout - self.widget = QWidget() - self.vbox = qt.QVBoxLayout() - self.widget.setLayout(self.vbox) - context.add_widget(self.widget) - - # config widget - self.config_widget = qt.QWidget() - self.config_widget.setStyleSheet('QWidget{margin-left:-1px;}') - self.config_layout = qt.QHBoxLayout() - self.config_widget.setLayout(self.config_layout) - self.config_widget.setFixedHeight(50) - - self.config_button = qt.QPushButton('Open Config...') - self.config_button.clicked.connect(self.select_config_file) - self.config_layout.addWidget(self.config_button) - - self.config_label = qt.QLabel('config filename: ') - self.config_layout.addWidget(self.config_label) - self.vbox.addWidget(self.config_widget) - - # trajectory widget - self.trajectory_widget = qt.QWidget() - self.trajectory_layout = qt.QVBoxLayout() - self.trajectory_widget.setLayout(self.trajectory_layout) - self.vbox.addWidget(self.trajectory_widget) - - self.tab_widget = qt.QTabWidget() - self.trajectory_layout.addWidget(self.tab_widget) - - # button widget - self.button_widget = qt.QWidget() - self.button_layout = qt.QHBoxLayout() - self.button_widget.setLayout(self.button_layout) - self.vbox.addWidget(self.button_widget) - - self.publish_button = qt.QPushButton('Publish') - self.publish_button.clicked.connect(self.publish_trajectory) - self.button_layout.addWidget(self.publish_button) - - self.trajectory_type_label = qt.QLabel('Type: ') - self.button_layout.addWidget(self.trajectory_type_label) - - self.trajectory_type_combo_box = qt.QComboBox() - self.trajectory_type_combo_box.addItem('Fixed Trajectory') - self.trajectory_type_combo_box.addItem('Global Plan') - self.button_layout.addWidget(self.trajectory_type_combo_box) - - def publish_trajectory(self): - trajectory_type = self.trajectory_type_combo_box.currentText() - trajectory_name = self.tab_widget.tabText(self.tab_widget.currentIndex()) - msg = FixedTrajectory() - msg.type = trajectory_name - for attribute, value in iter(self.attribute_settings[trajectory_name].items()): - key_value = KeyValue() - key_value.key = attribute - key_value.value = value - msg.attributes.append(key_value) - if trajectory_type == 'Fixed Trajectory': - self.fixed_trajectory_pub.publish(msg) - elif trajectory_type == 'Global Plan': - self.global_plan_fixed_trajectory_pub.publish(msg) - - def select_config_file(self): - starting_path = get_package_share_directory('rqt_fixed_trajectory_generator') + '/config/' - print(starting_path) - filename = qt.QFileDialog.getOpenFileName(self.widget, 'Open Config File', starting_path, "Config Files (*.yaml)")[0] - self.set_config(filename) - - def set_config(self, filename): - if filename != '': - self.config_filename = filename - if self.config_filename != None: - self.config_label.setText('config filename: ' + os.path.basename(self.config_filename)) - self.init_buttons(filename) - - def init_buttons(self, filename): - y = yaml.load(open(filename, 'r').read(), Loader=yaml.Loader) - print(y) - - def get_attribute_changed_function(trajectory_name, attribute_name): - def attribute_changed(text): - if trajectory_name not in self.attribute_settings: - self.attribute_settings[trajectory_name] = {} - self.attribute_settings[trajectory_name][attribute_name] = text - return attribute_changed - - def get_publish_function(trajectory_name): - def publish_function(): - msg = FixedTrajectory() - msg.type = trajectory_name - for attribute, value in iter(self.attribute_settings[trajectory_name].items()): - key_value = KeyValue() - key_value.key = attribute - key_value.value = value - msg.attributes.append(key_value) - self.fixed_trajectory_pub.publish(msg) - return publish_function - - - for trajectory in y['trajectories']: - trajectory_name = list(trajectory.keys())[0] - attributes = trajectory[trajectory_name]['attributes'] - - trajectory_tab = qt.QWidget() - trajectory_layout = qt.QVBoxLayout() - trajectory_tab.setLayout(trajectory_layout) - - for attribute in attributes: - attribute_widget = qt.QWidget() - attribute_layout = qt.QHBoxLayout() - attribute_widget.setLayout(attribute_layout) - - attribute_label = qt.QLabel() - attribute_label.setText(attribute) - attribute_layout.addWidget(attribute_label) - - attribute_default = '0' - if attribute == 'frame_id': - attribute_default = 'world' - if trajectory_name in self.attribute_settings.keys(): - if attribute in self.attribute_settings[trajectory_name].keys(): - attribute_default = self.attribute_settings[trajectory_name][attribute] - - attribute_edit = qt.QLineEdit() - attribute_edit.textChanged.connect(get_attribute_changed_function(trajectory_name, - attribute)) - attribute_edit.setText(attribute_default) - - attribute_layout.addWidget(attribute_edit) - - trajectory_layout.addWidget(attribute_widget) - - #publish_button = qt.QPushButton('Publish') - #publish_button.clicked.connect(get_publish_function(trajectory_name)) - #trajectory_layout.addWidget(publish_button) - - self.tab_widget.addTab(trajectory_tab, trajectory_name) - - - - def timer_callback(self): - bool_msg = Bool() - for key in self.button_dct.keys(): - bool_msg.data = self.button_dct[key]['data'] - self.button_dct[key]['publisher'].publish(bool_msg) - - def save_settings(self, plugin_settings, instance_settings): - instance_settings.set_value('config_filename', self.config_filename) - instance_settings.set_value('attribute_settings', pickle.dumps(self.attribute_settings)) - - def restore_settings(self, plugin_settings, instance_settings): - attribute_settings = instance_settings.value('attribute_settings') - if attribute_settings != None: - self.attribute_settings = pickle.loads(attribute_settings) - self.set_config(instance_settings.value('config_filename')) - - ''' - def save_settings(self, plugin_settings, instance_settings): - pass - #instance_settings.set_value('some_variable', self.some_variable) - - def restore_settings(self, plugin_settings, instance_settings): - pass - #self.some_variable = instance_settings.value('some_variable', default_value) - ''' - def trigger_configuration(self): - options = [ - {'title': 'Option 1', - 'description': 'Description of option 1.', - 'enabled': True}, - {'title': 'Option 2', - 'description': 'Description of option 2.'}, - ] - dialog = SimpleSettingsDialog(title='Options') - dialog.add_exclusive_option_group(title='List of options:', options=options, selected_index=0) - selected_index = dialog.get_settings()[0] - if selected_index != None: - selected_index = selected_index['selected_index'] - print('selected_index ', selected_index) - - def shutdown_console_widget(self): - pass - - def shutdown_plugin(self): - self.shutdown_console_widget() diff --git a/robot/ros_ws/src/global/planners/exploration/launch/robot_launch_gazebo/gz_behavior_launch.xml b/robot/ros_ws/src/global/planners/exploration/launch/robot_launch_gazebo/gz_behavior_launch.xml index 549a166ca..cef093702 100644 --- a/robot/ros_ws/src/global/planners/exploration/launch/robot_launch_gazebo/gz_behavior_launch.xml +++ b/robot/ros_ws/src/global/planners/exploration/launch/robot_launch_gazebo/gz_behavior_launch.xml @@ -2,31 +2,14 @@ - - - - + + + + + - - - - - - - - - - - - - - - - - - - - diff --git a/robot/ros_ws/src/global/planners/exploration/launch/robot_launch_gazebo/gz_local_launch.xml b/robot/ros_ws/src/global/planners/exploration/launch/robot_launch_gazebo/gz_local_launch.xml index 0e0c9dd76..93217a883 100644 --- a/robot/ros_ws/src/global/planners/exploration/launch/robot_launch_gazebo/gz_local_launch.xml +++ b/robot/ros_ws/src/global/planners/exploration/launch/robot_launch_gazebo/gz_local_launch.xml @@ -8,29 +8,39 @@ - - + + + + + + + + + + + + - - - - - + + + - - + diff --git a/robot/ros_ws/src/interface/drone_safety_monitor/README.md b/robot/ros_ws/src/interface/drone_safety_monitor/README.md deleted file mode 100644 index efb46f285..000000000 --- a/robot/ros_ws/src/interface/drone_safety_monitor/README.md +++ /dev/null @@ -1,114 +0,0 @@ -# drone_safety_monitor - -A ROS 2 package that monitors critical drone safety conditions and publishes fault flags to the rest of the autonomy stack. Currently the package watches the state estimate stream and raises a timeout flag whenever odometry stops arriving, allowing downstream nodes (e.g. the behavior executive) to react safely. - -## Overview - -| Item | Value | -|---|---| -| Package name | `drone_safety_monitor` | -| Node name | `odom_timeout_checker` | -| Language | C++ 17 | -| ROS distro | ROS 2 (Humble / Iron) | -| License | MIT | - -## Nodes - -### `drone_safety_monitor` (executable → node name `odom_timeout_checker`) - -Monitors the incoming state estimate (odometry) and publishes a Boolean timeout flag once per second. - -**How it works** - -1. Every time an odometry message is received on `state_estimate`, the internal `TimeChecker` records the current ROS clock time. -2. A 1 Hz timer fires every second and computes how long ago the last odometry message arrived. -3. If that elapsed time exceeds `state_estimate_timeout`, the node publishes `true` on `state_estimate_timed_out`; otherwise it publishes `false`. -4. If no odometry has ever been received (e.g. immediately after startup), the elapsed time is treated as infinite, so the timeout flag is raised immediately. - -#### Subscribed Topics - -| Topic | Type | Description | -|---|---|---| -| `state_estimate` | `nav_msgs/msg/Odometry` | Incoming odometry / state estimate from the localization system | - -#### Published Topics - -| Topic | Type | Description | -|---|---|---| -| `state_estimate_timed_out` | `std_msgs/msg/Bool` | `true` when the state estimate has not been received within `state_estimate_timeout` seconds; `false` otherwise. Published at 1 Hz. | - -#### Parameters - -| Parameter | Type | Default | Description | -|---|---|---|---| -| `state_estimate_timeout` | `double` | `1.0` | Maximum acceptable gap (in seconds) between consecutive state estimate messages before the timeout flag is raised | - -## Dependencies - -| Package | Purpose | -|---|---| -| `rclcpp` | ROS 2 C++ client library | -| `std_msgs` | `Bool` message type for the timeout flag | -| `nav_msgs` | `Odometry` message type for the state estimate | -| `airstack_common` | AirStack helper utilities (`airstack::get_param`) | -| `airstack_msgs` | AirStack custom message types | -| `trajectory_library` | Trajectory utilities (linked at build time) | -| `mavros_msgs` | MAVLink/PX4 bridge message types | -| `std_srvs` | Standard service types | - -## Building - -```bash -# From the workspace root -colcon build --packages-select drone_safety_monitor -source install/setup.bash -``` - -## Running - -```bash -ros2 run drone_safety_monitor drone_safety_monitor -``` - -Override the timeout at the command line: - -```bash -ros2 run drone_safety_monitor drone_safety_monitor \ - --ros-args -p state_estimate_timeout:=0.5 -``` - -## Configuration - -The `config/takeoff_landing_planner.yaml` file is included in the package install. Parameters can be loaded at launch time via `--params-file` or overridden inline with `--ros-args -p :=`. - -## Internal Classes - -### `TimeChecker` - -A lightweight helper that tracks the timestamp of the most recent event and computes the elapsed time since then. - -| Method | Description | -|---|---| -| `TimeChecker()` | Constructs the checker in an uninitialized state | -| `update(rclcpp::Time)` | Records the provided timestamp as the most recent update time | -| `elapsed_since_last_update(rclcpp::Time)` | Returns elapsed seconds since the last `update()` call, or `+∞` if `update()` has never been called | - -## Topic Graph - -``` -[localization / EKF] - │ - │ nav_msgs/Odometry - ▼ - state_estimate - │ - ▼ - drone_safety_monitor - │ - │ std_msgs/Bool - ▼ - state_estimate_timed_out - │ - ▼ - [behavior executive / safety arbiter] -``` diff --git a/robot/ros_ws/src/interface/drone_safety_monitor/config/takeoff_landing_planner.yaml b/robot/ros_ws/src/interface/drone_safety_monitor/config/takeoff_landing_planner.yaml deleted file mode 100644 index 23d11f151..000000000 --- a/robot/ros_ws/src/interface/drone_safety_monitor/config/takeoff_landing_planner.yaml +++ /dev/null @@ -1,14 +0,0 @@ -/**: - ros__parameters: - takeoff_height: 5.0 - high_takeoff_height: 5.0 - takeoff_landing_velocity: 0.3 - takeoff_acceptance_distance: 0.3 - takeoff_acceptance_time: 10.0 - landing_stationary_distance: 0.02 - landing_acceptance_time: 5.0 - landing_tracking_point_ahead_time: 5.0 - - takeoff_path_roll: 0. # in degrees - takeoff_path_pitch: 0. # in degrees - takeoff_path_relative_to_orientation: false diff --git a/robot/ros_ws/src/interface/drone_safety_monitor/launch/takeoff_landing_planner.launch.xml b/robot/ros_ws/src/interface/drone_safety_monitor/launch/takeoff_landing_planner.launch.xml deleted file mode 100644 index 7b4a46a66..000000000 --- a/robot/ros_ws/src/interface/drone_safety_monitor/launch/takeoff_landing_planner.launch.xml +++ /dev/null @@ -1,18 +0,0 @@ - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/robot/ros_ws/src/interface/drone_safety_monitor/src/drone_safety_monitor.cpp b/robot/ros_ws/src/interface/drone_safety_monitor/src/drone_safety_monitor.cpp deleted file mode 100644 index 86c21c94f..000000000 --- a/robot/ros_ws/src/interface/drone_safety_monitor/src/drone_safety_monitor.cpp +++ /dev/null @@ -1,84 +0,0 @@ -// Copyright (c) 2024 Carnegie Mellon University -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. - -#include - -TimeChecker::TimeChecker() - : time_initialized(false){ -} - -void TimeChecker::update(rclcpp::Time time){ - this->time = time; - time_initialized = true; -} - -double TimeChecker::elapsed_since_last_update(rclcpp::Time time){ - if(!time_initialized) - return std::numeric_limits::infinity(); - return (time - this->time).seconds(); -} - - -class DroneSafetyMonitorNode : public rclcpp::Node -{ -public: - rclcpp::Subscription::SharedPtr state_estimate_sub; - rclcpp::Publisher::SharedPtr state_estimate_timeout_pub; - rclcpp::TimerBase::SharedPtr timer; - float state_estimate_timeout; - - TimeChecker state_estimate_time_checker; - - DroneSafetyMonitorNode() : Node("odom_timeout_checker"){ - state_estimate_timeout = airstack::get_param(this, "state_estimate_timeout", 1.); - - // subscribers - state_estimate_sub = this->create_subscription("state_estimate", 10, - std::bind(&DroneSafetyMonitorNode::state_estimate_callback, - this, std::placeholders::_1)); - - // publishers - state_estimate_timeout_pub = this->create_publisher("state_estimate_timed_out", 10); - - // timers - timer = rclcpp::create_timer(this, this->get_clock(), std::chrono::milliseconds(1000), - std::bind(&DroneSafetyMonitorNode::timer_callback, this)); - - } - - void state_estimate_callback(const nav_msgs::msg::Odometry::SharedPtr msg){ - state_estimate_time_checker.update(this->get_clock()->now()); - } - - void timer_callback(){ - std_msgs::msg::Bool state_estimate_timeout_msg; - state_estimate_timeout_msg.data = - state_estimate_time_checker.elapsed_since_last_update(this->get_clock()->now()) > state_estimate_timeout; - state_estimate_timeout_pub->publish(state_estimate_timeout_msg); - } -}; - -int main(int argc, char **argv) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/robot/ros_ws/src/interface/interface_bringup/launch/interface.launch.py b/robot/ros_ws/src/interface/interface_bringup/launch/interface.launch.py index 9e0a1c494..1b1636424 100644 --- a/robot/ros_ws/src/interface/interface_bringup/launch/interface.launch.py +++ b/robot/ros_ws/src/interface/interface_bringup/launch/interface.launch.py @@ -122,21 +122,7 @@ def launch_setup(context, *args, **kwargs): ) actions.append(odometry_conversion_node) - # --- Drone safety monitor ----------------------------------------------- - drone_safety_monitor_node = Node( - package='drone_safety_monitor', - executable='drone_safety_monitor', - namespace='drone_safety_monitor', - output='screen', - parameters=[{ - 'state_estimate_timeout': 1.0, - }], - remappings=[ - ('state_estimate', - f'/{robot_name}/odometry_conversion/odometry'), - ], - ) - actions.append(drone_safety_monitor_node) + # NOTE: drone_safety_monitor is now launched from behavior_bringup return actions diff --git a/robot/ros_ws/src/interface/px4_interface/launch/px4_interface.launch.xml b/robot/ros_ws/src/interface/px4_interface/launch/px4_interface.launch.xml index 2e11a856c..a61891088 100644 --- a/robot/ros_ws/src/interface/px4_interface/launch/px4_interface.launch.xml +++ b/robot/ros_ws/src/interface/px4_interface/launch/px4_interface.launch.xml @@ -63,12 +63,6 @@ - - - - - + diff --git a/robot/ros_ws/src/local/controls/trajectory_controller/CMakeLists.txt b/robot/ros_ws/src/local/controls/trajectory_controller/CMakeLists.txt index 880d79242..f19e0bd6d 100644 --- a/robot/ros_ws/src/local/controls/trajectory_controller/CMakeLists.txt +++ b/robot/ros_ws/src/local/controls/trajectory_controller/CMakeLists.txt @@ -8,6 +8,7 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) find_package(pluginlib REQUIRED) find_package(geometry_msgs REQUIRED) find_package(visualization_msgs REQUIRED) @@ -18,6 +19,7 @@ find_package(tf2_ros REQUIRED) find_package(airstack_msgs REQUIRED) find_package(airstack_common REQUIRED) find_package(trajectory_library REQUIRED) +find_package(task_msgs REQUIRED) add_executable(trajectory_controller src/trajectory_controller.cpp) @@ -35,7 +37,19 @@ ament_target_dependencies(trajectory_controller airstack_msgs airstack_common trajectory_library) -install(TARGETS trajectory_controller + +add_executable(fixed_trajectory_task src/fixed_trajectory_task.cpp) +target_include_directories(fixed_trajectory_task PUBLIC + $ + $) +ament_target_dependencies(fixed_trajectory_task + rclcpp + rclcpp_action + nav_msgs + airstack_msgs + task_msgs) + +install(TARGETS trajectory_controller fixed_trajectory_task DESTINATION lib/${PROJECT_NAME}) install(DIRECTORY diff --git a/robot/ros_ws/src/local/controls/trajectory_controller/package.xml b/robot/ros_ws/src/local/controls/trajectory_controller/package.xml index 4c8876400..6d51bccf9 100644 --- a/robot/ros_ws/src/local/controls/trajectory_controller/package.xml +++ b/robot/ros_ws/src/local/controls/trajectory_controller/package.xml @@ -13,6 +13,7 @@ tf2_ros tf2_tools rclcpp + rclcpp_action pluginlib geometry_msgs visualization_msgs @@ -21,6 +22,7 @@ airstack_msgs airstack_common trajectory_library + task_msgs ament_lint_auto ament_lint_common diff --git a/robot/ros_ws/src/local/controls/trajectory_controller/src/fixed_trajectory_task.cpp b/robot/ros_ws/src/local/controls/trajectory_controller/src/fixed_trajectory_task.cpp new file mode 100644 index 000000000..e89445fe6 --- /dev/null +++ b/robot/ros_ws/src/local/controls/trajectory_controller/src/fixed_trajectory_task.cpp @@ -0,0 +1,595 @@ +// Copyright (c) 2024 Carnegie Mellon University +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// +// Ports fixed_trajectory_generator.py to a ROS 2 action server. + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "task_msgs/action/fixed_trajectory_task.hpp" + +using WaypointXYZVYaw = airstack_msgs::msg::WaypointXYZVYaw; +using TrajectoryXYZVYaw = airstack_msgs::msg::TrajectoryXYZVYaw; + +// ─────────────────────────── trajectory generation helpers ─────────────────── + +static void get_velocities(TrajectoryXYZVYaw & traj, double velocity, double max_acc) +{ + auto & wps = traj.waypoints; + double v_prev = 0.0; + for (size_t i = 0; i < wps.size(); ++i) { + size_t j = (i + 1) % wps.size(); + double dx = wps[j].position.x - wps[i].position.x; + double dy = wps[j].position.y - wps[i].position.y; + double dist = std::sqrt(dx * dx + dy * dy); + double v_limit = std::sqrt(v_prev * v_prev + 2.0 * max_acc * dist); + wps[i].velocity = std::min(velocity, v_limit); + v_prev = wps[i].velocity; + } +} + +static void get_velocities_dual(TrajectoryXYZVYaw & traj, double velocity, double max_acc) +{ + auto & wps = traj.waypoints; + double v_prev = 0.0; + for (size_t i = 0; i < wps.size(); ++i) { + size_t j = (i + 1) % wps.size(); + double dx = wps[j].position.x - wps[i].position.x; + double dy = wps[j].position.y - wps[i].position.y; + double dist = std::sqrt(dx * dx + dy * dy); + double v_limit = std::sqrt(v_prev * v_prev + 2.0 * max_acc * dist); + wps[i].velocity = std::min(velocity, v_limit); + v_prev = wps[i].velocity; + } + + v_prev = 0.0; + size_t tail_start = static_cast(wps.size() * 0.85); + for (size_t i = wps.size() - 1; i > tail_start; --i) { + size_t j = (i - 1 + wps.size()) % wps.size(); + double dx = wps[j].position.x - wps[i].position.x; + double dy = wps[j].position.y - wps[i].position.y; + double dist = std::sqrt(dx * dx + dy * dy); + double v_limit = std::sqrt(v_prev * v_prev + 2.0 * max_acc * dist); + wps[i].velocity = std::min(velocity, v_limit); + v_prev = wps[i].velocity; + } + wps.back().velocity = 0.0; +} + +static void get_accelerations(TrajectoryXYZVYaw & traj) +{ + auto & wps = traj.waypoints; + if (wps.size() < 3) {return;} + + for (size_t i = 0; i + 2 < wps.size(); ++i) { + size_t j = i + 1; + size_t k = i + 2; + + double dx1 = wps[j].position.x - wps[i].position.x; + double dy1 = wps[j].position.y - wps[i].position.y; + double dist1 = std::sqrt(dx1 * dx1 + dy1 * dy1); + if (dist1 < 1e-9) { + wps[i].acceleration.x = 0.0; + wps[i].acceleration.y = 0.0; + wps[i].acceleration.z = 0.0; + continue; + } + + double v_cx = wps[i].velocity * dx1 / dist1; + double v_cy = wps[i].velocity * dy1 / dist1; + + double dx2 = wps[k].position.x - wps[j].position.x; + double dy2 = wps[k].position.y - wps[j].position.y; + double dist2 = std::sqrt(dx2 * dx2 + dy2 * dy2); + if (dist2 < 1e-9) { + wps[i].acceleration.x = 0.0; + wps[i].acceleration.y = 0.0; + wps[i].acceleration.z = 0.0; + continue; + } + + double v_nx = wps[j].velocity * dx2 / dist2; + double v_ny = wps[j].velocity * dy2 / dist2; + + constexpr double acc_limit = 50.0; + wps[i].acceleration.x = (std::abs(dx1) < 1e-6) + ? 0.0 : std::min((v_nx * v_nx - v_cx * v_cx) / (2.0 * dx1), acc_limit); + wps[i].acceleration.y = (std::abs(dy1) < 1e-6) + ? 0.0 : std::min((v_ny * v_ny - v_cy * v_cy) / (2.0 * dy1), acc_limit); + wps[i].acceleration.z = 0.0; + } + + // zero out last two + if (wps.size() >= 2) { + wps[wps.size() - 2].acceleration.x = 0.0; + wps[wps.size() - 2].acceleration.y = 0.0; + wps[wps.size() - 2].acceleration.z = 0.0; + } + wps.back().acceleration.x = 0.0; + wps.back().acceleration.y = 0.0; + wps.back().acceleration.z = 0.0; +} + +static WaypointXYZVYaw make_wp(double x, double y, double z, double yaw, double v = 0.0) +{ + WaypointXYZVYaw wp; + wp.position.x = x; + wp.position.y = y; + wp.position.z = z; + wp.yaw = yaw; + wp.velocity = v; + return wp; +} + +static TrajectoryXYZVYaw generate_circle(const std::map & attrs) +{ + std::string frame_id = attrs.count("frame_id") ? attrs.at("frame_id") : "base_link"; + double radius = std::stod(attrs.at("radius")); + double velocity = std::stod(attrs.at("velocity")); + + TrajectoryXYZVYaw traj; + traj.header.frame_id = frame_id; + + traj.waypoints.push_back(make_wp(0.0, 0.0, 0.0, 0.0, velocity)); + traj.waypoints.push_back(make_wp(radius, 0.0, 0.0, 0.0, velocity)); + + for (double angle = 0.0; angle < 2.0 * M_PI; angle += 10.0 * M_PI / 180.0) { + traj.waypoints.push_back( + make_wp(radius * std::cos(angle), radius * std::sin(angle), 0.0, 0.0, velocity)); + } + + traj.waypoints.push_back(make_wp(radius, 0.0, 0.0, 0.0, velocity)); + traj.waypoints.push_back(make_wp(0.0, 0.0, 0.0, 0.0, velocity)); + return traj; +} + +static TrajectoryXYZVYaw generate_figure8(const std::map & attrs) +{ + std::string frame_id = attrs.count("frame_id") ? attrs.at("frame_id") : "base_link"; + double length = std::stod(attrs.at("length")); + double width = std::stod(attrs.at("width")); + double height = std::stod(attrs.at("height")); + double velocity = std::stod(attrs.at("velocity")); + double max_acc = std::stod(attrs.at("max_acceleration")); + + TrajectoryXYZVYaw traj; + traj.header.frame_id = frame_id; + + int n = 600; + for (int i = 0; i < n - 1; ++i) { + double t = 2.0 * M_PI * i / n; + double x = std::cos(t) * length - length; + double y = std::cos(t) * std::sin(t) * 2.0 * width; + double xd = -std::sin(t) * length; + double yd = (std::cos(t) * std::cos(t) - std::sin(t) * std::sin(t)) * 2.0 * width; + double yaw = std::atan2(yd, xd); + traj.waypoints.push_back(make_wp(x, y, height, yaw)); + } + + get_velocities_dual(traj, velocity, max_acc); + get_accelerations(traj); + return traj; +} + +static TrajectoryXYZVYaw generate_racetrack(const std::map & attrs) +{ + std::string frame_id = attrs.count("frame_id") ? attrs.at("frame_id") : "base_link"; + double length = std::stod(attrs.at("length")); + double width = std::stod(attrs.at("width")); + double height = std::stod(attrs.at("height")); + double velocity = std::stod(attrs.at("velocity")); + double turn_velocity = std::stod(attrs.at("turn_velocity")); + double max_acc = std::stod(attrs.at("max_acceleration")); + + TrajectoryXYZVYaw traj; + traj.header.frame_id = frame_id; + + double sl = length - width; // straightaway length + + // first straightaway + for (int i = 0; i <= 79; ++i) { + double x = sl * i / 79.0; + traj.waypoints.push_back(make_wp(x, 0.0, height, 0.0, velocity)); + } + + // first turn: semi-circle at (sl, width/2), from -pi/2 to pi/2 + int turn_n = 48; // 50 - 2 endpoints already covered + for (int i = 1; i < turn_n + 1; ++i) { + double t = -M_PI / 2.0 + M_PI * i / (turn_n + 1); + double x = width / 2.0 * std::cos(t) + sl; + double y = width / 2.0 * std::sin(t) + width / 2.0; + double xd = -width / 2.0 * std::sin(t); + double yd = width / 2.0 * std::cos(t); + double yaw = std::atan2(yd, xd); + traj.waypoints.push_back(make_wp(x, y, height, yaw, turn_velocity)); + } + + // second straightaway + for (int i = 0; i <= 79; ++i) { + double x = sl * (1.0 - i / 79.0); + traj.waypoints.push_back(make_wp(x, width, height, M_PI, velocity)); + } + + // second turn: semi-circle at (0, width/2), from pi/2 to 3pi/2 + for (int i = 1; i < turn_n + 1; ++i) { + double t = M_PI / 2.0 + M_PI * i / (turn_n + 1); + double x = width / 2.0 * std::cos(t); + double y = width / 2.0 * std::sin(t) + width / 2.0; + double xd = -width / 2.0 * std::sin(t); + double yd = width / 2.0 * std::cos(t); + double yaw = std::atan2(yd, xd) + M_PI; + traj.waypoints.push_back(make_wp(x, y, height, yaw, turn_velocity)); + } + + if (!traj.waypoints.empty()) { + traj.waypoints.front().velocity = 0.0; + traj.waypoints.back().velocity = 0.0; + } + + get_accelerations(traj); + (void)max_acc; + return traj; +} + +static TrajectoryXYZVYaw generate_line(const std::map & attrs) +{ + std::string frame_id = attrs.count("frame_id") ? attrs.at("frame_id") : "base_link"; + double length = std::stod(attrs.at("length")); + double height = std::stod(attrs.at("height")); + double velocity = std::stod(attrs.at("velocity")); + double max_acc = std::stod(attrs.at("max_acceleration")); + + TrajectoryXYZVYaw traj; + traj.header.frame_id = frame_id; + + for (double y = 0.0; y > -length; y -= 0.5) { + traj.waypoints.push_back(make_wp(-y, 0.0, height, 0.0)); + } + + get_velocities(traj, velocity, max_acc); + return traj; +} + +static TrajectoryXYZVYaw generate_point(const std::map & attrs) +{ + std::string frame_id = attrs.count("frame_id") ? attrs.at("frame_id") : "base_link"; + double x = std::stod(attrs.at("x")); + double y = std::stod(attrs.at("y")); + double height = std::stod(attrs.at("height")); + double velocity = std::stod(attrs.at("velocity")); + double max_acc = std::stod(attrs.at("max_acceleration")); + + TrajectoryXYZVYaw traj; + traj.header.frame_id = frame_id; + traj.waypoints.push_back(make_wp(x, y, height, 0.0)); + + get_velocities(traj, velocity, max_acc); + return traj; +} + +static TrajectoryXYZVYaw generate_lawnmower(const std::map & attrs) +{ + std::string frame_id = attrs.count("frame_id") ? attrs.at("frame_id") : "base_link"; + double length = std::stod(attrs.at("length")); + double width = std::stod(attrs.at("width")); + double height = std::stod(attrs.at("height")); + double velocity = std::stod(attrs.at("velocity")); + bool vertical = (attrs.count("vertical") && std::stoi(attrs.at("vertical")) != 0); + + TrajectoryXYZVYaw traj; + traj.header.frame_id = frame_id; + + int strips = static_cast(std::abs(height / width)); + double h_sign = (height >= 0) ? 1.0 : -1.0; + + for (int i = 0; i < strips; ++i) { + double z = h_sign * (i + 1) * width; + double yaw = std::atan2(length, 0.0); // pointing along y + + if (i % 2 == 0) { + // y goes 0 → length + traj.waypoints.push_back(make_wp(0.0, 0.0, z, vertical ? 0.0 : yaw, 0.1)); + traj.waypoints.push_back(make_wp(0.0, 0.5, z, vertical ? 0.0 : yaw, 0.1)); + traj.waypoints.push_back(make_wp(0.0, 0.5, z, vertical ? 0.0 : yaw, velocity)); + traj.waypoints.push_back(make_wp(0.0, length - 0.5, z, vertical ? 0.0 : yaw, velocity)); + traj.waypoints.push_back(make_wp(0.0, length, z, vertical ? 0.0 : yaw, 0.1)); + } else { + // y goes length → 0 + traj.waypoints.push_back(make_wp(0.0, length, z, vertical ? 0.0 : -yaw, 0.1)); + traj.waypoints.push_back(make_wp(0.0, length - 0.5, z, vertical ? 0.0 : -yaw, 0.1)); + traj.waypoints.push_back(make_wp(0.0, length - 0.5, z, vertical ? 0.0 : -yaw, velocity)); + traj.waypoints.push_back(make_wp(0.0, 0.5, z, vertical ? 0.0 : -yaw, velocity)); + traj.waypoints.push_back(make_wp(0.0, 0.0, z, vertical ? 0.0 : -yaw, 0.1)); + } + } + + // return waypoint + traj.waypoints.push_back(make_wp(0.0, 0.0, 0.0, 0.0, 0.5)); + + // if not vertical, swap x and z + if (!vertical) { + for (auto & wp : traj.waypoints) { + std::swap(wp.position.x, wp.position.z); + } + } + + return traj; +} + +// ─────────────────────────── action server node ─────────────────────────────── + +class FixedTrajectoryTaskNode : public rclcpp::Node +{ +public: + using FixedTrajectoryTask = task_msgs::action::FixedTrajectoryTask; + using GoalHandle = rclcpp_action::ServerGoalHandle; + + FixedTrajectoryTaskNode() + : rclcpp::Node("fixed_trajectory_task") + { + completion_sub_ = this->create_subscription( + "trajectory_completion_percentage", 1, + [this](std_msgs::msg::Float32::SharedPtr msg) { + std::lock_guard lock(completion_mutex_); + completion_percentage_ = msg->data; + }); + + odom_sub_ = this->create_subscription( + "odometry", rclcpp::QoS(1).best_effort(), + [this](nav_msgs::msg::Odometry::SharedPtr msg) { + std::lock_guard lock(odom_mutex_); + current_odom_ = *msg; + }); + + traj_override_pub_ = + this->create_publisher("trajectory_override", 1); + + traj_mode_client_ = + this->create_client("set_trajectory_mode"); + + action_server_ = rclcpp_action::create_server( + this, "~/fixed_trajectory_task", + std::bind(&FixedTrajectoryTaskNode::handle_goal, this, + std::placeholders::_1, std::placeholders::_2), + std::bind(&FixedTrajectoryTaskNode::handle_cancel, this, std::placeholders::_1), + std::bind(&FixedTrajectoryTaskNode::handle_accepted, this, std::placeholders::_1)); + + RCLCPP_INFO(this->get_logger(), "FixedTrajectoryTaskNode started"); + } + +private: + std::atomic task_active_{false}; + std::atomic cancel_requested_{false}; + + std::mutex completion_mutex_; + float completion_percentage_{0.0f}; + + std::mutex odom_mutex_; + nav_msgs::msg::Odometry current_odom_; + + rclcpp::Subscription::SharedPtr completion_sub_; + rclcpp::Subscription::SharedPtr odom_sub_; + rclcpp::Publisher::SharedPtr traj_override_pub_; + rclcpp::Client::SharedPtr traj_mode_client_; + rclcpp_action::Server::SharedPtr action_server_; + + bool set_trajectory_mode(int32_t mode) + { + if (!traj_mode_client_->wait_for_service(std::chrono::seconds(2))) { + RCLCPP_ERROR(this->get_logger(), "set_trajectory_mode service not available"); + return false; + } + auto req = std::make_shared(); + req->mode = mode; + auto future = traj_mode_client_->async_send_request(req); + future.wait(); + return future.get()->success; + } + + rclcpp_action::GoalResponse handle_goal( + const rclcpp_action::GoalUUID &, + std::shared_ptr goal) + { + if (task_active_) { + RCLCPP_WARN(this->get_logger(), "FixedTrajectoryTask rejected: task already active"); + return rclcpp_action::GoalResponse::REJECT; + } + // validate trajectory type + const std::string & type = goal->trajectory_spec.type; + if (type != "Figure8" && type != "Circle" && type != "Racetrack" && + type != "Line" && type != "Point" && type != "Lawnmower") + { + RCLCPP_WARN(this->get_logger(), + "FixedTrajectoryTask rejected: unknown trajectory type '%s'", type.c_str()); + return rclcpp_action::GoalResponse::REJECT; + } + task_active_ = true; + cancel_requested_ = false; + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + } + + rclcpp_action::CancelResponse handle_cancel(std::shared_ptr) + { + cancel_requested_ = true; + return rclcpp_action::CancelResponse::ACCEPT; + } + + void handle_accepted(std::shared_ptr goal_handle) + { + std::thread{ + std::bind(&FixedTrajectoryTaskNode::execute, this, std::placeholders::_1), + goal_handle}.detach(); + } + + void execute(std::shared_ptr goal_handle) + { + const auto goal = goal_handle->get_goal(); + auto result = std::make_shared(); + auto feedback = std::make_shared(); + + // parse attributes into a map + std::map attrs; + for (const auto & kv : goal->trajectory_spec.attributes) { + attrs[kv.key] = kv.value; + } + + // generate trajectory + TrajectoryXYZVYaw traj; + const std::string & type = goal->trajectory_spec.type; + try { + if (type == "Circle") { + traj = generate_circle(attrs); + } else if (type == "Figure8") { + traj = generate_figure8(attrs); + } else if (type == "Racetrack") { + traj = generate_racetrack(attrs); + } else if (type == "Line") { + traj = generate_line(attrs); + } else if (type == "Point") { + traj = generate_point(attrs); + } else if (type == "Lawnmower") { + traj = generate_lawnmower(attrs); + } + } catch (const std::exception & e) { + RCLCPP_ERROR(this->get_logger(), "FixedTrajectoryTask: trajectory generation failed: %s", + e.what()); + result->success = false; + result->message = std::string("trajectory generation failed: ") + e.what(); + goal_handle->abort(result); + task_active_ = false; + return; + } + + if (traj.waypoints.empty()) { + result->success = false; + result->message = "trajectory generation produced no waypoints"; + goal_handle->abort(result); + task_active_ = false; + return; + } + + traj.header.stamp = this->now(); + + // set trajectory mode to TRACK and publish + if (!set_trajectory_mode(airstack_msgs::srv::TrajectoryMode::Request::TRACK)) { + result->success = false; + result->message = "failed to set trajectory mode"; + goal_handle->abort(result); + task_active_ = false; + return; + } + + traj_override_pub_->publish(traj); + RCLCPP_INFO(this->get_logger(), "FixedTrajectoryTask: executing %s (%zu waypoints)", + type.c_str(), traj.waypoints.size()); + + // reset completion percentage after publishing + { + std::lock_guard lock(completion_mutex_); + completion_percentage_ = 0.0f; + } + + // monitor completion + rclcpp::Rate rate(5); + while (rclcpp::ok()) { + if (cancel_requested_) { + set_trajectory_mode(airstack_msgs::srv::TrajectoryMode::Request::ROBOT_POSE); + result->success = false; + result->message = "cancelled"; + goal_handle->canceled(result); + task_active_ = false; + return; + } + + float completion; + { + std::lock_guard lock(completion_mutex_); + completion = completion_percentage_; + } + + nav_msgs::msg::Odometry odom; + { + std::lock_guard lock(odom_mutex_); + odom = current_odom_; + } + + feedback->progress = completion / 100.0f; + feedback->status = type + " trajectory running"; + feedback->current_position.x = odom.pose.pose.position.x; + feedback->current_position.y = odom.pose.pose.position.y; + feedback->current_position.z = odom.pose.pose.position.z; + goal_handle->publish_feedback(feedback); + + if (completion >= 100.0f) { + if (goal->loop) { + // republish and reset + traj.header.stamp = this->now(); + traj_override_pub_->publish(traj); + { + std::lock_guard lock(completion_mutex_); + completion_percentage_ = 0.0f; + } + RCLCPP_INFO(this->get_logger(), "FixedTrajectoryTask: looping %s", type.c_str()); + } else { + RCLCPP_INFO(this->get_logger(), "FixedTrajectoryTask: %s complete", type.c_str()); + set_trajectory_mode(airstack_msgs::srv::TrajectoryMode::Request::ROBOT_POSE); + result->success = true; + result->message = type + " trajectory complete"; + goal_handle->succeed(result); + task_active_ = false; + return; + } + } + + rate.sleep(); + } + + set_trajectory_mode(airstack_msgs::srv::TrajectoryMode::Request::ROBOT_POSE); + result->success = false; + result->message = "node shutting down"; + goal_handle->abort(result); + task_active_ = false; + } +}; + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node); + executor.spin(); + rclcpp::shutdown(); + return 0; +} diff --git a/robot/ros_ws/src/local/local_bringup/launch/local.launch.xml b/robot/ros_ws/src/local/local_bringup/launch/local.launch.xml index 9c7283035..fd46b9282 100644 --- a/robot/ros_ws/src/local/local_bringup/launch/local.launch.xml +++ b/robot/ros_ws/src/local/local_bringup/launch/local.launch.xml @@ -15,29 +15,41 @@ - - + + + + + + + + + + + + + - - - - - + + + - - + diff --git a/robot/ros_ws/src/local/local_bringup/launch/local_droan_cpu.launch.xml b/robot/ros_ws/src/local/local_bringup/launch/local_droan_cpu.launch.xml index a65d20b96..453674779 100644 --- a/robot/ros_ws/src/local/local_bringup/launch/local_droan_cpu.launch.xml +++ b/robot/ros_ws/src/local/local_bringup/launch/local_droan_cpu.launch.xml @@ -15,29 +15,41 @@ - - + + + + + + + + + + + + + - - - - - + + + - - + diff --git a/robot/ros_ws/src/local/planners/takeoff_landing_planner/CMakeLists.txt b/robot/ros_ws/src/local/planners/takeoff_landing_planner/CMakeLists.txt index 1adb3320b..2022599a1 100644 --- a/robot/ros_ws/src/local/planners/takeoff_landing_planner/CMakeLists.txt +++ b/robot/ros_ws/src/local/planners/takeoff_landing_planner/CMakeLists.txt @@ -8,18 +8,21 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) find_package(std_msgs REQUIRED) find_package(airstack_msgs REQUIRED) find_package(airstack_common REQUIRED) find_package(trajectory_library REQUIRED) find_package(mavros_msgs REQUIRED) find_package(std_srvs REQUIRED) +find_package(task_msgs REQUIRED) +find_package(nav_msgs REQUIRED) add_executable(takeoff_landing_planner src/takeoff_landing_planner.cpp) target_include_directories(takeoff_landing_planner PUBLIC $ $) -target_compile_features(takeoff_landing_planner PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +target_compile_features(takeoff_landing_planner PUBLIC c_std_99 cxx_std_17) ament_target_dependencies( takeoff_landing_planner std_msgs @@ -30,7 +33,25 @@ ament_target_dependencies( std_srvs ) -install(TARGETS takeoff_landing_planner +add_executable(takeoff_landing_task src/takeoff_landing_task.cpp) +target_include_directories(takeoff_landing_task PUBLIC + $ + $) +target_compile_features(takeoff_landing_task PUBLIC c_std_99 cxx_std_17) +ament_target_dependencies( + takeoff_landing_task + rclcpp + rclcpp_action + std_msgs + nav_msgs + airstack_msgs + airstack_common + trajectory_library + mavros_msgs + task_msgs +) + +install(TARGETS takeoff_landing_planner takeoff_landing_task DESTINATION lib/${PROJECT_NAME}) install(DIRECTORY diff --git a/robot/ros_ws/src/local/planners/takeoff_landing_planner/README.md b/robot/ros_ws/src/local/planners/takeoff_landing_planner/README.md index 3cb5ca33e..d1505bfe2 100644 --- a/robot/ros_ws/src/local/planners/takeoff_landing_planner/README.md +++ b/robot/ros_ws/src/local/planners/takeoff_landing_planner/README.md @@ -2,116 +2,119 @@ ## Overview -The TakeoffLandingPlanner provides a rich interface for handling takeoff/landing commands, with support for both trajectory-based and ArduPilot command-based modes. It monitors the robot's position, generates appropriate trajectories, and tracks the completion status of takeoff and landing maneuvers. +The TakeoffLandingPlanner provides ROS 2 action servers for takeoff and landing maneuvers. It monitors the robot's state, generates trajectory overrides to reach the target altitude (takeoff) or ground (landing), and tracks completion via position and time thresholds. -## Highlights +## Features -- Two takeoff modes: standard and high altitude -- Configurable takeoff and landing velocities -- Configurable takeoff trajectory direction -- Trajectory-based takeoff and landing with position tracking -- Direct ArduPilot takeoff command support -- Completion monitoring with distance and time thresholds -- State publishing for integration with higher-level systems +- Trajectory-based takeoff to a configurable target altitude and velocity +- Trajectory-based landing with on-ground detection, followed by automatic disarm +- Configurable takeoff trajectory direction (roll/pitch, absolute or body-relative) +- Precondition checking: rejects goals when state estimate is timed out or a task is already running +- Publishes `is_airborne` for downstream consumers (e.g. the RViz Tasks Panel) ## Topics ### Subscriptions -| Topic | Type | Description | -| ---------------------------------- | ------------------------ | ------------------------------------------------------------------------------------------------------------------------------------------------ | -| `trajectory_completion_percentage` | `std_msgs/Float32` | Current completion percentage of trajectory (from [trajectory_controller](../../c_controls/trajectory_controller/src/trajectory_controller.cpp)) | -| `tracking_point` | `airstack_msgs/Odometry` | Current tracking point for trajectory (from [trajectory_controller](../../c_controls/trajectory_controller/src/trajectory_controller.cpp)) | -| `odometry` | `nav_msgs/Odometry` | Robot odometry data | -| `ekf_active` | `std_msgs/Bool` | EKF status flag | -| `high_takeoff` | `std_msgs/Bool` | Flag to trigger high altitude takeoff | +| Topic | Type | Description | +|------------------------------------|-----------------------------------|------------------------------------------------------| +| `odometry` | `nav_msgs/Odometry` | Robot pose and velocity | +| `tracking_point` | `airstack_msgs/Odometry` | Current trajectory tracking point | +| `trajectory_completion_percentage` | `std_msgs/Float32` | Trajectory completion (0–100) from trajectory_controller | +| `is_armed` | `std_msgs/Bool` | Vehicle arm status | +| `has_control` | `std_msgs/Bool` | Offboard control acquired | +| `state_estimate_timed_out` | `std_msgs/Bool` | Whether the state estimate has timed out | +| `extended_state` | `mavros_msgs/ExtendedState` | MAVROS landed state (used to publish `is_airborne`) | ### Publications -| Topic | Type | Description | -| --------------------- | --------------------------------- | -------------------------------------------------------- | -| `trajectory_override` | `airstack_msgs/TrajectoryXYZVYaw` | Generated takeoff/landing trajectory | -| `takeoff_state` | `std_msgs/String` | Current takeoff state ("NONE", "TAKING_OFF", "COMPLETE") | -| `landing_state` | `std_msgs/String` | Current landing state ("NONE", "LANDING", "COMPLETE") | +| Topic | Type | Description | +|---------------------|-----------------------------------|---------------------------------------------------------| +| `trajectory_override` | `airstack_msgs/TrajectoryXYZVYaw` | Takeoff/landing trajectory sent to trajectory_controller | +| `is_airborne` | `std_msgs/Bool` | True when `landed_state == LANDED_STATE_IN_AIR` | -## Services +## Service Clients -| Service | Type | Description | -| ----------------------------- | ------------------------------------- | ----------------------------------------- | -| `set_takeoff_landing_command` | `airstack_msgs/TakeoffLandingCommand` | Sets command mode (NONE, TAKEOFF, LAND) | -| `ardupilot_takeoff` | `std_srvs/Trigger` | Triggers direct ArduPilot takeoff command | +| Service | Type | Description | +|-----------------------|-----------------------------------|----------------------------------------| +| `set_trajectory_mode` | `airstack_msgs/TrajectoryMode` | Switch trajectory controller mode | +| `robot_command` | `airstack_msgs/RobotCommand` | Arm, request offboard control, disarm | -## Parameters +## Action Servers -| Parameter | Type | Default | Description | -| -------------------------------------- | ----- | ------- | ------------------------------------------------------- | -| `takeoff_height` | float | 0.5 | Standard takeoff height in meters | -| `high_takeoff_height` | float | 1.2 | High altitude takeoff height in meters | -| `takeoff_velocity` | float | 0.3 | Velocity during takeoff in m/s | -| `landing_velocity` | float | 0.3 | Velocity during landing in m/s | -| `takeoff_acceptance_distance` | float | 0.1 | Maximum distance to consider takeoff complete in meters | -| `takeoff_acceptance_time` | float | 2.0 | Required time at acceptance distance in seconds | -| `landing_stationary_distance` | float | 0.02 | Maximum movement distance to consider landed in meters | -| `landing_acceptance_time` | float | 5.0 | Required time at stationary distance in seconds | -| `landing_tracking_point_ahead_time` | float | 5.0 | How far ahead the tracking point should be | -| `takeoff_path_roll` | float | 0.0 | Roll angle for takeoff path in degrees | -| `takeoff_path_pitch` | float | 0.0 | Pitch angle for takeoff path in degrees | -| `takeoff_path_relative_to_orientation` | bool | false | Whether to use robot's current orientation for takeoff | +| Action | Type | Description | +|-----------------|---------------------------|------------------------------| +| `~/takeoff_task` | `task_msgs/TakeoffTask` | Execute a takeoff maneuver | +| `~/land_task` | `task_msgs/LandTask` | Execute a landing maneuver | -## Usage +### TakeoffTask -### Basic Usage +**Goal** -To launch the TakeoffLandingPlanner node: +| Field | Type | Description | +|---------------------|---------|--------------------------------| +| `target_altitude_m` | float32 | Target altitude in meters | +| `velocity_m_s` | float32 | Ascent velocity in m/s | -```bash -ros2 run takeoff_landing_planner takeoff_landing_planner -``` +**Feedback** -### Initiating Takeoff +| Field | Type | Description | +|----------------------|---------|--------------------------------| +| `status` | string | Human-readable status message | +| `current_altitude_m` | float32 | Current altitude in meters | +| `target_altitude_m` | float32 | Target altitude in meters | -To command a takeoff: +**Result** -```bash -ros2 service call /set_takeoff_landing_command airstack_msgs/srv/TakeoffLandingCommand "{command: 1}" -``` +| Field | Type | Description | +|-----------|---------|--------------------------| +| `success` | bool | Whether takeoff succeeded | +| `message` | string | Outcome description | -### Initiating Landing +### LandTask -To command a landing: +**Goal** -```bash -ros2 service call /set_takeoff_landing_command airstack_msgs/srv/TakeoffLandingCommand "{command: 2}" -``` +| Field | Type | Description | +|----------------|---------|--------------------------| +| `velocity_m_s` | float32 | Descent velocity in m/s | -### ArduPilot Takeoff +**Feedback** -To use direct ArduPilot takeoff commands: +| Field | Type | Description | +|----------------------|---------|-------------------------------| +| `status` | string | Human-readable status message | +| `current_altitude_m` | float32 | Current altitude in meters | -```bash -ros2 service call /ardupilot_takeoff std_srvs/srv/Trigger "{}" -``` +**Result** -## State Monitoring +| Field | Type | Description | +|-----------|--------|---------------------------| +| `success` | bool | Whether landing succeeded | +| `message` | string | Outcome description | -The node publishes the current state of takeoff and landing operations through the `takeoff_state` and `landing_state` topics. Monitor these topics to track the progress and completion of operations: +## Parameters -```bash -ros2 topic echo /takeoff_state -ros2 topic echo /landing_state -``` +| Parameter | Type | Default | Description | +|----------------------------------------|-------|---------|---------------------------------------------------------------| +| `takeoff_velocity` | float | 1.0 | Default ascent velocity in m/s | +| `landing_velocity` | float | 0.3 | Default descent velocity in m/s | +| `takeoff_acceptance_distance` | float | 0.3 | Distance threshold to consider target altitude reached (m) | +| `takeoff_acceptance_time` | float | 1.0 | Time that must be spent within acceptance distance (s) | +| `landing_stationary_distance` | float | 0.02 | Max movement to consider the drone stationary on landing (m) | +| `landing_acceptance_time` | float | 5.0 | Time the drone must remain stationary to confirm landing (s) | +| `landing_tracking_point_ahead_time` | float | 5.0 | Lookahead time for the landing tracking point | +| `takeoff_path_roll` | float | 0.0 | Roll offset for the takeoff trajectory (degrees) | +| `takeoff_path_pitch` | float | 0.0 | Pitch offset for the takeoff trajectory (degrees) | +| `takeoff_path_relative_to_orientation` | bool | false | Apply roll/pitch relative to the robot's current orientation | ## Dependencies -- airstack_msgs -- nav_msgs -- mavros_msgs -- std_msgs -- std_srvs -- tf2_ros - -## Notes - -- The node uses TF2 to track relative positions between the robot and tracking points -- Landing completion detection relies on both position stability and tracking point metrics -- The default landing target is hardcoded (-10000.0) and should be parameterized in future versions +- `rclcpp` / `rclcpp_action` +- `airstack_msgs` +- `airstack_common` +- `nav_msgs` +- `mavros_msgs` +- `std_msgs` +- `task_msgs` +- `trajectory_library` diff --git a/robot/ros_ws/src/local/planners/takeoff_landing_planner/include/takeoff_landing_planner/takeoff_landing_task.hpp b/robot/ros_ws/src/local/planners/takeoff_landing_planner/include/takeoff_landing_planner/takeoff_landing_task.hpp new file mode 100644 index 000000000..7873484ce --- /dev/null +++ b/robot/ros_ws/src/local/planners/takeoff_landing_planner/include/takeoff_landing_planner/takeoff_landing_task.hpp @@ -0,0 +1,143 @@ +// Copyright (c) 2024 Carnegie Mellon University +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#pragma once + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "task_msgs/action/land_task.hpp" +#include "task_msgs/action/takeoff_task.hpp" + +class TakeoffLandingTaskNode : public rclcpp::Node +{ +public: + using TakeoffTask = task_msgs::action::TakeoffTask; + using TakeoffGoalHandle = rclcpp_action::ServerGoalHandle; + using LandTask = task_msgs::action::LandTask; + using LandGoalHandle = rclcpp_action::ServerGoalHandle; + + TakeoffLandingTaskNode(); + +private: + // parameters + double default_takeoff_velocity_; + double default_landing_velocity_; + double takeoff_acceptance_distance_; + double takeoff_acceptance_time_; + double landing_stationary_distance_; + double landing_acceptance_time_; + double landing_tracking_point_ahead_time_; + double takeoff_path_roll_; + double takeoff_path_pitch_; + bool takeoff_path_relative_to_orientation_; + + // shared state (protected by mutex) + std::mutex odom_mutex_; + nav_msgs::msg::Odometry robot_odom_; + bool got_robot_odom_{false}; + + std::mutex tracking_point_mutex_; + airstack_msgs::msg::Odometry tracking_point_odom_; + bool got_tracking_point_{false}; + + std::mutex completion_mutex_; + float completion_percentage_{0.0f}; + bool got_completion_percentage_{false}; + + // precondition state (atomic for lock-free reads in handle_goal) + std::atomic is_armed_{false}; + std::atomic has_control_{false}; + std::atomic state_estimate_timed_out_{false}; + + // landed state from mavros + std::atomic landed_state_{0}; // mavros_msgs::msg::ExtendedState::LANDED_STATE_UNDEFINED + + // task exclusion + std::atomic task_active_{false}; + std::atomic cancel_requested_{false}; + + // set by land_execute on success; cleared by takeoff_execute on start. + // prevents extended_state callback from re-publishing is_airborne=true + // after MAVROS transiently reverts to IN_AIR post-landing. + std::atomic landed_{false}; + + // subscribers + rclcpp::Subscription::SharedPtr robot_odom_sub_; + rclcpp::Subscription::SharedPtr tracking_point_sub_; + rclcpp::Subscription::SharedPtr is_armed_sub_; + rclcpp::Subscription::SharedPtr has_control_sub_; + rclcpp::Subscription::SharedPtr state_estimate_timed_out_sub_; + rclcpp::Subscription::SharedPtr completion_percentage_sub_; + rclcpp::Subscription::SharedPtr extended_state_sub_; + + // publishers + rclcpp::Publisher::SharedPtr traj_override_pub_; + rclcpp::Publisher::SharedPtr is_airborne_pub_; + + // service clients + rclcpp::Client::SharedPtr traj_mode_client_; + rclcpp::Client::SharedPtr robot_command_client_; + + // action servers + rclcpp_action::Server::SharedPtr takeoff_server_; + rclcpp_action::Server::SharedPtr land_server_; + + // subscription callbacks + void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg); + void tracking_point_callback(const airstack_msgs::msg::Odometry::SharedPtr msg); + void completion_percentage_callback(const std_msgs::msg::Float32::SharedPtr msg); + + // helpers + bool set_trajectory_mode(int32_t mode); + bool send_robot_command(uint8_t command); + + // TakeoffTask action server callbacks + rclcpp_action::GoalResponse takeoff_handle_goal( + const rclcpp_action::GoalUUID & uuid, + std::shared_ptr goal); + rclcpp_action::CancelResponse takeoff_handle_cancel( + std::shared_ptr goal_handle); + void takeoff_handle_accepted(std::shared_ptr goal_handle); + void takeoff_execute(std::shared_ptr goal_handle); + + // LandTask action server callbacks + rclcpp_action::GoalResponse land_handle_goal( + const rclcpp_action::GoalUUID & uuid, + std::shared_ptr goal); + rclcpp_action::CancelResponse land_handle_cancel( + std::shared_ptr goal_handle); + void land_handle_accepted(std::shared_ptr goal_handle); + void land_execute(std::shared_ptr goal_handle); +}; diff --git a/robot/ros_ws/src/local/planners/takeoff_landing_planner/package.xml b/robot/ros_ws/src/local/planners/takeoff_landing_planner/package.xml index 410fb2b74..a718f1309 100644 --- a/robot/ros_ws/src/local/planners/takeoff_landing_planner/package.xml +++ b/robot/ros_ws/src/local/planners/takeoff_landing_planner/package.xml @@ -10,12 +10,15 @@ ament_cmake rclcpp + rclcpp_action std_msgs + nav_msgs airstack_msgs airstack_common trajectory_library mavros_msgs std_srvs + task_msgs ament_lint_auto ament_lint_common diff --git a/robot/ros_ws/src/local/planners/takeoff_landing_planner/src/takeoff_landing_task.cpp b/robot/ros_ws/src/local/planners/takeoff_landing_planner/src/takeoff_landing_task.cpp new file mode 100644 index 000000000..3e736d05a --- /dev/null +++ b/robot/ros_ws/src/local/planners/takeoff_landing_planner/src/takeoff_landing_task.cpp @@ -0,0 +1,509 @@ +// Copyright (c) 2024 Carnegie Mellon University +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#include + +#include +#include +#include + +TakeoffLandingTaskNode::TakeoffLandingTaskNode() +: rclcpp::Node("takeoff_landing_task") +{ + // parameters + default_takeoff_velocity_ = airstack::get_param(this, "takeoff_velocity", 1.0); + default_landing_velocity_ = airstack::get_param(this, "landing_velocity", 0.3); + takeoff_acceptance_distance_ = airstack::get_param(this, "takeoff_acceptance_distance", 0.3); + takeoff_acceptance_time_ = airstack::get_param(this, "takeoff_acceptance_time", 1.0); + landing_stationary_distance_ = airstack::get_param(this, "landing_stationary_distance", 0.02); + landing_acceptance_time_ = airstack::get_param(this, "landing_acceptance_time", 5.0); + landing_tracking_point_ahead_time_ = + airstack::get_param(this, "landing_tracking_point_ahead_time", 5.0); + takeoff_path_roll_ = airstack::get_param(this, "takeoff_path_roll", 0.0) * M_PI / 180.0; + takeoff_path_pitch_ = airstack::get_param(this, "takeoff_path_pitch", 0.0) * M_PI / 180.0; + takeoff_path_relative_to_orientation_ = + airstack::get_param(this, "takeoff_path_relative_to_orientation", false); + + // subscribers + auto sensor_qos = rclcpp::QoS(1); + sensor_qos.reliability(rclcpp::ReliabilityPolicy::BestEffort); + + robot_odom_sub_ = this->create_subscription( + "odometry", sensor_qos, + std::bind(&TakeoffLandingTaskNode::odom_callback, this, std::placeholders::_1)); + + tracking_point_sub_ = this->create_subscription( + "tracking_point", 1, + std::bind(&TakeoffLandingTaskNode::tracking_point_callback, this, std::placeholders::_1)); + + completion_percentage_sub_ = this->create_subscription( + "trajectory_completion_percentage", 1, + std::bind( + &TakeoffLandingTaskNode::completion_percentage_callback, this, std::placeholders::_1)); + + is_armed_sub_ = this->create_subscription( + "is_armed", 1, + [this](std_msgs::msg::Bool::SharedPtr msg) { is_armed_ = msg->data; }); + + has_control_sub_ = this->create_subscription( + "has_control", 1, + [this](std_msgs::msg::Bool::SharedPtr msg) { has_control_ = msg->data; }); + + state_estimate_timed_out_sub_ = this->create_subscription( + "state_estimate_timed_out", 1, + [this](std_msgs::msg::Bool::SharedPtr msg) { state_estimate_timed_out_ = msg->data; }); + + extended_state_sub_ = this->create_subscription( + "extended_state", 1, + [this](mavros_msgs::msg::ExtendedState::SharedPtr msg) { + landed_state_ = msg->landed_state; + bool in_air = (msg->landed_state == mavros_msgs::msg::ExtendedState::LANDED_STATE_IN_AIR); + if (!in_air) { + // Definitive non-airborne state from MAVROS — always publish false. + std_msgs::msg::Bool airborne_msg; + airborne_msg.data = false; + is_airborne_pub_->publish(airborne_msg); + } else if (!landed_) { + // IN_AIR and no confirmed landing — publish true. + std_msgs::msg::Bool airborne_msg; + airborne_msg.data = true; + is_airborne_pub_->publish(airborne_msg); + } + // If landed_ is set and MAVROS reports IN_AIR, publish nothing: + // land_execute already published false; MAVROS is transiently wrong. + }); + + // publishers + traj_override_pub_ = + this->create_publisher("trajectory_override", 1); + is_airborne_pub_ = this->create_publisher("is_airborne", 1); + + // service clients + traj_mode_client_ = + this->create_client("set_trajectory_mode"); + robot_command_client_ = + this->create_client("robot_command"); + + // action servers + takeoff_server_ = rclcpp_action::create_server( + this, "~/takeoff_task", + std::bind(&TakeoffLandingTaskNode::takeoff_handle_goal, this, + std::placeholders::_1, std::placeholders::_2), + std::bind(&TakeoffLandingTaskNode::takeoff_handle_cancel, this, std::placeholders::_1), + std::bind(&TakeoffLandingTaskNode::takeoff_handle_accepted, this, std::placeholders::_1)); + + land_server_ = rclcpp_action::create_server( + this, "~/land_task", + std::bind(&TakeoffLandingTaskNode::land_handle_goal, this, + std::placeholders::_1, std::placeholders::_2), + std::bind(&TakeoffLandingTaskNode::land_handle_cancel, this, std::placeholders::_1), + std::bind(&TakeoffLandingTaskNode::land_handle_accepted, this, std::placeholders::_1)); + + RCLCPP_INFO(this->get_logger(), "TakeoffLandingTaskNode started"); +} + +// ─────────────────────────── subscription callbacks ─────────────────────────── + +void TakeoffLandingTaskNode::odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg) +{ + std::lock_guard lock(odom_mutex_); + robot_odom_ = *msg; + got_robot_odom_ = true; +} + +void TakeoffLandingTaskNode::tracking_point_callback( + const airstack_msgs::msg::Odometry::SharedPtr msg) +{ + std::lock_guard lock(tracking_point_mutex_); + tracking_point_odom_ = *msg; + got_tracking_point_ = true; +} + +void TakeoffLandingTaskNode::completion_percentage_callback( + const std_msgs::msg::Float32::SharedPtr msg) +{ + std::lock_guard lock(completion_mutex_); + completion_percentage_ = msg->data; + got_completion_percentage_ = true; +} + +// ─────────────────────────── helper ─────────────────────────────────────────── + +bool TakeoffLandingTaskNode::set_trajectory_mode(int32_t mode) +{ + if (!traj_mode_client_->wait_for_service(std::chrono::seconds(2))) { + RCLCPP_ERROR(this->get_logger(), "set_trajectory_mode service not available"); + return false; + } + auto request = std::make_shared(); + request->mode = mode; + auto future = traj_mode_client_->async_send_request(request); + future.wait(); + return future.get()->success; +} + +bool TakeoffLandingTaskNode::send_robot_command(uint8_t command) +{ + if (!robot_command_client_->wait_for_service(std::chrono::seconds(2))) { + RCLCPP_ERROR(this->get_logger(), "robot_command service not available"); + return false; + } + auto request = std::make_shared(); + request->command = command; + auto future = robot_command_client_->async_send_request(request); + future.wait(); + return future.get()->success; +} + +// ─────────────────────────── TakeoffTask ────────────────────────────────────── + +rclcpp_action::GoalResponse TakeoffLandingTaskNode::takeoff_handle_goal( + const rclcpp_action::GoalUUID &, + std::shared_ptr goal) +{ + if (task_active_) { + RCLCPP_WARN(this->get_logger(), "TakeoffTask rejected: another task is already active"); + return rclcpp_action::GoalResponse::REJECT; + } + if (state_estimate_timed_out_) { + RCLCPP_WARN(this->get_logger(), "TakeoffTask rejected: state estimate timed out"); + return rclcpp_action::GoalResponse::REJECT; + } + if (goal->target_altitude_m <= 0.0f) { + RCLCPP_WARN(this->get_logger(), "TakeoffTask rejected: target_altitude_m must be positive"); + return rclcpp_action::GoalResponse::REJECT; + } + task_active_ = true; + cancel_requested_ = false; + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +rclcpp_action::CancelResponse TakeoffLandingTaskNode::takeoff_handle_cancel( + std::shared_ptr) +{ + cancel_requested_ = true; + return rclcpp_action::CancelResponse::ACCEPT; +} + +void TakeoffLandingTaskNode::takeoff_handle_accepted(std::shared_ptr goal_handle) +{ + std::thread{ + std::bind(&TakeoffLandingTaskNode::takeoff_execute, this, std::placeholders::_1), + goal_handle}.detach(); +} + +void TakeoffLandingTaskNode::takeoff_execute(std::shared_ptr goal_handle) +{ + const auto goal = goal_handle->get_goal(); + float target_altitude = goal->target_altitude_m; + float velocity = (goal->velocity_m_s > 0.0f) ? goal->velocity_m_s : default_takeoff_velocity_; + + auto result = std::make_shared(); + auto feedback = std::make_shared(); + feedback->target_altitude_m = target_altitude; + + landed_ = false; // clear latch — a new takeoff is starting + + // wait for odometry + rclcpp::Rate wait_rate(10); + int wait_count = 0; + while (!got_robot_odom_ && rclcpp::ok()) { + if (wait_count++ > 50) { + RCLCPP_ERROR(this->get_logger(), "TakeoffTask aborted: no odometry received"); + result->success = false; + result->message = "no odometry received"; + goal_handle->abort(result); + task_active_ = false; + return; + } + wait_rate.sleep(); + } + + // arm the robot + if (!is_armed_) { + RCLCPP_INFO(this->get_logger(), "TakeoffTask: arming robot"); + if (!send_robot_command(airstack_msgs::srv::RobotCommand::Request::ARM)) { + RCLCPP_ERROR(this->get_logger(), "TakeoffTask aborted: failed to arm"); + result->success = false; + result->message = "failed to arm"; + goal_handle->abort(result); + task_active_ = false; + return; + } + } + + // request offboard control + if (!has_control_) { + RCLCPP_INFO(this->get_logger(), "TakeoffTask: requesting offboard control"); + if (!send_robot_command(airstack_msgs::srv::RobotCommand::Request::REQUEST_CONTROL)) { + RCLCPP_ERROR(this->get_logger(), "TakeoffTask aborted: failed to request offboard control"); + result->success = false; + result->message = "failed to request offboard control"; + goal_handle->abort(result); + task_active_ = false; + return; + } + } + + // set trajectory mode to TRACK + if (!set_trajectory_mode(airstack_msgs::srv::TrajectoryMode::Request::TRACK)) { + result->success = false; + result->message = "failed to set trajectory mode"; + goal_handle->abort(result); + task_active_ = false; + return; + } + + // generate and publish takeoff trajectory + { + std::lock_guard odom_lock(odom_mutex_); + std::lock_guard tp_lock(tracking_point_mutex_); + + airstack_msgs::msg::Odometry start_point; + if (got_tracking_point_) { + start_point = tracking_point_odom_; + } else { + // fall back to current robot odom as start point + start_point.header = robot_odom_.header; + start_point.pose.position.x = robot_odom_.pose.pose.position.x; + start_point.pose.position.y = robot_odom_.pose.pose.position.y; + start_point.pose.position.z = robot_odom_.pose.pose.position.z; + start_point.pose.orientation = robot_odom_.pose.pose.orientation; + } + + // height is relative offset; target_altitude_m is absolute + float current_z = robot_odom_.pose.pose.position.z; + float relative_height = target_altitude - current_z; + if (relative_height <= 0.0f) { + RCLCPP_WARN(this->get_logger(), + "TakeoffTask: target_altitude_m (%.2f) is not above current altitude (%.2f)", + target_altitude, current_z); + relative_height = 0.5f; // minimum ascent + } + + if (takeoff_path_relative_to_orientation_) { + start_point.pose.orientation = robot_odom_.pose.pose.orientation; + } + + TakeoffTrajectory traj_gen( + relative_height, velocity, + takeoff_path_roll_, takeoff_path_pitch_, + takeoff_path_relative_to_orientation_); + traj_override_pub_->publish(traj_gen.get_trajectory(start_point)); + } + + RCLCPP_INFO(this->get_logger(), "TakeoffTask: ascending to %.2fm at %.2f m/s", + target_altitude, velocity); + + // monitor completion + rclcpp::Rate rate(10); // 10 Hz feedback + rclcpp::Time acceptance_start; + bool in_acceptance_window = false; + + while (rclcpp::ok()) { + if (cancel_requested_) { + set_trajectory_mode(airstack_msgs::srv::TrajectoryMode::Request::ROBOT_POSE); + result->success = false; + result->message = "cancelled"; + goal_handle->canceled(result); + task_active_ = false; + return; + } + + float current_z; + { + std::lock_guard lock(odom_mutex_); + current_z = robot_odom_.pose.pose.position.z; + } + + feedback->current_altitude_m = current_z; + goal_handle->publish_feedback(feedback); + + // check completion: within acceptance distance of target for acceptance_time + float dist = std::abs(current_z - target_altitude); + if (dist <= takeoff_acceptance_distance_) { + if (!in_acceptance_window) { + in_acceptance_window = true; + acceptance_start = this->now(); + } + if ((this->now() - acceptance_start).seconds() >= takeoff_acceptance_time_) { + RCLCPP_INFO(this->get_logger(), "TakeoffTask: complete at %.2fm", current_z); + set_trajectory_mode(airstack_msgs::srv::TrajectoryMode::Request::ROBOT_POSE); + result->success = true; + result->message = "takeoff complete"; + goal_handle->succeed(result); + task_active_ = false; + return; + } + } else { + in_acceptance_window = false; + } + + rate.sleep(); + } + + set_trajectory_mode(airstack_msgs::srv::TrajectoryMode::Request::ROBOT_POSE); + result->success = false; + result->message = "node shutting down"; + goal_handle->abort(result); + task_active_ = false; +} + +// ─────────────────────────── LandTask ───────────────────────────────────────── + +rclcpp_action::GoalResponse TakeoffLandingTaskNode::land_handle_goal( + const rclcpp_action::GoalUUID &, + std::shared_ptr) +{ + if (task_active_) { + RCLCPP_WARN(this->get_logger(), "LandTask rejected: another task is already active"); + return rclcpp_action::GoalResponse::REJECT; + } + task_active_ = true; + cancel_requested_ = false; + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +rclcpp_action::CancelResponse TakeoffLandingTaskNode::land_handle_cancel( + std::shared_ptr) +{ + cancel_requested_ = true; + return rclcpp_action::CancelResponse::ACCEPT; +} + +void TakeoffLandingTaskNode::land_handle_accepted(std::shared_ptr goal_handle) +{ + std::thread{ + std::bind(&TakeoffLandingTaskNode::land_execute, this, std::placeholders::_1), + goal_handle}.detach(); +} + +void TakeoffLandingTaskNode::land_execute(std::shared_ptr goal_handle) +{ + const auto goal = goal_handle->get_goal(); + float velocity = (goal->velocity_m_s > 0.0f) ? goal->velocity_m_s : default_landing_velocity_; + + auto result = std::make_shared(); + auto feedback = std::make_shared(); + + // wait for odometry + rclcpp::Rate wait_rate(10); + int wait_count = 0; + while (!got_robot_odom_ && rclcpp::ok()) { + if (wait_count++ > 50) { + RCLCPP_ERROR(this->get_logger(), "LandTask aborted: no odometry received"); + result->success = false; + result->message = "no odometry received"; + goal_handle->abort(result); + task_active_ = false; + return; + } + wait_rate.sleep(); + } + + // set trajectory mode to TRACK + if (!set_trajectory_mode(airstack_msgs::srv::TrajectoryMode::Request::TRACK)) { + result->success = false; + result->message = "failed to set trajectory mode"; + goal_handle->abort(result); + task_active_ = false; + return; + } + + // generate and publish landing trajectory + { + std::lock_guard tp_lock(tracking_point_mutex_); + airstack_msgs::msg::Odometry start_point; + if (got_tracking_point_) { + start_point = tracking_point_odom_; + } else { + std::lock_guard odom_lock(odom_mutex_); + start_point.header = robot_odom_.header; + start_point.pose.position.x = robot_odom_.pose.pose.position.x; + start_point.pose.position.y = robot_odom_.pose.pose.position.y; + start_point.pose.position.z = robot_odom_.pose.pose.position.z; + start_point.pose.orientation = robot_odom_.pose.pose.orientation; + } + TakeoffTrajectory land_traj(-10000.0, velocity); + traj_override_pub_->publish(land_traj.get_trajectory(start_point)); + } + + RCLCPP_INFO(this->get_logger(), "LandTask: descending at %.2f m/s", velocity); + + rclcpp::Rate rate(10); + while (rclcpp::ok()) { + if (cancel_requested_) { + set_trajectory_mode(airstack_msgs::srv::TrajectoryMode::Request::ROBOT_POSE); + result->success = false; + result->message = "cancelled"; + goal_handle->canceled(result); + task_active_ = false; + return; + } + + float current_z; + { + std::lock_guard lock(odom_mutex_); + current_z = robot_odom_.pose.pose.position.z; + } + + feedback->current_altitude_m = current_z; + feedback->status = "landing"; + goal_handle->publish_feedback(feedback); + + // check if mavros reports on-ground + if (landed_state_ == mavros_msgs::msg::ExtendedState::LANDED_STATE_ON_GROUND) { + RCLCPP_INFO(this->get_logger(), "LandTask: landed (mavros landed_state = ON_GROUND) at %.2fm", + current_z); + set_trajectory_mode(airstack_msgs::srv::TrajectoryMode::Request::ROBOT_POSE); + send_robot_command(airstack_msgs::srv::RobotCommand::Request::DISARM); + landed_ = true; + std_msgs::msg::Bool airborne_msg; + airborne_msg.data = false; + is_airborne_pub_->publish(airborne_msg); + result->success = true; + result->message = "landing complete"; + goal_handle->succeed(result); + task_active_ = false; + return; + } + + rate.sleep(); + } + + set_trajectory_mode(airstack_msgs::srv::TrajectoryMode::Request::ROBOT_POSE); + result->success = false; + result->message = "node shutting down"; + goal_handle->abort(result); + task_active_ = false; +} + +// ─────────────────────────── main ───────────────────────────────────────────── + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node); + executor.spin(); + rclcpp::shutdown(); + return 0; +} diff --git a/robot/ros_ws/src/local/planners/trajectory_library/CMakeLists.txt b/robot/ros_ws/src/local/planners/trajectory_library/CMakeLists.txt index b582b711f..cc014b395 100644 --- a/robot/ros_ws/src/local/planners/trajectory_library/CMakeLists.txt +++ b/robot/ros_ws/src/local/planners/trajectory_library/CMakeLists.txt @@ -62,7 +62,6 @@ install(DIRECTORY DESTINATION share/${PROJECT_NAME}) -install(PROGRAMS scripts/fixed_trajectory_generator.py DESTINATION lib/${PROJECT_NAME}) ament_export_include_directories( include diff --git a/robot/ros_ws/src/local/planners/trajectory_library/scripts/fixed_trajectory_generator.py b/robot/ros_ws/src/local/planners/trajectory_library/scripts/fixed_trajectory_generator.py deleted file mode 100755 index 958a405a1..000000000 --- a/robot/ros_ws/src/local/planners/trajectory_library/scripts/fixed_trajectory_generator.py +++ /dev/null @@ -1,551 +0,0 @@ -#!/usr/bin/python3 -import numpy as np -from nav_msgs.msg import Odometry -from geometry_msgs.msg import Point -from airstack_msgs.msg import WaypointXYZVYaw -from airstack_msgs.msg import TrajectoryXYZVYaw -from airstack_msgs.msg import FixedTrajectory -import time -import copy -import rclpy -from rclpy.node import Node - -logger = None - -def get_velocities(traj, velocity, max_acc): - v_prev = 0.0 - - for i in range(len(traj.waypoints)): - j = (i + 1) % len(traj.waypoints) - dx = traj.waypoints[j].position.x - traj.waypoints[i].position.x - dy = traj.waypoints[j].position.y - traj.waypoints[i].position.y - - dist = np.sqrt(dx**2 + dy**2) - v_limit = np.sqrt(v_prev**2 + 2 * max_acc * dist) - traj.waypoints[i].velocity = min(velocity, v_limit) - v_prev = traj.waypoints[i].velocity - - -def get_velocities_dual(traj, velocity, max_acc): - v_prev = 0.0 - - for i in range(len(traj.waypoints)): - j = (i + 1) % len(traj.waypoints) - dx = traj.waypoints[j].position.x - traj.waypoints[i].position.x - dy = traj.waypoints[j].position.y - traj.waypoints[i].position.y - - dist = np.sqrt(dx**2 + dy**2) - v_limit = np.sqrt(v_prev**2 + 2 * max_acc * dist) - traj.waypoints[i].velocity = min(velocity, v_limit) - v_prev = traj.waypoints[i].velocity - - v_prev = 0.0 - for i in range(len(traj.waypoints) - 1, int(len(traj.waypoints) * 0.85), -1): - j = (i - 1) % len(traj.waypoints) - dx = traj.waypoints[j].position.x - traj.waypoints[i].position.x - dy = traj.waypoints[j].position.y - traj.waypoints[i].position.y - - dist = np.sqrt(dx**2 + dy**2) - v_limit = np.sqrt(v_prev**2 + 2 * max_acc * dist) - traj.waypoints[i].velocity = min(velocity, v_limit) - v_prev = traj.waypoints[i].velocity - - traj.waypoints[len(traj.waypoints) - 1].velocity = 0.0 - -def get_velocities_adjust(traj, max_acc): - slow_down_adjust = [] - speed_up_adjust = [] - - for i in range(len(traj.waypoints)): - if i != len(traj.waypoints)-1: - if traj.waypoints[i].velocity > traj.waypoints[i+1].velocity: - v_start = traj.waypoints[i+1].velocity - v_target = traj.waypoints[i].velocity - slow_down_adjust.append((i+1, v_start, v_target)) - elif traj.waypoints[i].velocity < traj.waypoints[i+1].velocity: - v_start = traj.waypoints[i].velocity - v_target = traj.waypoints[i+1].velocity - speed_up_adjust.append((i, v_start, v_target)) - - for start_index, v_start, v_target in slow_down_adjust: - prev_i = start_index - v_prev = v_start - for i in range(start_index-1, -1, -1): - dx = traj.waypoints[prev_i].position.x - traj.waypoints[i].position.x - dy = traj.waypoints[prev_i].position.y - traj.waypoints[i].position.y - dist = np.sqrt(dx**2 + dy**2) - v_limit = np.sqrt(v_prev**2 + 2 * max_acc * dist) - traj.waypoints[i].velocity = min(v_target, v_limit) - v_prev = traj.waypoints[i].velocity - if v_prev >= v_target: - break - - for start_index, v_start, v_target in speed_up_adjust: - prev_i = start_index - v_prev = v_start - for i in range(start_index+1, len(traj.waypoints)): - dx = traj.waypoints[prev_i].position.x - traj.waypoints[i].position.x - dy = traj.waypoints[prev_i].position.y - traj.waypoints[i].position.y - dist = np.sqrt(dx**2 + dy**2) - v_limit = np.sqrt(v_prev**2 + 2 * max_acc * dist) - if v_limit > traj.waypoints[i].velocity: - break - traj.waypoints[i].velocity = min(v_target, v_limit) - v_prev = traj.waypoints[i].velocity - if v_prev >= v_target: - break - - -def get_accelerations(traj): - a_prev = 0.0 - - for i in range(len(traj.waypoints) - 2): - j = (i + 1) % len(traj.waypoints) - k = (i + 2) % len(traj.waypoints) - - dx1 = traj.waypoints[j].position.x - traj.waypoints[i].position.x - dy1 = traj.waypoints[j].position.y - traj.waypoints[i].position.y - dist1 = np.sqrt(dx1**2 + dy1**2) - - v_currx = traj.waypoints[i].velocity * (dx1) / dist1 - v_curry = traj.waypoints[i].velocity * (dy1) / dist1 - - dx2 = traj.waypoints[k].position.x - traj.waypoints[j].position.x - dy2 = traj.waypoints[k].position.y - traj.waypoints[j].position.y - dist2 = np.sqrt(dx2**2 + dy2**2) - - v_nextx = traj.waypoints[j].velocity * (dx2) / dist2 - v_nexty = traj.waypoints[j].velocity * (dy2) / dist2 - - acc_limit = 50. - if abs(dx1 - 0) < 1e-6: - traj.waypoints[i].acceleration.x = 0.0 - else: - traj.waypoints[i].acceleration.x = min( - (v_nextx**2 - v_currx**2) / (2 * dx1), acc_limit - ) - - if abs(dy1 - 0) < 1e-6: - traj.waypoints[i].acceleration.y = 0.0 - else: - traj.waypoints[i].acceleration.y = min( - (v_nexty**2 - v_curry**2) / (2 * dy1), acc_limit - ) - - traj.waypoints[i].acceleration.z = 0.0 - - traj.waypoints[len(traj.waypoints) - 2].acceleration.x = 0.0 - traj.waypoints[len(traj.waypoints) - 2].acceleration.y = 0.0 - traj.waypoints[len(traj.waypoints) - 2].acceleration.z = 0.0 - traj.waypoints[len(traj.waypoints) - 1].acceleration.x = 0.0 - traj.waypoints[len(traj.waypoints) - 1].acceleration.y = 0.0 - traj.waypoints[len(traj.waypoints) - 1].acceleration.z = 0.0 - - -def get_racetrack_waypoints(attributes): # length, width, height): - frame_id = str(attributes["frame_id"]) - length = float(attributes["length"]) - width = float(attributes["width"]) - height = float(attributes["height"]) - velocity = float(attributes["velocity"]) - turn_velocity = float(attributes["turn_velocity"]) - max_acceleration = float(attributes["max_acceleration"]) - - traj = TrajectoryXYZVYaw() - traj.header.frame_id = frame_id - straightaway_length = length - width - - # first straightaway - xs1 = np.linspace(0, straightaway_length, 80) - ys1 = np.zeros(xs1.shape) - vs1 = np.ones(xs1.shape)*velocity - yaws1 = np.zeros(xs1.shape) - - # first turn - t = np.linspace(-np.pi / 2, np.pi / 2, 50)[1:-1] - xs2 = width / 2.0 * np.cos(t) + straightaway_length - ys2 = width / 2.0 * np.sin(t) + width / 2.0 - xs2d = -width / 2.0 * np.sin(t) # derivative of xs - ys2d = width / 2.0 * np.cos(t) # derivative of ys - vs2 = np.ones(xs2.shape)*turn_velocity - yaws2 = np.arctan2(ys2d, xs2d) - - # second straightaway - xs3 = np.linspace(straightaway_length, 0, 80) - ys3 = width * np.ones(xs3.shape) - vs3 = np.ones(xs3.shape)*velocity - yaws3 = np.pi * np.ones(xs3.shape) - - # second turn - t = np.linspace(np.pi / 2, 3 * np.pi / 2, 50)[1:-1] - xs4 = width / 2.0 * np.cos(t) - ys4 = width / 2.0 * np.sin(t) + width / 2.0 - vs4 = np.ones(xs4.shape)*turn_velocity - yaws4 = yaws2 + np.pi - - xs = np.hstack((xs1, xs2, xs3, xs4)) - ys = np.hstack((ys1, ys2, ys3, ys4)) - vs = np.hstack((vs1, vs2, vs3, vs4)) - vs[0] = 0. - vs[-1] = 0. - yaws = np.hstack((yaws1, yaws2, yaws3, yaws4)) - - # now = rospy.Time.now() - for i in range(xs.shape[0]): - wp = WaypointXYZVYaw() - wp.position.x = xs[i] - wp.position.y = ys[i] - wp.position.z = height - wp.velocity = vs[i] - - #logger.info(str(i) + ' ' + str(wp.velocity)) - - wp.yaw = yaws[i] - - traj.waypoints.append(wp) - - #get_velocities_dual(traj, velocity, max_acceleration) - get_velocities_adjust(traj, max_acceleration) - get_accelerations(traj) - - return traj - - -def get_figure8_waypoints(attributes): # length, width, height): - frame_id = str(attributes["frame_id"]) - length = float(attributes["length"]) - width = float(attributes["width"]) - height = float(attributes["height"]) - velocity = float(attributes["velocity"]) - max_acceleration = float(attributes["max_acceleration"]) - - traj = TrajectoryXYZVYaw() - traj.header.frame_id = frame_id - # now = rospy.Time.now() - - # figure 8 points - t = np.linspace(0, 2 * np.pi, 600) - x = np.cos(t) * length - length - y = np.cos(t) * np.sin(t) * 2 * width - - # derivative of figure 8 curve, used to find yaw - xd = -np.sin(t) * length - yd = (np.cos(t) ** 2 - np.sin(t) ** 2) * 2 * width - - for i in range(t.shape[0] - 1): - x1 = x[i] - y1 = y[i] - x2 = x1 + xd[i] - y2 = y1 + yd[i] - - yaw = np.arctan2(y2 - y1, x2 - x1) - - wp = WaypointXYZVYaw() - wp.position.x = x1 - wp.position.y = y1 - wp.position.z = height - wp.yaw = yaw - - traj.waypoints.append(wp) - - get_velocities_dual(traj, velocity, max_acceleration) - get_accelerations(traj) - - return traj - - -def get_line_waypoints(attributes): # length, height): - frame_id = str(attributes["frame_id"]) - length = float(attributes["length"]) - height = float(attributes["height"]) - velocity = float(attributes["velocity"]) - max_acceleration = float(attributes["max_acceleration"]) - - traj = TrajectoryXYZVYaw() - traj.header.frame_id = frame_id - - for y in np.arange(0, -length, -0.5): - wp = WaypointXYZVYaw() - wp.position.x = -y - wp.position.y = 0. - wp.position.z = height - wp.yaw = 0. - - traj.waypoints.append(wp) - - get_velocities(traj, velocity, max_acceleration) - - return traj - - -def get_point_waypoints(attributes): # length, height): - frame_id = str(attributes["frame_id"]) - x = float(attributes["x"]) - y = float(attributes["y"]) - height = float(attributes["height"]) - velocity = float(attributes["velocity"]) - max_acceleration = float(attributes["max_acceleration"]) - - traj = TrajectoryXYZVYaw() - traj.header.frame_id = frame_id - - # add first point - wp = WaypointXYZVYaw() - wp.position.x = x - wp.position.y = y - wp.position.z = height - wp.yaw = 0. - - traj.waypoints.append(wp) - - get_velocities(traj, velocity, max_acceleration) - - return traj - - -def get_box_waypoints(attributes): # length, height): - frame_id = str(attributes["frame_id"]) - length = float(attributes["length"]) - height = float(attributes["height"]) - velocity = float(attributes["velocity"]) - max_acceleration = float(attributes["max_acceleration"]) - - traj = TrajectoryXYZVYaw() - traj.header.frame_id = frame_id - - wp1 = WaypointXYZVYaw() - wp1.position.x = 0.0 - wp1.position.y = 0.0 - wp1.position.z = height - wp1.yaw = 0 - traj.waypoints.append(wp1) - - wp2 = WaypointXYZVYaw() - wp2.position.x = length - wp2.position.y = 0.0 - wp2.position.z = height - wp2.yaw = 0.0 - traj.waypoints.append(wp2) - - wp3 = WaypointXYZVYaw() - wp3.position.x = length - wp3.position.y = 0.0 - wp3.position.z = height + height - wp3.yaw = 0.0 - traj.waypoints.append(wp3) - - wp4 = WaypointXYZVYaw() - wp4.position.x = 0.0 - wp4.position.y = 0.0 - wp4.position.z = height + height - wp4.yaw = 0.0 - traj.waypoints.append(wp4) - - return traj - - -def get_lawnmower_waypoints(attributes): # length, width, height, velocity): - frame_id = str(attributes["frame_id"]) - length = float(attributes["length"]) - width = float(attributes["width"]) - height = float(attributes["height"]) - velocity = float(attributes["velocity"]) - vertical = bool(int(attributes["vertical"])) - - traj = TrajectoryXYZVYaw() - traj.header.frame_id = frame_id - - for i in range(abs(int(height / width))): - wp1 = WaypointXYZVYaw() - wp1.position.x = 0.0 - wp1.position.y = 0.0 - wp1.position.z = np.sign(height) * (i + 1) * width - wp1.yaw = 0.0 - wp1.velocity = 0.1 - - wp1_ = WaypointXYZVYaw() - wp1_.position.x = 0.0 - wp1_.position.y = 0.5 - wp1_.position.z = np.sign(height) * (i + 1) * width - wp1_.yaw = 0.0 - wp1_.velocity = velocity - - wp2 = WaypointXYZVYaw() - wp2.position.x = 0.0 - wp2.position.y = length - wp2.position.z = np.sign(height) * (i + 1) * width - wp2.yaw = 0.0 - wp2.velocity = 0.1 - - wp2_ = WaypointXYZVYaw() - wp2_.position.x = 0.0 - wp2_.position.y = length - 0.5 - wp2_.position.z = np.sign(height) * (i + 1) * width - wp2_.yaw = 0.0 - wp2_.velocity = velocity - - yaw = np.arctan2(wp2.position.y - wp1.position.y, wp2.position.z - wp1.position.z) - - if i % 2 == 0: - if not vertical: - wp1.yaw = yaw - wp1_.yaw = yaw - wp2_.yaw = yaw - wp2.yaw = yaw - - traj.waypoints.append(wp1) - wp1_slow = copy.deepcopy(wp1_) - wp1_slow.velocity = 0.1 - traj.waypoints.append(wp1_slow) - traj.waypoints.append(wp1_) - traj.waypoints.append(wp2_) - traj.waypoints.append(wp2) - else: - if not vertical: - wp2.yaw = -yaw - wp2_.yaw = -yaw - wp1_.yaw = -yaw - wp1.yaw = -yaw - - traj.waypoints.append(wp2) - wp2_slow = copy.deepcopy(wp2_) - wp2_slow.velocity = 0.1 - traj.waypoints.append(wp2_slow) - traj.waypoints.append(wp2_) - traj.waypoints.append(wp1_) - traj.waypoints.append(wp1) - - wp = WaypointXYZVYaw() - wp.position.x = 0.0 - wp.position.y = 0.0 - wp.position.z = 0.0 - wp.yaw = 0.0 - wp.velocity = 0.5 - traj.waypoints.append(wp) - - if not vertical: - for i, wp in enumerate(traj.waypoints): - wp.position.x, wp.position.y, wp.position.z = wp.position.z, wp.position.y, wp.position.x - - return traj - - -def get_circle_waypoints(attributes): # radius, velocity, frame_id): - frame_id = str(attributes["frame_id"]) - radius = float(attributes["radius"]) - velocity = float(attributes["velocity"]) - - traj = TrajectoryXYZVYaw() - traj.header.frame_id = frame_id - # traj.header.stamp = rospy.Time.now() - - wp0 = WaypointXYZVYaw() - wp0.position.x = 0.0 - wp0.position.y = 0.0 - wp0.position.z = 0.0 - wp0.yaw = 0.0 - wp0.velocity = velocity - traj.waypoints.append(wp0) - - wp1 = WaypointXYZVYaw() - wp1.position.x = radius - wp1.position.y = 0.0 - wp1.position.z = 0.0 - wp1.yaw = 0.0 - wp1.velocity = velocity - traj.waypoints.append(wp1) - - for angle in np.arange(0, 2 * np.pi, 10.0 * np.pi / 180.0): - wp = WaypointXYZVYaw() - wp.position.x = radius * np.cos(angle) - wp.position.y = radius * np.sin(angle) - wp.position.z = 0.0 - wp.yaw = 0.0 - wp.velocity = velocity - traj.waypoints.append(wp) - - wp_end0 = WaypointXYZVYaw() - wp_end0.position.x = radius - wp_end0.position.y = 0.0 - wp_end0.position.z = 0.0 - wp_end0.yaw = 0.0 - wp_end0.velocity = velocity - traj.waypoints.append(wp_end0) - - wp_end1 = WaypointXYZVYaw() - wp_end1.position.x = 0.0 - wp_end1.position.y = 0.0 - wp_end1.position.z = 0.0 - wp_end1.yaw = 0.0 - wp_end1.velocity = velocity - traj.waypoints.append(wp_end1) - - return traj - - -class FixedTrajectoryGenerator(Node): - def __init__(self): - super().__init__("fixed_trajectory_generator") - self.fixed_trajectory_sub = self.create_subscription( - FixedTrajectory, - "fixed_trajectory_command", - self.fixed_trajectory_callback, - 1, - ) - self.trajectory_override_pub = self.create_publisher( - TrajectoryXYZVYaw, "trajectory_override", 1 - ) - - def fixed_trajectory_callback(self, msg): - print("generating") - attributes = {} - - for key_value in msg.attributes: - attributes[key_value.key] = key_value.value - - trajectory_msg = None - - if msg.type == "Figure8": - trajectory_msg = get_figure8_waypoints(attributes) - elif msg.type == "Circle": - trajectory_msg = get_circle_waypoints(attributes) - elif msg.type == "Racetrack": - trajectory_msg = get_racetrack_waypoints(attributes) - elif msg.type == "Line": - trajectory_msg = get_line_waypoints(attributes) - elif msg.type == "Point": - trajectory_msg = get_point_waypoints(attributes) - elif msg.type == "Lawnmower": - trajectory_msg = get_lawnmower_waypoints(attributes) - - if trajectory_msg != None: - self.trajectory_override_pub.publish(trajectory_msg) - else: - print("No trajectory sent.") - -#import matplotlib.pyplot as plt - -if __name__ == "__main__": - ''' - dct = {'frame_id': 'base_link', 'length': 2, 'width': 1, 'height': 0, 'velocity': 2, 'turn_velocity': 0.5, 'max_acceleration': 0.1} - traj1 = get_racetrack_waypoints(dct) - dct['max_acceleration'] = 100000000. - traj2 = get_racetrack_waypoints(dct) - - v1 = [wp.velocity for wp in traj1.waypoints] - v2 = [wp.velocity for wp in traj2.waypoints] - - plt.plot(v1, '.-') - plt.plot(v2, '.-') - plt.show() - exit() - - ''' - rclpy.init(args=None) - - node = FixedTrajectoryGenerator() - logger = node.get_logger() - - rclpy.spin(node) - node.destroy_node() - rclpy.shutdown() diff --git a/robot/ros_ws/src/local/planners/trajectory_library/src/fixed_trajectory_generator.py b/robot/ros_ws/src/local/planners/trajectory_library/src/fixed_trajectory_generator.py deleted file mode 100755 index b13f7e7c8..000000000 --- a/robot/ros_ws/src/local/planners/trajectory_library/src/fixed_trajectory_generator.py +++ /dev/null @@ -1,453 +0,0 @@ -#!/usr/bin/python -import numpy as np -from nav_msgs.msg import Odometry -from geometry_msgs.msg import Point -import rospy -import tf.transformations as trans -import argparse - -# from at_fcs_mavros.msg import Trajectory -# from at_fcs_mavros.msg import Waypoint -# from ca_nav_msgs.msg import XYZVPsi -# from ca_nav_msgs.msg import PathXYZVPsi -from airstack_msgs.msg import WaypointXYZVYaw -from airstack_msgs.msg import TrajectoryXYZVYaw -from trajectory_controller.msg import Trajectory -from airstack_msgs.msg import FixedTrajectory -import time -import copy - - -def get_velocities(traj, velocity, max_acc): - v_prev = 0.0 - - for i in range(len(traj.waypoints)): - j = (i + 1) % len(traj.waypoints) - dx = traj.waypoints[j].position.x - traj.waypoints[i].position.x - dy = traj.waypoints[j].position.y - traj.waypoints[i].position.y - - dist = np.sqrt(dx**2 + dy**2) - v_limit = np.sqrt(v_prev**2 + 2 * max_acc * dist) - traj.waypoints[i].velocity = min(velocity, v_limit) - v_prev = traj.waypoints[i].velocity - - -def get_velocities_dual(traj, velocity, max_acc): - v_prev = 0.0 - - for i in range(len(traj.waypoints)): - j = (i + 1) % len(traj.waypoints) - dx = traj.waypoints[j].position.x - traj.waypoints[i].position.x - dy = traj.waypoints[j].position.y - traj.waypoints[i].position.y - - dist = np.sqrt(dx**2 + dy**2) - v_limit = np.sqrt(v_prev**2 + 2 * max_acc * dist) - traj.waypoints[i].velocity = min(velocity, v_limit) - v_prev = traj.waypoints[i].velocity - - v_prev = 0.0 - for i in range(len(traj.waypoints) - 1, int(len(traj.waypoints) * 0.85), -1): - j = (i - 1) % len(traj.waypoints) - dx = traj.waypoints[j].position.x - traj.waypoints[i].position.x - dy = traj.waypoints[j].position.y - traj.waypoints[i].position.y - - dist = np.sqrt(dx**2 + dy**2) - v_limit = np.sqrt(v_prev**2 + 2 * max_acc * dist) - traj.waypoints[i].velocity = min(velocity, v_limit) - v_prev = traj.waypoints[i].velocity - - traj.waypoints[len(traj.waypoints) - 1].velocity = 0 - - -def get_accelerations(traj): - a_prev = 0.0 - - for i in range(len(traj.waypoints) - 2): - j = (i + 1) % len(traj.waypoints) - k = (i + 2) % len(traj.waypoints) - - dx1 = traj.waypoints[j].position.x - traj.waypoints[i].position.x - dy1 = traj.waypoints[j].position.y - traj.waypoints[i].position.y - dist1 = np.sqrt(dx1**2 + dy1**2) - - v_currx = traj.waypoints[i].velocity * (dx1) / dist1 - v_curry = traj.waypoints[i].velocity * (dy1) / dist1 - - dx2 = traj.waypoints[k].position.x - traj.waypoints[j].position.x - dy2 = traj.waypoints[k].position.y - traj.waypoints[j].position.y - dist2 = np.sqrt(dx2**2 + dy2**2) - - v_nextx = traj.waypoints[j].velocity * (dx2) / dist2 - v_nexty = traj.waypoints[j].velocity * (dy2) / dist2 - - acc_limit = 50 - if abs(dx1 - 0) < 1e-6: - traj.waypoints[i].acceleration.x = 0 - else: - traj.waypoints[i].acceleration.x = min( - (v_nextx**2 - v_currx**2) / (2 * dx1), acc_limit - ) - - if abs(dy1 - 0) < 1e-6: - traj.waypoints[i].acceleration.y = 0 - else: - traj.waypoints[i].acceleration.y = min( - (v_nexty**2 - v_curry**2) / (2 * dy1), acc_limit - ) - - traj.waypoints[i].acceleration.z = 0 - - traj.waypoints[len(traj.waypoints) - 2].acceleration.x = 0 - traj.waypoints[len(traj.waypoints) - 2].acceleration.y = 0 - traj.waypoints[len(traj.waypoints) - 2].acceleration.z = 0 - traj.waypoints[len(traj.waypoints) - 1].acceleration.x = 0 - traj.waypoints[len(traj.waypoints) - 1].acceleration.y = 0 - traj.waypoints[len(traj.waypoints) - 1].acceleration.z = 0 - - -def get_racetrack_waypoints(attributes): # length, width, height): - frame_id = str(attributes["frame_id"]) - length = float(attributes["length"]) - width = float(attributes["width"]) - height = float(attributes["height"]) - velocity = float(attributes["velocity"]) - max_acceleration = float(attributes["max_acceleration"]) - - traj = TrajectoryXYZVYaw() - traj.header.frame_id = frame_id - straightaway_length = length - width - - # first straightaway - xs1 = np.linspace(0, straightaway_length, 80) - ys1 = np.zeros(xs1.shape) - yaws1 = np.zeros(xs1.shape) - - # first turn - t = np.linspace(-np.pi / 2, np.pi / 2, 50)[1:-1] - xs2 = width / 2.0 * np.cos(t) + straightaway_length - ys2 = width / 2.0 * np.sin(t) + width / 2.0 - xs2d = -width / 2.0 * np.sin(t) # derivative of xs - ys2d = width / 2.0 * np.cos(t) # derivative of ys - yaws2 = np.arctan2(ys2d, xs2d) - - # second straightaway - xs3 = np.linspace(straightaway_length, 0, 80) - ys3 = width * np.ones(xs3.shape) - yaws3 = np.pi * np.ones(xs3.shape) - - # second turn - t = np.linspace(np.pi / 2, 3 * np.pi / 2, 50)[1:-1] - xs4 = width / 2.0 * np.cos(t) - ys4 = width / 2.0 * np.sin(t) + width / 2.0 - yaws4 = yaws2 + np.pi - - xs = np.hstack((xs1, xs2, xs3, xs4)) - ys = np.hstack((ys1, ys2, ys3, ys4)) - yaws = np.hstack((yaws1, yaws2, yaws3, yaws4)) - - now = rospy.Time.now() - for i in range(xs.shape[0]): - wp = WaypointXYZVYaw() - wp.position.x = xs[i] - wp.position.y = ys[i] - wp.position.z = height - wp.yaw = yaws[i] - - traj.waypoints.append(wp) - - get_velocities_dual(traj, velocity, max_acceleration) - get_accelerations(traj) - - return traj - - -def get_figure8_waypoints(attributes): # length, width, height): - frame_id = str(attributes["frame_id"]) - length = float(attributes["length"]) - width = float(attributes["width"]) - height = float(attributes["height"]) - velocity = float(attributes["velocity"]) - max_acceleration = float(attributes["max_acceleration"]) - - traj = TrajectoryXYZVYaw() - traj.header.frame_id = frame_id - now = rospy.Time.now() - - # figure 8 points - t = np.linspace(0, 2 * np.pi, 600) - x = np.cos(t) * length - length - y = np.cos(t) * np.sin(t) * 2 * width - - # derivative of figure 8 curve, used to find yaw - xd = -np.sin(t) * length - yd = (np.cos(t) ** 2 - np.sin(t) ** 2) * 2 * width - - for i in range(t.shape[0] - 1): - x1 = x[i] - y1 = y[i] - x2 = x1 + xd[i] - y2 = y1 + yd[i] - - yaw = np.arctan2(y2 - y1, x2 - x1) - - wp = WaypointXYZVYaw() - wp.position.x = x1 - wp.position.y = y1 - wp.position.z = height - wp.yaw = yaw - - traj.waypoints.append(wp) - - get_velocities_dual(traj, velocity, max_acceleration) - get_accelerations(traj) - - return traj - - -def get_line_waypoints(attributes): # length, height): - frame_id = str(attributes["frame_id"]) - length = float(attributes["length"]) - height = float(attributes["height"]) - velocity = float(attributes["velocity"]) - max_acceleration = float(attributes["max_acceleration"]) - - traj = TrajectoryXYZVYaw() - traj.header.frame_id = frame_id - - for y in np.arange(0, -length, -0.5): - wp = WaypointXYZVYaw() - wp.position.x = -y - wp.position.y = 0 - wp.position.z = height - wp.yaw = 0 - - traj.waypoints.append(wp) - - get_velocities(traj, velocity, max_acceleration) - - return traj - - -def get_point_waypoints(attributes): # length, height): - frame_id = str(attributes["frame_id"]) - x = float(attributes["x"]) - y = float(attributes["y"]) - height = float(attributes["height"]) - velocity = float(attributes["velocity"]) - max_acceleration = float(attributes["max_acceleration"]) - - traj = TrajectoryXYZVYaw() - traj.header.frame_id = frame_id - - # add first point - wp = WaypointXYZVYaw() - wp.position.x = x - wp.position.y = y - wp.position.z = height - wp.yaw = 0 - - traj.waypoints.append(wp) - - get_velocities(traj, velocity, max_acceleration) - - return traj - - -def get_box_waypoints(attributes): # length, height): - frame_id = str(attributes["frame_id"]) - length = float(attributes["length"]) - height = float(attributes["height"]) - velocity = float(attributes["velocity"]) - max_acceleration = float(attributes["max_acceleration"]) - - traj = TrajectoryXYZVYaw() - traj.header.frame_id = frame_id - - wp1 = WaypointXYZVYaw() - wp1.position.x = 0.0 - wp1.position.y = 0.0 - wp1.position.z = height - wp1.yaw = 0 - traj.waypoints.append(wp1) - - wp2 = WaypointXYZVYaw() - wp2.position.x = length - wp2.position.y = 0.0 - wp2.position.z = height - wp2.yaw = 0 - traj.waypoints.append(wp2) - - wp3 = WaypointXYZVYaw() - wp3.position.x = length - wp3.position.y = 0.0 - wp3.position.z = height + height - wp3.yaw = 0 - traj.waypoints.append(wp3) - - wp4 = WaypointXYZVYaw() - wp4.position.x = 0.0 - wp4.position.y = 0.0 - wp4.position.z = height + height - wp4.yaw = 0 - traj.waypoints.append(wp4) - - return traj - - -def get_vertical_lawnmower_waypoints(attributes): # length, width, height, velocity): - frame_id = str(attributes["frame_id"]) - length = float(attributes["length"]) - width = float(attributes["width"]) - height = float(attributes["height"]) - velocity = float(attributes["velocity"]) - - traj = TrajectoryXYZVYaw() - traj.header.frame_id = frame_id - - for i in range(abs(int(height / width))): - wp1 = WaypointXYZVYaw() - wp1.position.x = 0 - wp1.position.y = 0 - wp1.position.z = np.sign(height) * (i + 1) * width - wp1.yaw = 0 - wp1.velocity = 0.1 - - wp1_ = WaypointXYZVYaw() - wp1_.position.x = 0 - wp1_.position.y = 0.5 - wp1_.position.z = np.sign(height) * (i + 1) * width - wp1_.yaw = 0 - wp1_.velocity = velocity - - wp2 = WaypointXYZVYaw() - wp2.position.x = 0 - wp2.position.y = length - wp2.position.z = np.sign(height) * (i + 1) * width - wp2.yaw = 0 - wp2.velocity = 0.1 - - wp2_ = WaypointXYZVYaw() - wp2_.position.x = 0 - wp2_.position.y = length - 0.5 - wp2_.position.z = np.sign(height) * (i + 1) * width - wp2_.yaw = 0 - wp2_.velocity = velocity - - if i % 2 == 0: - traj.waypoints.append(wp1) - wp1_slow = copy.deepcopy(wp1_) - wp1_slow.velocity = 0.1 - traj.waypoints.append(wp1_slow) - traj.waypoints.append(wp1_) - traj.waypoints.append(wp2_) - traj.waypoints.append(wp2) - else: - traj.waypoints.append(wp2) - wp2_slow = copy.deepcopy(wp2_) - wp2_slow.velocity = 0.1 - traj.waypoints.append(wp2_slow) - traj.waypoints.append(wp2_) - traj.waypoints.append(wp1_) - traj.waypoints.append(wp1) - - wp = WaypointXYZVYaw() - wp.position.x = 0 - wp.position.y = 0 - wp.position.z = 0 - wp.yaw = 0 - wp.velocity = 0.5 - traj.waypoints.append(wp) - - return traj - - -def get_circle_waypoints(attributes): # radius, velocity, frame_id): - frame_id = str(attributes["frame_id"]) - radius = float(attributes["radius"]) - velocity = float(attributes["velocity"]) - - traj = TrajectoryXYZVYaw() - traj.header.frame_id = frame_id - traj.header.stamp = rospy.Time.now() - - wp0 = WaypointXYZVYaw() - wp0.position.x = 0 - wp0.position.y = 0 - wp0.position.z = 0 - wp0.yaw = 0 - wp0.velocity = velocity - traj.waypoints.append(wp0) - - wp1 = WaypointXYZVYaw() - wp1.position.x = radius - wp1.position.y = 0 - wp1.position.z = 0 - wp1.yaw = 0 - wp1.velocity = velocity - traj.waypoints.append(wp1) - - for angle in np.arange(0, 2 * np.pi, 10.0 * np.pi / 180.0): - wp = WaypointXYZVYaw() - wp.position.x = radius * np.cos(angle) - wp.position.y = radius * np.sin(angle) - wp.position.z = 0 - wp.yaw = 0 - wp.velocity = velocity - traj.waypoints.append(wp) - - wp_end0 = WaypointXYZVYaw() - wp_end0.position.x = radius - wp_end0.position.y = 0 - wp_end0.position.z = 0 - wp_end0.yaw = 0 - wp_end0.velocity = velocity - traj.waypoints.append(wp_end0) - - wp_end1 = WaypointXYZVYaw() - wp_end1.position.x = 0 - wp_end1.position.y = 0 - wp_end1.position.z = 0 - wp_end1.yaw = 0 - wp_end1.velocity = velocity - traj.waypoints.append(wp_end1) - - return traj - - -def fixed_trajectory_callback(msg): - attributes = {} - - for key_value in msg.attributes: - attributes[key_value.key] = key_value.value - - trajectory_msg = None - - if msg.type == "Figure8": - trajectory_msg = get_figure8_waypoints(attributes) - elif msg.type == "Circle": - trajectory_msg = get_circle_waypoints(attributes) - elif msg.type == "Racetrack": - trajectory_msg = get_racetrack_waypoints(attributes) - elif msg.type == "Line": - trajectory_msg = get_line_waypoints(attributes) - elif msg.type == "Point": - trajectory_msg = get_point_waypoints(attributes) - - if trajectory_msg != None: - trajectory_override_pub.publish(trajectory_msg) - else: - print("No trajectory sent.") - - -if __name__ == "__main__": - rospy.init_node("fixed_trajectory_generator") - - fixed_trajectory_sub = rospy.Subscriber( - "fixed_trajectory", FixedTrajectory, fixed_trajectory_callback - ) - - trajectory_override_pub = rospy.Publisher( - "trajectory_override", TrajectoryXYZVYaw, queue_size=1 - ) - - rospy.spin() diff --git a/scenes/two_drone_RetroNeighborhood.usd b/scenes/two_drone_RetroNeighborhood.usd deleted file mode 100644 index 18bfb3358..000000000 Binary files a/scenes/two_drone_RetroNeighborhood.usd and /dev/null differ diff --git a/scenes/two_drone_fire_new.usd b/scenes/two_drone_fire_new.usd deleted file mode 100644 index dd57a8d79..000000000 Binary files a/scenes/two_drone_fire_new.usd and /dev/null differ