diff --git a/terrain_planner_benchmark/CMakeLists.txt b/terrain_planner_benchmark/CMakeLists.txt index d0f13b41..f1ad03f0 100644 --- a/terrain_planner_benchmark/CMakeLists.txt +++ b/terrain_planner_benchmark/CMakeLists.txt @@ -9,9 +9,7 @@ find_package(ompl REQUIRED) find_package(Boost REQUIRED COMPONENTS serialization system filesystem) include(cmake/CPM.cmake) -CPMAddPackage("gh:nlohmann/json@3.10.5") - -# find_package(nlohmann_json 3.10.5 REQUIRED) +CPMAddPackage("gh:nlohmann/json@3.11.3") find_package(catkin REQUIRED COMPONENTS eigen_catkin diff --git a/terrain_planner_benchmark/Tools/duebendorf_color.tif b/terrain_planner_benchmark/Tools/duebendorf_color.tif new file mode 100644 index 00000000..2d36e415 Binary files /dev/null and b/terrain_planner_benchmark/Tools/duebendorf_color.tif differ diff --git a/terrain_planner_benchmark/launch/rallypoints.rviz b/terrain_planner_benchmark/launch/rallypoints.rviz index 8b761851..14fe1e94 100644 --- a/terrain_planner_benchmark/launch/rallypoints.rviz +++ b/terrain_planner_benchmark/launch/rallypoints.rviz @@ -15,8 +15,9 @@ Panels: - /MarkerArray1 - /MarkerArray2 - /MarkerArray3 + - /MarkerArray4 Splitter Ratio: 0.5 - Tree Height: 839 + Tree Height: 845 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -65,7 +66,7 @@ Visualization Manager: Marker Topic: /start_position Name: Marker Namespaces: - "": true + {} Queue Size: 100 Value: true - Class: rviz/Marker @@ -73,7 +74,7 @@ Visualization Manager: Marker Topic: /goal_position Name: Marker Namespaces: - "": true + {} Queue Size: 100 Value: true - Alpha: 1 @@ -186,7 +187,7 @@ Visualization Manager: Marker Topic: /path_segments Name: MarkerArray Namespaces: - normals: true + {} Queue Size: 100 Value: true - Class: rviz/MarkerArray @@ -194,7 +195,7 @@ Visualization Manager: Marker Topic: /rallypoints_marker Name: MarkerArray Namespaces: - rallypoints: true + {} Queue Size: 100 Value: true - Class: rviz/MarkerArray @@ -202,7 +203,15 @@ Visualization Manager: Marker Topic: /abort_path_segments Name: MarkerArray Namespaces: - normals: true + {} + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /geofence_marker + Name: MarkerArray + Namespaces: + geofence: true Queue Size: 100 Value: true Enabled: true @@ -233,7 +242,7 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 3155.947265625 + Distance: 4618.54638671875 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -241,17 +250,17 @@ Visualization Manager: Value: false Field of View: 0.7853981852531433 Focal Point: - X: -509.687255859375 - Y: -545.7493896484375 - Z: 1358.635009765625 + X: 79.93209075927734 + Y: -697.7658081054688 + Z: 860.8773193359375 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.6647965312004089 + Pitch: 0.5997965931892395 Target Frame: - Yaw: 0.7581591010093689 + Yaw: 5.066342830657959 Saved: ~ Window Geometry: Displays: @@ -259,7 +268,7 @@ Window Geometry: Height: 1136 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000001a1000003d2fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000003d2000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000020c000001b10000000000000000000000010000010f000003d2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000003d2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d0065010000000000000738000003bc00fffffffb0000000800540069006d0065010000000000000450000000000000000000000591000003d200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000001a1000003d6fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000003d6000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000020c000001b10000000000000000000000010000010f000003d2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000003d2000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d00650100000000000007380000030700fffffffb0000000800540069006d0065010000000000000450000000000000000000000591000003d600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: diff --git a/terrain_planner_benchmark/launch/run_dynamic_rallypoints.launch b/terrain_planner_benchmark/launch/run_dynamic_rallypoints.launch index 79861b65..7693feaa 100644 --- a/terrain_planner_benchmark/launch/run_dynamic_rallypoints.launch +++ b/terrain_planner_benchmark/launch/run_dynamic_rallypoints.launch @@ -1,15 +1,15 @@ - + - - - + + + diff --git a/terrain_planner_benchmark/resources/duebendorf.tif b/terrain_planner_benchmark/resources/duebendorf.tif new file mode 100644 index 00000000..2d36e415 Binary files /dev/null and b/terrain_planner_benchmark/resources/duebendorf.tif differ diff --git a/terrain_planner_benchmark/resources/duebendorf_color.tif b/terrain_planner_benchmark/resources/duebendorf_color.tif new file mode 100644 index 00000000..5687f8ca Binary files /dev/null and b/terrain_planner_benchmark/resources/duebendorf_color.tif differ diff --git a/terrain_planner_benchmark/src/run_dynamic_rallypoints.cpp b/terrain_planner_benchmark/src/run_dynamic_rallypoints.cpp index 0483d902..a49f4444 100644 --- a/terrain_planner_benchmark/src/run_dynamic_rallypoints.cpp +++ b/terrain_planner_benchmark/src/run_dynamic_rallypoints.cpp @@ -51,6 +51,7 @@ #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" @@ -58,7 +59,8 @@ #include using json = nlohmann::json; -void publishPathSegments(ros::Publisher& pub, Path& trajectory, Eigen::Vector3d color = Eigen::Vector3d(0.0, 1.0, 0.0)) { +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; @@ -80,8 +82,8 @@ void publishPathSegments(ros::Publisher& pub, Path& trajectory, Eigen::Vector3d pub.publish(msg); } -visualization_msgs::Marker getGoalMarker(const int id, const Eigen::Vector3d &position, - const double radius, const Eigen::Vector3d color) { +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(); @@ -113,13 +115,13 @@ visualization_msgs::Marker getGoalMarker(const int id, const Eigen::Vector3d &po 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") { +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) { + for (const auto& position : positions) { visualization_msgs::Marker marker; marker = getGoalMarker(marker_id, position, radius, color); marker.ns = name_space; @@ -130,7 +132,6 @@ void publishRallyPoints(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 publishGridMap(const ros::Publisher& pub, const grid_map::GridMap& map) { + grid_map_msgs::GridMap message; + grid_map::GridMapRosConverter::toMessage(map, message); + pub.publish(message); +} + void getDubinsShortestPath(std::shared_ptr& dubins_ss, const Eigen::Vector3d start_pos, const double start_yaw, const Eigen::Vector3d goal_pos, const double goal_yaw, std::vector& path) { @@ -290,6 +336,7 @@ int main(int argc, char** argv) { auto path_segment_pub = nh.advertise("path_segments", 1, true); auto abort_path_pub = nh.advertise("abort_path_segments", 1, true); auto rallypoint_pub = nh.advertise("rallypoints_marker", 1); + auto geofence_pub = nh.advertise("geofence_marker", 1); std::string map_path, color_file_path, output_directory, location, mission_file_path; nh_private.param("map_path", map_path, ""); @@ -302,16 +349,6 @@ int main(int argc, char** argv) { auto data_logger = std::make_shared(); data_logger->setKeys({"x", "y", "z"}); - ///TODO: Parse json file to get geofence information - ///TODO: Generate path from waypoint list - std::ifstream f(mission_file_path); - json data = json::parse(f); - json polygons = data["geoFence"]["polygons"]; - // json polygon = polygons["inclusion"]; - for (json::iterator it = polygons.begin(); it != polygons.end(); ++it) { - std::cout << *it << '\n'; - } - // Load terrain map from defined tif paths auto terrain_map = std::make_shared(); terrain_map->initializeFromGeotiff(map_path); @@ -324,6 +361,38 @@ int main(int argc, char** argv) { terrain_map->AddLayerHorizontalDistanceTransform(radius, "ics_+", "distance_surface"); terrain_map->AddLayerHorizontalDistanceTransform(-radius, "ics_-", "max_elevation"); + // Parse json 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; + for (auto polygon_vertex : polygons[0]["polygon"]) { + Eigen::Vector2d vertex_wgs84 = Eigen::Vector2d(polygon_vertex[0], polygon_vertex[1]); + std::cout << " - Vertex WGS84: " << vertex_wgs84.transpose() << std::endl; + // 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)); + } + + /// TODO: Visualize Geofence + for (auto vertex : geofence_polygon) { + std::cout << "Parsed vertex: " << vertex.transpose() << std::endl; + } + publishGeoFence(geofence_pub, geofence_polygon); + terrain_map->getGridMap().setTimestamp(ros::Time::now().toNSec()); + publishGridMap(grid_map_pub, terrain_map->getGridMap()); + + /// TODO: Generate path from waypoint list + // Initialize planner with loaded terrain map auto planner = std::make_shared(); planner->setMap(terrain_map); @@ -351,7 +420,6 @@ int main(int argc, char** argv) { throw std::runtime_error("Specified goal position is NOT valid"); } - Path path; planner->setupProblem(start, goal); if (planner->Solve(20.0, path)) { @@ -372,7 +440,6 @@ int main(int argc, char** argv) { path.appendSegment(goal_loiter_path); - /// Generate candidate rally points std::cout << "Generating rally points" << std::endl; const int num_rally_points = 6; @@ -381,12 +448,13 @@ int main(int argc, char** argv) { bool sample_is_valid = false; while (!sample_is_valid) { Eigen::Vector3d random_sample; - random_sample(0) = getRandom(map_pos(0) - (0.4 * map_width_x - radius), map_pos(0) + (0.4 * map_width_x - radius)); - random_sample(1) = getRandom(map_pos(1) - (0.4 * map_width_y - radius), map_pos(1) + (0.4 * map_width_y - radius)); + random_sample(0) = + getRandom(map_pos(0) - (0.4 * map_width_x - radius), map_pos(0) + (0.4 * map_width_x - radius)); + random_sample(1) = + getRandom(map_pos(1) - (0.4 * map_width_y - radius), map_pos(1) + (0.4 * map_width_y - radius)); Eigen::Vector3d candidate_loiter_position = random_sample; Eigen::Vector3d new_loiter_position; - sample_is_valid = - validatePosition(terrain_map->getGridMap(), candidate_loiter_position, new_loiter_position); + sample_is_valid = validatePosition(terrain_map->getGridMap(), candidate_loiter_position, new_loiter_position); if (sample_is_valid) { rally_points.push_back(new_loiter_position); } @@ -396,10 +464,10 @@ int main(int argc, char** argv) { std::cout << "Successfully Generated rally points" << std::endl; - ///TODO: Iterate over path segments to find abort paths + /// TODO: Iterate over path segments to find abort paths int segment_id = 0; Path abort_path_list; - for (const auto &path_segment : path.segments) { + for (const auto& path_segment : path.segments) { std::cout << " - segment ID: " << segment_id << std::endl; Eigen::Vector3d segment_end = path_segment.states.back().position; Eigen::Vector3d segment_end_tangent = path_segment.states.back().velocity; @@ -416,9 +484,7 @@ int main(int argc, char** argv) { // Repeatedly publish results terrain_map->getGridMap().setTimestamp(ros::Time::now().toNSec()); - grid_map_msgs::GridMap message; - grid_map::GridMapRosConverter::toMessage(terrain_map->getGridMap(), message); - grid_map_pub.publish(message); + publishGridMap(grid_map_pub, terrain_map->getGridMap()); publishPathSegments(path_segment_pub, path, Eigen::Vector3d(0.0, 1.0, 0.0)); publishPathSegments(abort_path_pub, abort_path_list, Eigen::Vector3d(1.0, 0.0, 0.0));