mutac: change terrain from flat plain to real heights

Author: rdasilva01

.vscode/c_cpp_properties.json

@@ -0,0 +1,44 @@
+{
+  "configurations": [
+    {
+      "browse": {
+        "databaseFilename": "${default}",
+        "limitSymbolsToIncludedHeaders": false
+      },
+      "includePath": [
+        "/home/rodri/catkin_ws/devel/include/**",
+        "/opt/ros/noetic/include/**",
+        "/home/rodri/catkin_ws/src/rpg_quadrotor_control/control/autopilot/include/**",
+        "/home/rodri/catkin_ws/src/multi_uav_test_area_coverage/execution_monitor/include/**",
+        "/home/rodri/catkin_ws/src/flightmare/flightlib/include/**",
+        "/home/rodri/catkin_ws/src/flightmare/flightros/include/**",
+        "/home/rodri/catkin_ws/src/rpg_quadrotor_control/utils/manual_flight_assistant/include/**",
+        "/home/rodri/catkin_ws/src/mav_comm/mav_msgs/include/**",
+        "/home/rodri/catkin_ws/src/mav_comm/mav_planning_msgs/include/**",
+        "/home/rodri/catkin_ws/src/multi_uav_test_area_coverage/multi_robot_simulator/include/**",
+        "/home/rodri/catkin_ws/src/rpg_quadrotor_control/trajectory_planning/polynomial_trajectories/include/**",
+        "/home/rodri/catkin_ws/src/rpg_quadrotor_control/control/position_controller/include/**",
+        "/home/rodri/catkin_ws/src/rpg_quadrotor_common/quadrotor_common/include/**",
+        "/home/rodri/catkin_ws/src/rotors_simulator/rotors_control/include/**",
+        "/home/rodri/catkin_ws/src/rotors_simulator/rotors_gazebo_plugins/include/**",
+        "/home/rodri/catkin_ws/src/rotors_simulator/rotors_hil_interface/include/**",
+        "/home/rodri/catkin_ws/src/rotors_simulator/rotors_joy_interface/include/**",
+        "/home/rodri/catkin_ws/src/rpg_quadrotor_control/test/rpg_quadrotor_integration_test/include/**",
+        "/home/rodri/catkin_ws/src/rpg_quadrotor_control/simulation/rpg_rotors_interface/include/**",
+        "/home/rodri/catkin_ws/src/rpg_single_board_io/include/**",
+        "/home/rodri/catkin_ws/src/rpg_quadrotor_control/bridges/sbus_bridge/include/**",
+        "/home/rodri/catkin_ws/src/rpg_quadrotor_common/state_predictor/include/**",
+        "/home/rodri/catkin_ws/src/rpg_quadrotor_control/trajectory_planning/trajectory_generation_helper/include/**",
+        "/usr/include/**",
+        "/home/rodri/catkin_ws/src/multi_uav_test_area_coverage/planner/src/multi_uav_planning_binpat/planning_binpat/include",
+        "/home/rodri/catkin_ws/src/multi_uav_test_area_coverage/performance_evaluation/performance_evaluator/include"
+      ],
+      "name": "ROS",
+      "intelliSenseMode": "gcc-x64",
+      "compilerPath": "/usr/bin/gcc",
+      "cStandard": "gnu11",
+      "cppStandard": "c++14"
+    }
+  ],
+  "version": 4
+}

.vscode/settings.json

@@ -0,0 +1,87 @@
+{
+    "python.autoComplete.extraPaths": [
+        "/home/rodri/catkin_ws/devel/lib/python3/dist-packages",
+        "/opt/ros/noetic/lib/python3/dist-packages"
+    ],
+    "files.associations": {
+        "cctype": "cpp",
+        "clocale": "cpp",
+        "cmath": "cpp",
+        "csignal": "cpp",
+        "cstdarg": "cpp",
+        "cstddef": "cpp",
+        "cstdio": "cpp",
+        "cstdlib": "cpp",
+        "cstring": "cpp",
+        "ctime": "cpp",
+        "cwchar": "cpp",
+        "cwctype": "cpp",
+        "any": "cpp",
+        "array": "cpp",
+        "atomic": "cpp",
+        "hash_map": "cpp",
+        "hash_set": "cpp",
+        "strstream": "cpp",
+        "*.tcc": "cpp",
+        "bitset": "cpp",
+        "chrono": "cpp",
+        "codecvt": "cpp",
+        "complex": "cpp",
+        "condition_variable": "cpp",
+        "cstdint": "cpp",
+        "deque": "cpp",
+        "forward_list": "cpp",
+        "list": "cpp",
+        "unordered_map": "cpp",
+        "unordered_set": "cpp",
+        "vector": "cpp",
+        "exception": "cpp",
+        "algorithm": "cpp",
+        "filesystem": "cpp",
+        "functional": "cpp",
+        "iterator": "cpp",
+        "map": "cpp",
+        "memory": "cpp",
+        "memory_resource": "cpp",
+        "numeric": "cpp",
+        "optional": "cpp",
+        "random": "cpp",
+        "ratio": "cpp",
+        "regex": "cpp",
+        "set": "cpp",
+        "string": "cpp",
+        "string_view": "cpp",
+        "system_error": "cpp",
+        "tuple": "cpp",
+        "type_traits": "cpp",
+        "utility": "cpp",
+        "fstream": "cpp",
+        "future": "cpp",
+        "initializer_list": "cpp",
+        "iomanip": "cpp",
+        "iosfwd": "cpp",
+        "iostream": "cpp",
+        "istream": "cpp",
+        "limits": "cpp",
+        "mutex": "cpp",
+        "new": "cpp",
+        "ostream": "cpp",
+        "shared_mutex": "cpp",
+        "sstream": "cpp",
+        "stdexcept": "cpp",
+        "streambuf": "cpp",
+        "thread": "cpp",
+        "cfenv": "cpp",
+        "cinttypes": "cpp",
+        "typeindex": "cpp",
+        "typeinfo": "cpp",
+        "valarray": "cpp",
+        "variant": "cpp",
+        "bit": "cpp",
+        "*.ipp": "cpp"
+    },
+    "python.analysis.extraPaths": [
+        "/home/rodri/catkin_ws/devel/lib/python3/dist-packages",
+        "/opt/ros/noetic/lib/python3/dist-packages"
+    ]
+}

execution_monitor/src/execution_monitor.cpp

@@ -55,7 +55,8 @@ ExecutionMonitor::ExecutionMonitor(int id) {
     pose_sub = nh.subscribe("drone_pose", 100, &LocalDrone::positionCallBack, dynamic_cast<MonitorData*>(drone.get()));
     battery_sub = nh.subscribe("battery_state", 100, &LocalDrone::batteryCallBack, drone.get());
 
-    trj_sub = nh.subscribe("/mutac/planned_paths", 100, &ExecutionMonitor::trajectoryCallBack, this);
+    //trj_sub = nh.subscribe("/mutac/planned_paths", 100, &ExecutionMonitor::trajectoryCallBack, this);
+    trj_sub = nh.subscribe("/mutac/real_planned_paths", 100, &ExecutionMonitor::trajectoryCallBack, this);
     alarm_sub = nh.subscribe("/mutac/drone_alarm", 100, &ExecutionMonitor::cameraCallBack, this);
 }
 

execution_monitor/src/global_monitor.cpp

@@ -53,7 +53,8 @@ GlobalMonitor::GlobalMonitor() {
         pose_subs.push_back(nh.subscribe("drone" + to_string(i+1) + "/drone_pose", 1, &GlobalDrone::positionCallBack, drone.get()));
     }
 
-    trj_sub = nh.subscribe("planned_paths", 100, &GlobalMonitor::trajectoryCallBack, this);
+    //trj_sub = nh.subscribe("planned_paths", 100, &GlobalMonitor::trajectoryCallBack, this);
+    trj_sub = nh.subscribe("real_planned_paths", 100, &GlobalMonitor::trajectoryCallBack, this);
     events_sub = nh.subscribe("drone_events", 100, &GlobalMonitor::eventCallBack, this);
     covered_sub = nh.subscribe("covered_points", 100, &GlobalMonitor::waypointCallBack, this);
 

multi_robot_simulator/include/simulator/flightmare_simulator.h

@@ -89,6 +89,9 @@ class Simulator {
 
     ros::Subscriber path_sub;
 
+    ros::Publisher real_path_pub;
+
+
     std::vector<ros::Publisher> pose_pubs;
     std::vector<ros::Publisher> speed_pubs;
 
@@ -101,6 +104,19 @@ class Simulator {
 
     double battery_per_min;
 
+    int terrain_altitude[11][28] = {
+        {30, 29, 29, 28, 27, 27, 25, 24, 23, 21, 21, 18, 18, 17, 17, 16, 16, 16, 16, 16, 16, 17, 17, 15, 14, 11, 11, 11},
+        {32, 31, 31, 31, 31, 30, 30, 29, 27, 27, 27, 26, 25, 26, 25, 25, 25, 25, 23, 23, 23, 23, 22, 21, 20, 19, 19, 18},
+        {30, 30, 29, 29, 30, 28, 29, 27, 29, 28, 27, 27, 26, 26, 26, 26, 25, 26, 24, 25, 24, 24, 23, 23, 22, 22, 21, 20},
+        {27, 27, 26, 26, 26, 26, 27, 25, 27, 26, 24, 24, 25, 24, 25, 25, 24, 24, 25, 25, 25, 25, 24, 24, 23, 22, 21, 20},
+        {26, 25, 24, 24, 23, 23, 24, 22, 23, 21, 21, 21, 21, 21, 22, 21, 21, 21, 23, 22, 23, 22, 23, 22, 22, 21, 21, 20},
+        {25, 24, 25, 24, 23, 24, 23, 23, 22, 22, 21, 20, 19, 18, 18, 17, 16, 16, 19, 16, 17, 16, 19, 16, 19, 17, 17, 18},
+        {27, 26, 25, 25, 24, 24, 24, 24, 23, 22, 22, 21, 20, 20, 19, 18, 17, 17, 16, 13, 14, 12, 13, 11, 11, 10, 12, 11},
+        {28, 27, 26, 26, 25, 25, 25, 24, 23, 23, 23, 22, 21, 21, 20, 19, 18, 16, 16, 14, 13, 12, 11,  9,  8,  7,  6,  3},
+        {27, 27, 26, 26, 25, 25, 24, 24, 23, 23, 23, 22, 21, 20, 19, 18, 17, 14, 15, 12, 10,  9,  8,  7,  6,  4,  2,  0},
+        {26, 26, 25, 25, 25, 24, 24, 23, 23, 23, 22, 22, 21, 21, 19, 18, 17, 16, 13, 13, 12, 11, 10,  8,  5,  6,  5,  2},
+        {24, 24, 23, 23, 23, 23, 23, 23, 23, 22, 22, 22, 21, 21, 20, 20, 18, 18, 16, 16, 14, 14, 12, 12,  9,  9,  7,  7}};
+
 private:
     void connectROS();
     void configDrones();

multi_robot_simulator/src/simulator/flightmare_simulator.cpp

@@ -181,22 +181,63 @@ void Simulator::connectROS() {
     path_sub = nh.subscribe("planned_paths", 100, &Simulator::pathCallback, this);
 
     // Publishers
+    real_path_pub = nh.advertise<mutac_msgs::Plan>("real_planned_paths", 16u);
     for (int i = 0; i < n_drones; i++) {
         pose_pubs.push_back(nh.advertise<geometry_msgs::Pose>("drone" + std::to_string(i+1) + "/drone_pose", 100));
         speed_pubs.push_back(nh.advertise<geometry_msgs::Twist>("drone" + std::to_string(i+1) + "/drone_twist", 100));
     }
 }
 
 void Simulator::pathCallback(const mutac_msgs::Plan &msg) {
+
+    //es el punto 3 el principio del camino (0, 1, 2 tienen otra altura, por lo que probablemente n, n-1 y n-2 tambien)
+
+    mutac_msgs::Plan temp = msg;
+
     for (size_t i = 0; i < msg.paths.size(); i++) {
-        int droneID = msg.paths[i].identifier.natural;
+        for (size_t j = 1; j < msg.paths[i].points.size() - 1; j++) {
+            double valor_altura = -11.1;
+            double real_x = temp.paths[i].points[j].point.x + 13.5;
+            double real_y = (-1) * temp.paths[i].points[j].point.y + 22.6;
+
+            double diff_x = (real_x*10)/244 - round((real_x*10)/244);
+
+            if(diff_x < 0) {
+                valor_altura += ( (1 - ((-1)*diff_x))*terrain_altitude[(int) round((real_x*10)/244)][(int) round((real_y*27)/126)] + ((-1)*diff_x)*terrain_altitude[(int) round((real_x*10)/244) - 1][(int) round((real_y*27)/126)]);
+            } else {
+                valor_altura += ( (1 - diff_x)*terrain_altitude[(int) round((real_x*10)/244)][(int) round((real_y*27)/126)] + diff_x*terrain_altitude[(int) round((real_x*10)/244) + 1][(int) round((real_y*27)/126)]);
+            }
+
+            if(valor_altura > 100)
+                std::cout << valor_altura << ", " << diff_x << " -   " << real_x << "  ,    " << terrain_altitude[(int) round((real_x*10)/244)][(int) round((real_y*27)/126)] << " + " << ((-1)*diff_x)*terrain_altitude[((int) round((real_x*10)/244)) - 1][(int) round((real_y*27)/126)] << " ... " << ((int) round((real_x*10)/244)) - 1 << std::endl;
+
+            temp.paths[i].points[j].point.z = temp.paths[i].points[j].point.z + valor_altura;
+        }
+    }
+
+
+    real_path_pub.publish(temp);
+
+
+    /*std::cout << temp.paths[0].points[2]
+              << temp.paths[0].points[3]
+              << temp.paths[1].points[2]
+              << temp.paths[1].points[3]
+              << temp.paths[2].points[2]
+              << temp.paths[2].points[3];*/
+
+    //for (size_t i = 0; i < msg.paths.size(); i++) {
+    for (size_t i = 0; i < temp.paths.size(); i++) {
+        //int droneID = msg.paths[i].identifier.natural;
+        int droneID = temp.paths[i].identifier.natural;
 
         if ((size_t)droneID > drones.size() - 1 || droneID < 0) {
             std::cout << "WARNING: Trajectory received for a drone that doesn't exists." << std::endl;
             return;
         }
 
-        std::vector<mutac_msgs::LabeledPoint> path = msg.paths[i].points;
+        //std::vector<mutac_msgs::LabeledPoint> path = msg.paths[i].points;
+        std::vector<mutac_msgs::LabeledPoint> path = temp.paths[i].points;
 
         if (path.size() < 2) {
             std::cout << "WARNING: Trajectoy needs to have at leat 2 waypoints." << std::endl;
@@ -212,7 +253,7 @@ void Simulator::pathCallback(const mutac_msgs::Plan &msg) {
         drones[droneID]->clearTrjPoints();
 
         // Add the points of the new plan
-        for (size_t i = 1; i < path.size(); i++) {
+        for (size_t i = 1; i < path.size(); i++) {            
             drones[droneID]->addPoint(std::vector<double>{path[i].point.x, path[i].point.y, path[i].point.z});
         }
 

performance_evaluation/performance_evaluator/src/metricsGenerator.cpp

@@ -56,7 +56,8 @@ MetricsGenerator::MetricsGenerator() {
 
     metrics_pub = nh.advertise<mutac_msgs::Metrics>("performance_metrics", 100);
     state_sub = nh.subscribe("drone_events", 100, &MetricsGenerator::stateCallBack, this);
-    trj_sub = nh.subscribe("planned_paths", 100, &MetricsGenerator::trajectoryCallBack, this);
+    //trj_sub = nh.subscribe("planned_paths", 100, &MetricsGenerator::trajectoryCallBack, this);real_planned_paths
+    trj_sub = nh.subscribe("real_planned_paths", 100, &MetricsGenerator::trajectoryCallBack, this);
 }
 
 // Sends messages with the global metrics and the drones metrics with a frequency of 10Hz

performance_evaluation/performance_viewer/config.yaml

@@ -3,27 +3,25 @@
 ##################################
 
 # Map image path
-img_path: ${MUTAC_WS}/performance_evaluation/performance_viewer/src/performance_viewer/resources/Map.png
+img_path: ${MUTAC_WS}/performance_evaluation/performance_viewer/src/performance_viewer/resources/solarfarm.png
 
 # Top left corner and bottom right corner coordinates from the map
 img_coordinates:
-  top: [900, 55]
-  bottom: [55, 445]
+  top: [-13.61, 22.61]
+  bottom: [230.75, -104.03]
 
 # Points that form the region to inspect
 regions:
   area1:
-    - [540, 345]
-    - [635, 120]
-    - [825, 120]
-    - [730, 345]
-    - [540, 345]
+    - [-5, 8]
+    - [58.85, 10]
+    - [80.17, -91.83]
+    - [22.86, -92.60]
+    - [-5, 8]
   area2:
-    - [560, 112]
-    - [520, 232]
-    - [480, 232]
-    - [410, 212]
-    - [140, 192]
-    - [240, 112]
-    - [560, 112]
-  
+    - [75, 5.5]
+    - [185, 9]
+    - [215, -35]
+    - [97.5, -60]
+    - [89.58, -60]
+    - [75, 5.5]

performance_evaluation/performance_viewer/config.yaml.old

@@ -0,0 +1,29 @@
+##################################
+# Performance viewer config file #
+##################################
+
+# Map image path
+img_path: ${MUTAC_WS}/performance_evaluation/performance_viewer/src/performance_viewer/resources/Map.png
+
+# Top left corner and bottom right corner coordinates from the map
+img_coordinates:
+  top: [900, 55]
+  bottom: [55, 445]
+
+# Points that form the region to inspect
+regions:
+  area1:
+    - [540, 345]
+    - [635, 120]
+    - [825, 120]
+    - [730, 345]
+    - [540, 345]
+  area2:
+    - [560, 112]
+    - [520, 232]
+    - [480, 232]
+    - [410, 212]
+    - [140, 192]
+    - [240, 112]
+    - [560, 112]
+  

performance_evaluation/performance_viewer/src/performance_viewer/Map.py

@@ -319,7 +319,7 @@ def show_inspected_regions(self, show):
 
     def subscribe(self):
         # Planned trajectories topic
-        rospy.Subscriber("planned_paths", Plan, self.trajectory_callback)
+        rospy.Subscriber("real_planned_paths", Plan, self.trajectory_callback)
 
         # Lost drones topic
         rospy.Subscriber("drone_events", State, self.state_callback)