diff --git a/terrain_abort_planner/CMakeLists.txt b/terrain_abort_planner/CMakeLists.txt new file mode 100644 index 00000000..bb5f3f7f --- /dev/null +++ b/terrain_abort_planner/CMakeLists.txt @@ -0,0 +1,66 @@ +cmake_minimum_required(VERSION 2.8.12) +project(terrain_abort_planner) + +add_definitions(-std=c++17 -Wall) + +find_package(GDAL) +find_package(OpenCV REQUIRED) +find_package(ompl REQUIRED) +find_package(Boost REQUIRED COMPONENTS serialization system filesystem) + +include(cmake/CPM.cmake) +CPMAddPackage("gh:nlohmann/json@3.11.3") + +find_package(catkin REQUIRED COMPONENTS + eigen_catkin + grid_map_core + grid_map_cv + grid_map_msgs + grid_map_ros + grid_map_pcl + grid_map_geo + terrain_navigation + terrain_planner + terrain_navigation_ros +) + +catkin_package( + INCLUDE_DIRS include + # LIBRARIES terrain_planner_benchmark +) + +include_directories( + ${catkin_INCLUDE_DIRS} + include + ${Boost_INCLUDE_DIR} + ${Eigen_INCLUDE_DIRS} + ${GeographicLib_INCLUDE_DIRS} + ${OMPL_INCLUDE_DIR} +) + +add_library(${PROJECT_NAME} + src/mission_io.cpp +) +add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} ${GeographicLib_LIBRARIES} ${OMPL_LIBRARIES}) + + +add_executable(run_dynamic_rallypoints + src/run_dynamic_rallypoints.cpp +) +add_dependencies(run_dynamic_rallypoints ${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(run_dynamic_rallypoints ${PROJECT_NAME} ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} ${OMPL_LIBRARIES} nlohmann_json::nlohmann_json) + +if(CATKIN_ENABLE_TESTING) + # Add gtest based cpp test target and link libraries + catkin_add_gtest(${PROJECT_NAME}-test test/main.cpp + test/test_mission_io.cpp) + # test/test_terrain_map.cpp) + + if(TARGET ${PROJECT_NAME}-test) + target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME} + ${catkin_LIBRARIES} + ${YAML_CPP_LIBRARIES} ${OpenCV_LIBRARIES}) + endif() + +endif() diff --git a/terrain_abort_planner/cmake/CPM.cmake b/terrain_abort_planner/cmake/CPM.cmake new file mode 100644 index 00000000..baf2d8c3 --- /dev/null +++ b/terrain_abort_planner/cmake/CPM.cmake @@ -0,0 +1,24 @@ +# SPDX-License-Identifier: MIT +# +# SPDX-FileCopyrightText: Copyright (c) 2019-2023 Lars Melchior and contributors + +set(CPM_DOWNLOAD_VERSION 0.40.2) +set(CPM_HASH_SUM "c8cdc32c03816538ce22781ed72964dc864b2a34a310d3b7104812a5ca2d835d") + +if(CPM_SOURCE_CACHE) + set(CPM_DOWNLOAD_LOCATION "${CPM_SOURCE_CACHE}/cpm/CPM_${CPM_DOWNLOAD_VERSION}.cmake") +elseif(DEFINED ENV{CPM_SOURCE_CACHE}) + set(CPM_DOWNLOAD_LOCATION "$ENV{CPM_SOURCE_CACHE}/cpm/CPM_${CPM_DOWNLOAD_VERSION}.cmake") +else() + set(CPM_DOWNLOAD_LOCATION "${CMAKE_BINARY_DIR}/cmake/CPM_${CPM_DOWNLOAD_VERSION}.cmake") +endif() + +# Expand relative path. This is important if the provided path contains a tilde (~) +get_filename_component(CPM_DOWNLOAD_LOCATION ${CPM_DOWNLOAD_LOCATION} ABSOLUTE) + +file(DOWNLOAD + https://github.com/cpm-cmake/CPM.cmake/releases/download/v${CPM_DOWNLOAD_VERSION}/CPM.cmake + ${CPM_DOWNLOAD_LOCATION} EXPECTED_HASH SHA256=${CPM_HASH_SUM} +) + +include(${CPM_DOWNLOAD_LOCATION}) diff --git a/terrain_abort_planner/include/terrain_abort_planner/mission_io.h b/terrain_abort_planner/include/terrain_abort_planner/mission_io.h new file mode 100644 index 00000000..f0e295fd --- /dev/null +++ b/terrain_abort_planner/include/terrain_abort_planner/mission_io.h @@ -0,0 +1,102 @@ +#include "terrain_navigation/path.h" + +Eigen::Vector4d rpy2quaternion(double roll, double pitch, double yaw) { + double cy = std::cos(yaw * 0.5); + double sy = std::sin(yaw * 0.5); + double cp = std::cos(pitch * 0.5); + double sp = std::sin(pitch * 0.5); + double cr = std::cos(roll * 0.5); + double sr = std::sin(roll * 0.5); + + Eigen::Vector4d q; + q(0) = cr * cp * cy + sr * sp * sy; + q(1) = sr * cp * cy - cr * sp * sy; + q(2) = cr * sp * cy + sr * cp * sy; + q(3) = cr * cp * sy - sr * sp * cy; + + q.normalize(); + + return q; +} + + +PathSegment generateTrajectory(const double curvature, const double length, Eigen::Vector3d segment_start, + Eigen::Vector3d segment_end, Eigen::Vector3d current_vel, const double dt) { + PathSegment trajectory; + trajectory.states.clear(); + double progress = 0.0; + double flightpath_angle = std::sin((segment_end(2) - segment_start(2)) / length); + trajectory.flightpath_angle = flightpath_angle; + /// TODO: Fix sign conventions for curvature + trajectory.curvature = curvature; + trajectory.dt = dt; + // if (std::abs(curvature) < 0.0001) { + // curvature > 0.0 ? trajectory.curvature = 0.0001 : trajectory.curvature = -0.0001; + // } + + double current_yaw = std::atan2(-1.0 * current_vel(1), current_vel(0)); + Eigen::Vector2d arc_center; + if (std::abs(curvature) > 0.001) { + arc_center = PathSegment::getArcCenter(trajectory.curvature, segment_start.head(2), current_vel.head(2), + segment_end.head(2)); + double radial_yaw = std::atan2(-segment_start(1) + arc_center(1), segment_start(0) - arc_center(0)); + current_yaw = curvature > 0.0 ? radial_yaw - 0.5 * M_PI : radial_yaw + 0.5 * M_PI; + } + + for (int i = 0; i < std::max(1.0, length / dt); i++) { + State state_vector; + double yaw = -trajectory.curvature * progress + current_yaw; + if (std::abs(curvature) < 0.001) { + Eigen::Vector3d tangent = (segment_end - segment_start).normalized(); + Eigen::Vector3d pos = progress * tangent + segment_start; + + Eigen::Vector3d vel = current_vel; + const double roll = std::atan(trajectory.curvature / 9.81); + const double pitch = flightpath_angle; + Eigen::Vector4d att = rpy2quaternion(roll, -pitch, -yaw); // TODO: why the hell do you need to reverse signs? + + state_vector.position = pos; + state_vector.velocity = vel; + state_vector.attitude = att; + } else { + Eigen::Vector3d pos = (-1.0 / trajectory.curvature) * Eigen::Vector3d(std::sin(yaw) - std::sin(current_yaw), + std::cos(yaw) - std::cos(current_yaw), 0) + + Eigen::Vector3d(0, 0, progress * std::sin(flightpath_angle)) + segment_start; + + Eigen::Vector3d vel = Eigen::Vector3d(std::cos(flightpath_angle) * std::cos(yaw), + -std::cos(flightpath_angle) * std::sin(yaw), std::sin(flightpath_angle)); + const double roll = std::atan(trajectory.curvature / 9.81); + const double pitch = flightpath_angle; + Eigen::Vector4d att = rpy2quaternion(roll, -pitch, -yaw); // TODO: why the hell do you need to reverse signs? + + state_vector.position = pos; + state_vector.velocity = vel; + state_vector.attitude = att; + } + trajectory.states.push_back(state_vector); + + progress = progress + dt; + } + double end_yaw = -trajectory.curvature * length + current_yaw; + State end_state_vector; + end_state_vector.position = segment_end; + end_state_vector.velocity = Eigen::Vector3d(std::cos(end_yaw), -std::sin(end_yaw), 0.0); + end_state_vector.attitude = Eigen::Vector4d(1.0, 0.0, 0.0, 0.0); + trajectory.states.push_back(end_state_vector); + return trajectory; +} + + +/** + * @brief + * + * @param waypoint_list + * @param radius + * @return Path + */ +Path generatePathFromWaypoints (std::vector waypoint_list, const double radius); + +double getTurnAngle(double current_course, double next_course) { + double turn_angle = std::abs(M_PI - std::abs(next_course - current_course)); + return turn_angle; +} diff --git a/terrain_abort_planner/launch/rallypoints.rviz b/terrain_abort_planner/launch/rallypoints.rviz new file mode 100644 index 00000000..952a577d --- /dev/null +++ b/terrain_abort_planner/launch/rallypoints.rviz @@ -0,0 +1,300 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /Marker1 + - /Marker2 + - /Path1 + - /GridMap1 + - /GridMap2 + - /GridMap3 + - /MarkerArray1 + - /MarkerArray2 + - /MarkerArray3 + - /MarkerArray4 + - /MarkerArray5 + - /MarkerArray6 + Splitter Ratio: 0.5 + Tree Height: 839 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Name: Time + SyncMode: 0 + SyncSource: "" +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /start_position + Name: Marker + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /goal_position + Name: Marker + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /path + Unreliable: false + Value: true + - Alpha: 1 + Class: rviz/Axes + Enabled: true + Length: 1 + Name: Axes + Radius: 0.10000000149011612 + Reference Frame: + Show Trail: false + Value: true + - Alpha: 0.5 + Autocompute Intensity Bounds: true + Class: grid_map_rviz_plugin/GridMap + Color: 200; 200; 200 + Color Layer: color + Color Transformer: ColorLayer + ColorMap: default + Enabled: true + Grid Cell Decimation: 1 + Grid Line Thickness: 0.10000000149011612 + Height Layer: elevation + Height Transformer: GridMapLayer + History Length: 1 + Invert ColorMap: false + Max Color: 255; 255; 255 + Max Intensity: 10 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: GridMap + Show Grid Lines: false + Topic: /grid_map + Unreliable: false + Use ColorMap: true + Value: true + - Alpha: 0.20000000298023224 + Autocompute Intensity Bounds: true + Class: grid_map_rviz_plugin/GridMap + Color: 255; 0; 0 + Color Layer: color + Color Transformer: FlatColor + ColorMap: default + Enabled: true + Grid Cell Decimation: 1 + Grid Line Thickness: 0.10000000149011612 + Height Layer: max_elevation + Height Transformer: GridMapLayer + History Length: 1 + Invert ColorMap: false + Max Color: 255; 255; 255 + Max Intensity: 10 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: GridMap + Show Grid Lines: false + Topic: /grid_map + Unreliable: false + Use ColorMap: true + Value: true + - Alpha: 0.20000000298023224 + Autocompute Intensity Bounds: true + Class: grid_map_rviz_plugin/GridMap + Color: 0; 121; 255 + Color Layer: color + Color Transformer: FlatColor + ColorMap: default + Enabled: true + Grid Cell Decimation: 1 + Grid Line Thickness: 0.10000000149011612 + Height Layer: distance_surface + Height Transformer: Layer + History Length: 1 + Invert ColorMap: false + Max Color: 255; 255; 255 + Max Intensity: 10 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: GridMap + Show Grid Lines: false + Topic: /grid_map + Unreliable: false + Use ColorMap: true + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /path_segments + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /rallypoints_marker + Name: MarkerArray + Namespaces: + rallypoints: true + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /abort_path_segments + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /geofence_marker + Name: MarkerArray + Namespaces: + geofence: true + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: false + Marker Topic: /waypoint_marker + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /mission_path_segments + Name: MarkerArray + Namespaces: + normals: true + Queue Size: 100 + Value: true + Enabled: true + Global Options: + Background Color: 255; 255; 255 + Default Light: true + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 3775.116455078125 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: -257.23699951171875 + Y: 220.40480041503906 + Z: -199.47006225585938 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.7197965979576111 + Target Frame: + Yaw: 0.4981505870819092 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1136 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd0000000400000000000001a1000003d2fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000003d2000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000020c000001b10000000000000000000000010000010f000003d2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000003d2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d0065010000000000000738000003bc00fffffffb0000000800540069006d0065010000000000000450000000000000000000000591000003d200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1848 + X: 72 + Y: 27 diff --git a/terrain_abort_planner/launch/run_dynamic_rallypoints.launch b/terrain_abort_planner/launch/run_dynamic_rallypoints.launch new file mode 100644 index 00000000..e6a10275 --- /dev/null +++ b/terrain_abort_planner/launch/run_dynamic_rallypoints.launch @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + diff --git a/terrain_abort_planner/package.xml b/terrain_abort_planner/package.xml new file mode 100644 index 00000000..ad18da3b --- /dev/null +++ b/terrain_abort_planner/package.xml @@ -0,0 +1,32 @@ + + terrain_abort_planner + 0.0.1 + Terrain aware abort planning + + Jaeyoung Lim + + Jaeyoung Lim + + BSD-3 + + https://github.com/Jaeyoung-Lim/steep-terrain-navigation + https://github.com/Jaeyoung-Lim/steep-terrain-navigation/issues + + + catkin + eigen_catkin + grid_map_core + grid_map_cv + grid_map_ros + grid_map_msgs + grid_map_pcl + grid_map_geo + terrain_navigation + terrain_navigation_ros + ompl + terrain_planner + eigen_catkin + python3-gdal + + + diff --git a/terrain_abort_planner/resources/duebendorf.tif b/terrain_abort_planner/resources/duebendorf.tif new file mode 100644 index 00000000..2d36e415 Binary files /dev/null and b/terrain_abort_planner/resources/duebendorf.tif differ diff --git a/terrain_abort_planner/resources/duebendorf_color.tif b/terrain_abort_planner/resources/duebendorf_color.tif new file mode 100644 index 00000000..5687f8ca Binary files /dev/null and b/terrain_abort_planner/resources/duebendorf_color.tif differ diff --git a/terrain_abort_planner/src/mission_io.cpp b/terrain_abort_planner/src/mission_io.cpp new file mode 100644 index 00000000..db584905 --- /dev/null +++ b/terrain_abort_planner/src/mission_io.cpp @@ -0,0 +1,45 @@ +#include "terrain_abort_planner/mission_io.h" + + +Path generatePathFromWaypoints (std::vector waypoint_list, const double radius) { + Path mission_path; + double previous_distance_to_tangent = 0.0; + for (int waypoint_id = 1; waypoint_id < waypoint_list.size() - 1; waypoint_id++) { + const Eigen::Vector3d current_vertex = waypoint_list[waypoint_id]; + const Eigen::Vector3d current_edge = current_vertex - waypoint_list[waypoint_id - 1]; + const double current_edge_length = current_edge.norm(); + // if (current_edge_length < 0.01) continue; //Same position + const Eigen::Vector3d current_tangent = current_edge / current_edge_length; + const double current_course = std::atan2(current_tangent[1], current_tangent[0]); + + Eigen::Vector3d next_edge = waypoint_list[waypoint_id + 1] - current_vertex; + double next_edge_length = next_edge.norm(); + Eigen::Vector3d next_tangent = next_edge / next_edge_length; + double next_course = std::atan2(next_tangent[1], next_tangent[0]); + + double turn_angle = getTurnAngle(current_course, next_course); + double distance_to_tangent = radius / std::abs(std::tan(0.5 * turn_angle)); + + /// TODO: Arc segment + Eigen::Vector3d arc_segment_start = current_vertex - distance_to_tangent * current_tangent; + Eigen::Vector3d arc_segment_end = current_vertex + distance_to_tangent * next_tangent; + double arc_segment_length = std::abs(M_PI - turn_angle) * radius; + Eigen::Vector3d rotation_vector = current_tangent.cross(next_tangent); + double curvature = rotation_vector(2) > 0.0 ? 1.0 / radius : -1.0 / radius; + + auto arc_segment = + generateTrajectory(curvature, arc_segment_length, arc_segment_start, arc_segment_end, current_tangent, 0.1); + /// TODO: Line segment + Eigen::Vector3d line_segment_start = + waypoint_list[waypoint_id - 1] + current_tangent * previous_distance_to_tangent; + Eigen::Vector3d line_segment_end = arc_segment_start; + double line_segment_length = current_edge_length - previous_distance_to_tangent - distance_to_tangent; + auto line_segment = + generateTrajectory(0.0, line_segment_length, line_segment_start, line_segment_end, current_tangent, 0.1); + previous_distance_to_tangent = distance_to_tangent; + + mission_path.appendSegment(line_segment); + mission_path.appendSegment(arc_segment); + } + return mission_path; +} diff --git a/terrain_abort_planner/src/run_dynamic_rallypoints.cpp b/terrain_abort_planner/src/run_dynamic_rallypoints.cpp new file mode 100644 index 00000000..19a43239 --- /dev/null +++ b/terrain_abort_planner/src/run_dynamic_rallypoints.cpp @@ -0,0 +1,429 @@ +/**************************************************************************** + * + * Copyright (c) 2021 Jaeyoung Lim, Autonomous Systems Lab, + * ETH Zürich. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. 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. + * 3. Neither the name of the copyright holder 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. + * + ****************************************************************************/ +/** + * @brief ROS Node to test ompl + * + * + * @author Jaeyoung Lim + */ + +#include +#include +#include + +#include + +#include +#include +#include +#include +#include + +#include "terrain_navigation/data_logger.h" +#include "terrain_navigation_ros/geo_conversions.h" +#include "terrain_navigation_ros/visualization.h" +#include "terrain_planner/common.h" +#include "terrain_planner/terrain_ompl_rrt.h" +#include "terrain_abort_planner/mission_io.h" + +#include +using json = nlohmann::json; + +void publishPathSegments(ros::Publisher& pub, Path& trajectory, + Eigen::Vector3d color = Eigen::Vector3d(0.0, 1.0, 0.0)) { + visualization_msgs::MarkerArray msg; + + std::vector marker; + visualization_msgs::Marker mark; + mark.action = visualization_msgs::Marker::DELETEALL; + marker.push_back(mark); + msg.markers = marker; + pub.publish(msg); + + std::vector segment_markers; + int i = 0; + for (auto& segment : trajectory.segments) { + segment_markers.insert(segment_markers.begin(), trajectory2MarkerMsg(segment, i++, color)); + segment_markers.insert(segment_markers.begin(), + vector2ArrowsMsg(segment.position().front(), 5.0 * segment.velocity().front(), i++, color)); + segment_markers.insert(segment_markers.begin(), point2MarkerMsg(segment.position().back(), i++, color)); + } + msg.markers = segment_markers; + pub.publish(msg); +} + +visualization_msgs::Marker getGoalMarker(const int id, const Eigen::Vector3d& position, const double radius, + const Eigen::Vector3d color) { + visualization_msgs::Marker marker; + marker.header.frame_id = "map"; + marker.header.stamp = ros::Time::now(); + marker.id = id; + marker.type = visualization_msgs::Marker::LINE_STRIP; + marker.action = visualization_msgs::Marker::ADD; + std::vector points; + /// TODO: Generate circular path + double delta_theta = 0.05 * 2 * M_PI; + for (double theta = 0.0; theta < 2 * M_PI + delta_theta; theta += delta_theta) { + geometry_msgs::Point point; + point.x = position(0) + radius * std::cos(theta); + point.y = position(1) + radius * std::sin(theta); + point.z = position(2); + points.push_back(point); + } + marker.points = points; + marker.scale.x = 5.0; + marker.scale.y = 5.0; + marker.scale.z = 5.0; + marker.color.a = 0.5; // Don't forget to set the alpha! + marker.pose.orientation.w = 1.0; + marker.pose.orientation.x = 0.0; + marker.pose.orientation.y = 0.0; + marker.pose.orientation.z = 0.0; + marker.color.r = color(0); + marker.color.g = color(1); + marker.color.b = color(2); + return marker; +} + +void publishRallyPoints(const ros::Publisher& pub, const std::vector& positions, const double radius, + Eigen::Vector3d color = Eigen::Vector3d(1.0, 1.0, 0.0), + std::string name_space = "rallypoints") { + visualization_msgs::MarkerArray marker_array; + std::vector markers; + int marker_id = 1; + for (const auto& position : positions) { + visualization_msgs::Marker marker; + marker = getGoalMarker(marker_id, position, radius, color); + marker.ns = name_space; + markers.push_back(marker); + marker_id++; + } + marker_array.markers = markers; + pub.publish(marker_array); +} + +void publishGeoFence(const ros::Publisher& pub, const std::vector& geofence_polygon, + Eigen::Vector3d color = Eigen::Vector3d(1.0, 1.0, 0.0), std::string name_space = "geofence") { + visualization_msgs::MarkerArray marker_array; + std::vector markers; + for (int marker_id = 0; marker_id < geofence_polygon.size() - 1; marker_id++) { + visualization_msgs::Marker marker; + marker.header.frame_id = "map"; + marker.header.stamp = ros::Time::now(); + marker.id = marker_id; + marker.type = visualization_msgs::Marker::LINE_STRIP; + marker.action = visualization_msgs::Marker::ADD; + std::vector points; + for (auto vertex : geofence_polygon) { + geometry_msgs::Point point; + point.x = vertex[0]; + point.y = vertex[1]; + point.z = 500.0; + points.push_back(point); + } + points.push_back(points.front()); // Close the polygon + marker.points = points; + marker.scale.x = 10.0; + marker.scale.y = 10.0; + marker.scale.z = 10.0; + marker.color.a = 0.5; // Don't forget to set the alpha! + marker.pose.orientation.w = 1.0; + marker.pose.orientation.x = 0.0; + marker.pose.orientation.y = 0.0; + marker.pose.orientation.z = 0.0; + marker.color.r = color(0); + marker.color.g = color(1); + marker.color.b = color(2); + marker.ns = name_space; + markers.push_back(marker); + } + marker_array.markers = markers; + pub.publish(marker_array); +} + +void publishWaypoints(const ros::Publisher& pub, const std::vector& geofence_polygon, + Eigen::Vector3d color = Eigen::Vector3d(0.0, 0.0, 1.0), std::string name_space = "waypoint") { + visualization_msgs::MarkerArray marker_array; + std::vector markers; + for (int marker_id = 0; marker_id < geofence_polygon.size() - 1; marker_id++) { + visualization_msgs::Marker marker; + marker.header.frame_id = "map"; + marker.header.stamp = ros::Time::now(); + marker.id = marker_id; + marker.type = visualization_msgs::Marker::LINE_STRIP; + marker.action = visualization_msgs::Marker::ADD; + std::vector points; + for (auto vertex : geofence_polygon) { + geometry_msgs::Point point; + point.x = vertex[0]; + point.y = vertex[1]; + point.z = vertex[2]; + points.push_back(point); + } + points.push_back(points.front()); // Close the polygon + marker.points = points; + marker.scale.x = 10.0; + marker.scale.y = 10.0; + marker.scale.z = 10.0; + marker.color.a = 0.5; // Don't forget to set the alpha! + marker.pose.orientation.w = 1.0; + marker.pose.orientation.x = 0.0; + marker.pose.orientation.y = 0.0; + marker.pose.orientation.z = 0.0; + marker.color.r = color(0); + marker.color.g = color(1); + marker.color.b = color(2); + marker.ns = name_space; + markers.push_back(marker); + } + marker_array.markers = markers; + pub.publish(marker_array); +} + +void publishGridMap(const ros::Publisher& pub, const grid_map::GridMap& map) { + grid_map_msgs::GridMap message; + grid_map::GridMapRosConverter::toMessage(map, message); + pub.publish(message); +} + + +double mod2pi(double x) { return x - 2 * M_PI * floor(x * (0.5 / M_PI)); } + +PathSegment generateArcTrajectory(Eigen::Vector3d rate, const double horizon, Eigen::Vector3d current_pos, + Eigen::Vector3d current_vel, const double dt = 0.1) { + PathSegment trajectory; + trajectory.states.clear(); + + double cruise_speed_{20.0}; + + double time = 0.0; + const double current_yaw = std::atan2(-1.0 * current_vel(1), current_vel(0)); + const double climb_rate = rate(1); + trajectory.flightpath_angle = std::asin(climb_rate / cruise_speed_); + /// TODO: Fix sign conventions for curvature + trajectory.curvature = -rate(2) / cruise_speed_; + trajectory.dt = dt; + for (int i = 0; i < std::max(1.0, horizon / dt); i++) { + if (std::abs(rate(2)) < 0.0001) { + rate(2) > 0.0 ? rate(2) = 0.0001 : rate(2) = -0.0001; + } + double yaw = rate(2) * time + current_yaw; + + Eigen::Vector3d pos = + cruise_speed_ / rate(2) * + Eigen::Vector3d(std::sin(yaw) - std::sin(current_yaw), std::cos(yaw) - std::cos(current_yaw), 0) + + Eigen::Vector3d(0, 0, climb_rate * time) + current_pos; + Eigen::Vector3d vel = Eigen::Vector3d(cruise_speed_ * std::cos(yaw), -cruise_speed_ * std::sin(yaw), -climb_rate); + const double roll = std::atan(rate(2) * cruise_speed_ / 9.81); + const double pitch = std::atan(climb_rate / cruise_speed_); + Eigen::Vector4d att = rpy2quaternion(roll, -pitch, -yaw); // TODO: why the hell do you need to reverse signs? + + State state_vector; + state_vector.position = pos; + state_vector.velocity = vel; + state_vector.attitude = att; + trajectory.states.push_back(state_vector); + + time = time + dt; + } + return trajectory; +} + +int main(int argc, char** argv) { + ros::init(argc, argv, "ompl_rrt_planner"); + ros::NodeHandle nh(""); + ros::NodeHandle nh_private("~"); + + // Initialize ROS related publishers for visualization + auto grid_map_pub = nh.advertise("grid_map", 1, true); + auto trajectory_pub = nh.advertise("tree", 1, true); + auto abort_path_pub = nh.advertise("abort_path_segments", 1, true); + auto mission_path_pub = nh.advertise("mission_path_segments", 1, true); + auto rallypoint_pub = nh.advertise("rallypoints_marker", 1); + auto geofence_pub = nh.advertise("geofence_marker", 1); + auto waypoint_pub = nh.advertise("waypoint_marker", 1); + + std::string map_path, color_file_path, output_directory, location, mission_file_path; + nh_private.param("map_path", map_path, ""); + nh_private.param("color_file_path", color_file_path, ""); + nh_private.param("location", location, ""); + nh_private.param("mission_file_path", mission_file_path, ""); + nh_private.param("output_directory", output_directory, ""); + + // Initialize data logger for recording + auto data_logger = std::make_shared(); + data_logger->setKeys({"x", "y", "z"}); + + // Load terrain map from defined tif paths + auto terrain_map = std::make_shared(); + terrain_map->initializeFromGeotiff(map_path); + if (!color_file_path.empty()) { // Load color layer if the color path is nonempty + terrain_map->addColorFromGeotiff(color_file_path); + } + terrain_map->AddLayerDistanceTransform(20.0, "distance_surface"); + terrain_map->AddLayerDistanceTransform(200.0, "max_elevation"); + double radius = 66.667; + terrain_map->AddLayerHorizontalDistanceTransform(radius, "ics_+", "distance_surface"); + terrain_map->AddLayerHorizontalDistanceTransform(-radius, "ics_-", "max_elevation"); + + // Parse mission file to get geofence information + std::vector geofence_polygon; + std::ifstream f(mission_file_path); + json data = json::parse(f); + json polygons = data["geoFence"]["polygons"]; + std::cout << "Number of vertices: " << polygons[0]["polygon"].size() << std::endl; + + grid_map::Polygon map_polygon; + map_polygon.setFrameId(terrain_map->getGridMap().getFrameId()); + + for (auto polygon_vertex : polygons[0]["polygon"]) { + Eigen::Vector2d vertex_wgs84 = Eigen::Vector2d(polygon_vertex[0], polygon_vertex[1]); + // Convert vertex into lv03 + Eigen::Vector3d vertex_lv03; + GeoConversions::forward(vertex_wgs84[0], vertex_wgs84[1], 0.0, vertex_lv03.x(), vertex_lv03.y(), + vertex_lv03.z()); /// FIXME: Assuming zero AMSL + + // Convert vertex into local frame (Relative to map origin) + ESPG map_coordinate; + Eigen::Vector3d map_origin; + terrain_map->getGlobalOrigin(map_coordinate, map_origin); + Eigen::Vector3d vertex_local = vertex_lv03 - map_origin; + geofence_polygon.push_back(vertex_local.head(2)); + map_polygon.addVertex(vertex_local.head(2)); + } + + terrain_map->getGridMap().add("valid"); + terrain_map->getGridMap()["valid"].setConstant(0.0); + for (grid_map::PolygonIterator iterator(terrain_map->getGridMap(), map_polygon); !iterator.isPastEnd(); ++iterator) { + terrain_map->getGridMap().at("valid", *iterator) = 1.0; + } + + // for (auto vertex : polygon) { + // std::cout << "Parsed Geofence vertex: " << vertex.transpose() << std::endl; + // } + + // Parse mission to generate reference path + std::vector waypoint_list; + json mission = data["mission"]["items"]; + for (auto mission_item : mission) { + std::cout << "Mission Item: " << mission_item << std::endl; + int command = mission_item["command"]; + std::cout << " - command : " << command << std::endl; + switch (command) { + case 16: { // waypoint + const double waypoint_lat = mission_item["params"][4]; + const double waypoint_lon = mission_item["params"][5]; + const double waypoint_relative_alt = mission_item["params"][6]; + Eigen::Vector3d waypoint_lv03; + GeoConversions::forward(waypoint_lat, waypoint_lon, 0.0, waypoint_lv03.x(), waypoint_lv03.y(), + waypoint_lv03.z()); /// FIXME: Assuming zero AMSL + ESPG map_coordinate; + Eigen::Vector3d map_origin; + terrain_map->getGlobalOrigin(map_coordinate, map_origin); + Eigen::Vector3d waypoint_local = waypoint_lv03 - map_origin; + waypoint_local.z() = + terrain_map->getGridMap().atPosition("elevation", waypoint_local.head(2)) + waypoint_relative_alt; + waypoint_list.push_back(waypoint_local); + } + } + } + /// TODO: Generate Dubins path segments from waypoint list + Path mission_path = generatePathFromWaypoints(waypoint_list, radius); + + /// Parse rally points from mission file + std::vector rallypoint_list; + const double map_width_x = terrain_map->getGridMap().getLength().x(); + const double map_width_y = terrain_map->getGridMap().getLength().y(); + const Eigen::Vector2d map_pos = terrain_map->getGridMap().getPosition(); + + json mission_rallypoints = data["rallyPoints"]["points"]; + for (auto rallypoint : mission_rallypoints) { + std::cout << " - Rallypoint: " << rallypoint << std::endl; + const double rallypoint_lat = rallypoint[0]; + const double rallypoint_lon = rallypoint[1]; + const double rallypoint_relative_alt = rallypoint[2]; + Eigen::Vector3d rallypoint_lv03; + GeoConversions::forward(rallypoint_lat, rallypoint_lon, 0.0, rallypoint_lv03.x(), rallypoint_lv03.y(), + rallypoint_lv03.z()); /// FIXME: Assuming zero AMSL + ESPG map_coordinate; + Eigen::Vector3d map_origin; + terrain_map->getGlobalOrigin(map_coordinate, map_origin); + Eigen::Vector3d rallypoint_local = rallypoint_lv03 - map_origin; + rallypoint_local.z() = + terrain_map->getGridMap().atPosition("elevation", rallypoint_local.head(2)) + rallypoint_relative_alt; + rallypoint_list.push_back(rallypoint_local); + } + + /// TODO: Generate path from waypoint list + + // Initialize planner with loaded terrain map + auto planner = std::make_shared(); + planner->setMap(terrain_map); + /// TODO: Get bounds from gridmap + planner->setBoundsFromMap(terrain_map->getGridMap()); + + /// TODO: Iterate over path segments to find abort paths + int segment_id = 0; + Path abort_path_list; + for (const auto& path_segment : mission_path.segments) { + std::cout << " - segment ID: " << segment_id << " / " << mission_path.segments.size() << std::endl; + Eigen::Vector3d segment_end = path_segment.states.back().position; + Eigen::Vector3d segment_end_tangent = path_segment.states.back().velocity; + planner->setupProblem(segment_end, segment_end_tangent, rallypoint_list); + Path abort_path; + if (planner->Solve(2.0, abort_path)) { + std::cout << "[TestRRTCircleGoal] Found Solution!" << std::endl; + abort_path_list.appendSegment(abort_path); + } else { + std::cout << "[TestRRTCircleGoal] Unable to find solution" << std::endl; + } + segment_id++; + } + + // Repeatedly publish results + while (true) { + terrain_map->getGridMap().setTimestamp(ros::Time::now().toNSec()); + publishGridMap(grid_map_pub, terrain_map->getGridMap()); + publishWaypoints(waypoint_pub, waypoint_list); + publishRallyPoints(rallypoint_pub, rallypoint_list, radius, Eigen::Vector3d(1.0, 0.0, 0.0)); + publishGeoFence(geofence_pub, geofence_polygon); /// Visualize Geofence + + publishPathSegments(abort_path_pub, abort_path_list, Eigen::Vector3d(1.0, 0.0, 0.0)); + publishPathSegments(mission_path_pub, mission_path); + ros::Duration(1.0).sleep(); + ros::spinOnce(); + } + + return 0; +} diff --git a/terrain_abort_planner/test/main.cpp b/terrain_abort_planner/test/main.cpp new file mode 100644 index 00000000..4d820af7 --- /dev/null +++ b/terrain_abort_planner/test/main.cpp @@ -0,0 +1,6 @@ +#include + +int main(int argc, char **argv) { + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/terrain_abort_planner/test/test_mission_io.cpp b/terrain_abort_planner/test/test_mission_io.cpp new file mode 100644 index 00000000..049cdfae --- /dev/null +++ b/terrain_abort_planner/test/test_mission_io.cpp @@ -0,0 +1,20 @@ +#include "terrain_abort_planner/mission_io.h" + +#include +#include + +TEST(MissionIO, PathGeneration) { + EXPECT_NEAR(getTurnAngle(0.25 * M_PI, 0.25*M_PI), M_PI, 0.001); + EXPECT_NEAR(getTurnAngle(0.0, 0.5*M_PI), 0.5*M_PI, 0.001); + EXPECT_NEAR(getTurnAngle(0.0, -0.5*M_PI), 0.5*M_PI, 0.001); + EXPECT_NEAR(getTurnAngle(0.75 * M_PI, -0.75 * M_PI), 0.5*M_PI, 0.001); + + // EXPECT_NEAR(getTurnAngle(0.25 * M_PI, 0.25*M_PI), M_PI, 0.001); + // EXPECT_NEAR(getTurnAngle(0.25 * M_PI, 0.25*M_PI), M_PI, 0.001); + // EXPECT_NEAR(getTurnAngle(0.25 * M_PI, 0.25*M_PI), M_PI, 0.001); + + + std::vector waypoint_list; + double radius = 1.0; + +} diff --git a/terrain_planner/src/terrain_ompl.cpp b/terrain_planner/src/terrain_ompl.cpp index 51849bdc..6ba32e1c 100644 --- a/terrain_planner/src/terrain_ompl.cpp +++ b/terrain_planner/src/terrain_ompl.cpp @@ -17,7 +17,11 @@ bool TerrainValidityChecker::checkCollision(const Eigen::Vector3d state) const { if (check_collision_max_altitude_) { in_collision_max = isInCollision("max_elevation", state, false); } - return in_collision | in_collision_max; + bool valid; + if (map_.isInside(state.head(2))) { + valid = bool(map_.atPosition("valid", state.head(2)) > 0.5); + } + return in_collision | in_collision_max | !valid; } bool TerrainValidityChecker::isInCollision(const std::string& layer, const Eigen::Vector3d& position, diff --git a/terrain_planner_benchmark/resources/mission.plan b/terrain_planner_benchmark/resources/mission.plan new file mode 100644 index 00000000..4d947819 --- /dev/null +++ b/terrain_planner_benchmark/resources/mission.plan @@ -0,0 +1,944 @@ +{ + "fileType": "Plan", + "geoFence": { + "circles": [ + ], + "polygons": [ + { + "inclusion": true, + "polygon": [ + [ + 47.3995435, + 8.6342263 + ], + [ + 47.3998151, + 8.6347173 + ], + [ + 47.4051139, + 8.63628066018961 + ], + [ + 47.4034173, + 8.64407071678632 + ], + [ + 47.3992774, + 8.64647657961871 + ], + [ + 47.3973405, + 8.65530788210469 + ], + [ + 47.3987839, + 8.65980251496503 + ], + [ + 47.4120133, + 8.65854392435929 + ], + [ + 47.4163639, + 8.65264544141767 + ], + [ + 47.4185237, + 8.64483489332498 + ], + [ + 47.4212578, + 8.64397904970693 + ], + [ + 47.421464, + 8.64904916305701 + ], + [ + 47.4181109, + 8.67626220101532 + ], + [ + 47.414485, + 8.67950257599716 + ], + [ + 47.4084853, + 8.67683647521679 + ], + [ + 47.4044727, + 8.67051670218592 + ], + [ + 47.3981726, + 8.66316548035531 + ], + [ + 47.3954762, + 8.66205316972209 + ], + [ + 47.3967777, + 8.64632926631228 + ], + [ + 47.3983757, + 8.63849313164363 + ], + [ + 47.3994598, + 8.6351237 + ], + [ + 47.3991951, + 8.6346305 + ] + ], + "version": 1 + } + ], + "version": 2 + }, + "groundStation": "QGroundControl", + "mission": { + "cruiseSpeed": 15, + "firmwareType": 12, + "globalPlanAltitudeMode": 1, + "hoverSpeed": 5, + "items": [ + { + "AMSLAltAboveTerrain": 10, + "Altitude": 10, + "AltitudeMode": 1, + "autoContinue": true, + "command": 22, + "doJumpId": 1, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.3996294, + 8.634886, + 10 + ], + "type": "SimpleItem" + }, + { + "AMSLAltAboveTerrain": 40, + "Altitude": 40, + "AltitudeMode": 1, + "autoContinue": true, + "command": 16, + "doJumpId": 2, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.39889836687525, + 8.645315553166597, + 40 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 178, + "doJumpId": 3, + "frame": 2, + "params": [ + 1, + 20, + -1, + 0, + 0, + 0, + 0 + ], + "type": "SimpleItem" + }, + { + "AMSLAltAboveTerrain": 60, + "Altitude": 60, + "AltitudeMode": 1, + "autoContinue": true, + "command": 16, + "doJumpId": 4, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.39678792030329, + 8.654702191101194, + 60 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 178, + "doJumpId": 5, + "frame": 2, + "params": [ + 1, + 20, + -1, + 0, + 0, + 0, + 0 + ], + "type": "SimpleItem" + }, + { + "AMSLAltAboveTerrain": 100, + "Altitude": 100, + "AltitudeMode": 1, + "autoContinue": true, + "command": 16, + "doJumpId": 6, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.39606860110815, + 8.659074399928272, + 100 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 178, + "doJumpId": 7, + "frame": 2, + "params": [ + 1, + 15, + -1, + 0, + 0, + 0, + 0 + ], + "type": "SimpleItem" + }, + { + "AMSLAltAboveTerrain": 100, + "Altitude": 100, + "AltitudeMode": 1, + "autoContinue": true, + "command": 16, + "doJumpId": 8, + "frame": 3, + "params": [ + 0, + 90, + 0, + null, + 47.395881074800236, + 8.660207928060828, + 100 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 178, + "doJumpId": 9, + "frame": 2, + "params": [ + 1, + 15, + -1, + 0, + 0, + 0, + 0 + ], + "type": "SimpleItem" + }, + { + "AMSLAltAboveTerrain": 100, + "Altitude": 100, + "AltitudeMode": 1, + "autoContinue": true, + "command": 16, + "doJumpId": 10, + "frame": 3, + "params": [ + 0, + 120, + 0, + null, + 47.398503537843055, + 8.66145691302944, + 100 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 178, + "doJumpId": 11, + "frame": 2, + "params": [ + 1, + 20, + -1, + 0, + 0, + 0, + 0 + ], + "type": "SimpleItem" + }, + { + "AMSLAltAboveTerrain": 120, + "Altitude": 120, + "AltitudeMode": 1, + "autoContinue": true, + "command": 16, + "doJumpId": 12, + "frame": 3, + "params": [ + 0, + 100, + 0, + null, + 47.402528492920304, + 8.66708579301823, + 120 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 178, + "doJumpId": 13, + "frame": 2, + "params": [ + 1, + 20, + -1, + 0, + 0, + 0, + 0 + ], + "type": "SimpleItem" + }, + { + "AMSLAltAboveTerrain": 162, + "Altitude": 162, + "AltitudeMode": 1, + "autoContinue": true, + "command": 16, + "doJumpId": 14, + "frame": 3, + "params": [ + 0, + 100, + 0, + null, + 47.40904112549192, + 8.675663512366128, + 162 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 178, + "doJumpId": 15, + "frame": 2, + "params": [ + 1, + 20, + -1, + 0, + 0, + 0, + 0 + ], + "type": "SimpleItem" + }, + { + "AMSLAltAboveTerrain": 162, + "Altitude": 162, + "AltitudeMode": 1, + "autoContinue": true, + "command": 16, + "doJumpId": 16, + "frame": 3, + "params": [ + 0, + 80, + 0, + null, + 47.41490847409345, + 8.677297249971929, + 162 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 178, + "doJumpId": 17, + "frame": 2, + "params": [ + 1, + 20, + -1, + 0, + 0, + 0, + 0 + ], + "type": "SimpleItem" + }, + { + "AMSLAltAboveTerrain": 162, + "Altitude": 162, + "AltitudeMode": 1, + "autoContinue": true, + "command": 16, + "doJumpId": 18, + "frame": 3, + "params": [ + 0, + 75, + 0, + null, + 47.41804218634177, + 8.67217812890496, + 162 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 178, + "doJumpId": 19, + "frame": 2, + "params": [ + 1, + 20, + -1, + 0, + 0, + 0, + 0 + ], + "type": "SimpleItem" + }, + { + "AMSLAltAboveTerrain": 162, + "Altitude": 162, + "AltitudeMode": 1, + "autoContinue": true, + "command": 16, + "doJumpId": 20, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.41939741231282, + 8.658352683218709, + 162 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 178, + "doJumpId": 21, + "frame": 2, + "params": [ + 1, + 20, + -1, + 0, + 0, + 0, + 0 + ], + "type": "SimpleItem" + }, + { + "AMSLAltAboveTerrain": 162, + "Altitude": 162, + "AltitudeMode": 1, + "autoContinue": true, + "command": 16, + "doJumpId": 22, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.41918282563218, + 8.653337660166443, + 162 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 178, + "doJumpId": 23, + "frame": 2, + "params": [ + 1, + 20, + -1, + 0, + 0, + 0, + 0 + ], + "type": "SimpleItem" + }, + { + "AMSLAltAboveTerrain": 162, + "Altitude": 162, + "AltitudeMode": 1, + "autoContinue": true, + "command": 16, + "doJumpId": 24, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.41224393559117, + 8.660982227062817, + 162 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 178, + "doJumpId": 25, + "frame": 2, + "params": [ + 1, + 20, + -1, + 0, + 0, + 0, + 0 + ], + "type": "SimpleItem" + }, + { + "AMSLAltAboveTerrain": 162, + "Altitude": 162, + "AltitudeMode": 1, + "autoContinue": true, + "command": 16, + "doJumpId": 26, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.40947920992142, + 8.661151221615967, + 162 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 178, + "doJumpId": 27, + "frame": 2, + "params": [ + 1, + 20, + -1, + 0, + 0, + 0, + 0 + ], + "type": "SimpleItem" + }, + { + "AMSLAltAboveTerrain": 162, + "Altitude": 162, + "AltitudeMode": 1, + "autoContinue": true, + "command": 16, + "doJumpId": 28, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.406675648803215, + 8.661252567689417, + 162 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 178, + "doJumpId": 29, + "frame": 2, + "params": [ + 1, + 20, + -1, + 0, + 0, + 0, + 0 + ], + "type": "SimpleItem" + }, + { + "AMSLAltAboveTerrain": 110, + "Altitude": 110, + "AltitudeMode": 1, + "autoContinue": true, + "command": 16, + "doJumpId": 30, + "frame": 3, + "params": [ + 1, + 0, + 0, + null, + 47.4034519599179, + 8.661243753696738, + 110 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 178, + "doJumpId": 31, + "frame": 2, + "params": [ + 1, + 20, + -1, + 0, + 0, + 0, + 0 + ], + "type": "SimpleItem" + }, + { + "AMSLAltAboveTerrain": 100, + "Altitude": 100, + "AltitudeMode": 1, + "autoContinue": true, + "command": 16, + "doJumpId": 32, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.398541585470866, + 8.661261006343011, + 100 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 178, + "doJumpId": 33, + "frame": 2, + "params": [ + 1, + 20, + -1, + 0, + 0, + 0, + 0 + ], + "type": "SimpleItem" + }, + { + "AMSLAltAboveTerrain": 100, + "Altitude": 100, + "AltitudeMode": 1, + "autoContinue": true, + "command": 16, + "doJumpId": 34, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.39650241645701, + 8.66034975617157, + 100 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 178, + "doJumpId": 35, + "frame": 2, + "params": [ + 1, + 20, + -1, + 0, + 0, + 0, + 0 + ], + "type": "SimpleItem" + }, + { + "AMSLAltAboveTerrain": 60, + "Altitude": 60, + "AltitudeMode": 1, + "autoContinue": true, + "command": 16, + "doJumpId": 36, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.39684412703002, + 8.65473059687369, + 60 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 178, + "doJumpId": 37, + "frame": 2, + "params": [ + 1, + 20, + -1, + 0, + 0, + 0, + 0 + ], + "type": "SimpleItem" + }, + { + "AMSLAltAboveTerrain": 40, + "Altitude": 40, + "AltitudeMode": 1, + "autoContinue": true, + "command": 16, + "doJumpId": 38, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.39889101170981, + 8.64559487631118, + 40 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 178, + "doJumpId": 39, + "frame": 2, + "params": [ + 1, + 20, + -1, + 0, + 0, + 0, + 0 + ], + "type": "SimpleItem" + }, + { + "AMSLAltAboveTerrain": 35, + "Altitude": 35, + "AltitudeMode": 1, + "autoContinue": true, + "command": 16, + "doJumpId": 40, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.40279298790176, + 8.641026342847312, + 35 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 178, + "doJumpId": 41, + "frame": 2, + "params": [ + 1, + 20, + -1, + 0, + 0, + 0, + 0 + ], + "type": "SimpleItem" + }, + { + "AMSLAltAboveTerrain": 10, + "Altitude": 10, + "AltitudeMode": 1, + "autoContinue": true, + "command": 16, + "doJumpId": 42, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.40034874114077, + 8.636337653718812, + 10 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 178, + "doJumpId": 43, + "frame": 2, + "params": [ + 1, + 10, + -1, + 0, + 0, + 0, + 0 + ], + "type": "SimpleItem" + }, + { + "AMSLAltAboveTerrain": 10, + "Altitude": 10, + "AltitudeMode": 1, + "autoContinue": true, + "command": 16, + "doJumpId": 44, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.399686062158324, + 8.63501314113276, + 10 + ], + "type": "SimpleItem" + }, + { + "autoContinue": true, + "command": 178, + "doJumpId": 45, + "frame": 2, + "params": [ + 1, + 2, + -1, + 0, + 0, + 0, + 0 + ], + "type": "SimpleItem" + }, + { + "AMSLAltAboveTerrain": 10, + "Altitude": 10, + "AltitudeMode": 1, + "autoContinue": true, + "command": 21, + "doJumpId": 46, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.3995749303942, + 8.634793915809553, + 10 + ], + "type": "SimpleItem" + } + ], + "plannedHomePosition": [ + 47.39962126101363, + 8.634867914339253, + 437.98820282815694 + ], + "vehicleType": 2, + "version": 2 + }, + "rallyPoints": { + "points": [ + [ + 47.40945108448871, + 8.661601311114197, + 104 + ], + [ + 47.39581647104137, + 8.660439287833327, + 41 + ] + ], + "version": 2 + }, + "version": 1 +}