Remove debugging logging, to upload to UAV

include/detect/uv_led_detect_adaptive.cpp

@@ -65,7 +65,7 @@ const std::vector<cv::Point>& standardPoints) {
 
 
     //Print size of tracking points
-    std::cout << "[UVDARLedDetectAdaptive]: TRACKING POINTS SIZE: " << trackingPoints.size() << std::endl;
+    //std::cout << "[UVDARLedDetectAdaptive]: TRACKING POINTS SIZE: " << trackingPoints.size() << std::endl;
 
 
     if (trackingPoints.size() == 0 || trackingPoints.size() > 50) {
@@ -76,14 +76,14 @@ const std::vector<cv::Point>& standardPoints) {
     // Process each tracking point
     for (const auto& point : trackingPoints) {
         // Apply adaptive thresholding to the ROI around the tracking point
-        std::cout << "[UVDARLedDetectAdaptive]: PROCESSING TRACKING POINT: " << point << std::endl;
+        //std::cout << "[UVDARLedDetectAdaptive]: PROCESSING TRACKING POINT: " << point << std::endl;
         std::vector<cv::Point> roiDetectedPoints = applyAdaptiveThreshold(inputImage, point, neighborhoodSize_);
         numRois++;
 
 
         // Check if the detected points are empty
         if (roiDetectedPoints.empty()){
-            std::cout << "[UVDARLedDetectAdaptive]: EMPTY ROI DETECTED POINTS" << std::endl;
+            //std::cout << "[UVDARLedDetectAdaptive]: EMPTY ROI DETECTED POINTS" << std::endl;
             continue;
         }
         // Check each point for uniqueness before adding
@@ -332,7 +332,7 @@ std::vector<cv::Point> UVDARLedDetectAdaptive::applyAdaptiveThreshold(const cv::
 
     // Detect points within this ROI
     std::vector<cv::Point> roiDetectedPoints = detectPointsFromRoi(binaryRoi, roi);
-    std::cout << "[UVDARLedDetectAdaptive]: ROI DETECTED POINTS: " << roiDetectedPoints.size() << std::endl;
+    //std::cout << "[UVDARLedDetectAdaptive]: ROI DETECTED POINTS: " << roiDetectedPoints.size() << std::endl;
 
     if (roiDetectedPoints.size() > 3){
         //std::cout << "[UVDARLedDetectAdaptive]: NOISY ROI: " << roiDetectedPoints.size() << std::endl;
@@ -647,8 +647,8 @@ std::tuple<int, double> UVDARLedDetectAdaptive::findOptimalThresholdUsingKL(cons
     }
 
     //Print the minKLDivergence and optimalThreshold
-    std::cout << "[UVDARLedDetectAdaptive]: MIN KL DIVERGENCE: " << minKLDivergence << std::endl;
-    std::cout << "[UVDARLedDetectAdaptive]: OPTIMAL THRESHOLD: " << optimalThreshold << std::endl;
+    //std::cout << "[UVDARLedDetectAdaptive]: MIN KL DIVERGENCE: " << minKLDivergence << std::endl;
+    //std::cout << "[UVDARLedDetectAdaptive]: OPTIMAL THRESHOLD: " << optimalThreshold << std::endl;
 
 
     return std::make_tuple(optimalThreshold, minKLDivergence);

launch/sim_three_sided_combined_marlon.launch

@@ -130,7 +130,7 @@
       <param name="justReport" type="bool" value="true"/>
       <param name="threshold" type="int" value="$(arg threshold)"/>
       <param name="adaptive_threshold" type="bool" value="true"/>
-      <param name="adaptive_method" type="string" value="Otsu"/>
+      <param name="adaptive_method" type="string" value="otsu"/>
 
       <rosparam param="camera_topics"> ["camera_left", "camera_right", "camera_back"] </rosparam>
       <rosparam param="points_seen_topics"> ["points_seen_left", "points_seen_right", "points_seen_back"] </rosparam>

scripts/extracting_evaluation_distance.py

@@ -1,20 +1,14 @@
 #!/usr/bin/env python
 import rosbag
 import csv
-from geometry_msgs.msg import Point
 import os
 
 
 #bag_path = os.path.expanduser('~/Desktop/MRS_Master_Project/rosbags/simulation/test_standard_topics_3.bag')
 
 # List of ROS bag files
 bag_files = [
-    os.path.expanduser('~/Desktop/MRS_Master_Project/rosbags/simulation/heading_experiments/static_static_topics_5m_0.4.bag'),
-    os.path.expanduser('~/Desktop/MRS_Master_Project/rosbags/simulation/heading_experiments/static_static_topics_8m_0.4.bag'),
-    os.path.expanduser('~/Desktop/MRS_Master_Project/rosbags/simulation/heading_experiments/static_static_topics_11m_0.4.bag'),
-    os.path.expanduser('~/Desktop/MRS_Master_Project/rosbags/simulation/heading_experiments/static_static_topics_14m_0.4.bag'),
-
-
+    os.path.expanduser('~/Desktop/MRS_Master_Project/rosbags/simulation/multi/otsu_multiple_topics.bag'),
 
     #os.path.expanduser('~/Desktop/MRS_Master_Project/rosbags/simulation/static_standard_topics_8m.bag'),
     #os.path.expanduser('~/Desktop/MRS_Master_Project/rosbags/simulation/static_standard_topics_11m.bag'),
@@ -27,13 +21,12 @@
 #output_csv = os.path.expanduser('~/Desktop/MRS_Master_Project/rosbags/simulation/standard_test_3.csv')
 
 # Output CSV file
-output_csv = os.path.expanduser('~/Desktop/MRS_Master_Project/rosbags/simulation/raw_csv/static_static_heading_experiment_v3.csv')
+output_csv = os.path.expanduser('~/Desktop/MRS_Master_Project/rosbags/simulation/raw_csv/otsu_multiple_topics.csv')
 
 # Define the topics to extract
 topics = ['/uav1/uvdar/blinkers_seen_left','/uav1/uvdar/blinkers_seen_right','/uav1/control_manager/control_reference']
 
 
-# Initialize an empty list to store combined data from all bag files
 combined_data = []
 
 # Process each bag file
@@ -43,7 +36,6 @@
         for topic, msg, t in bag.read_messages(topics=topics):
             entry = {'timestamp': t.to_sec(), 'distance_x': None, 'value': None}
             if topic in ['/uav1/uvdar/blinkers_seen_left', '/uav1/uvdar/blinkers_seen_right']:
-                # Assuming you're interested in the first point's value if multiple points exist
                 #entry['value'] = msg.points[0].value if msg.points else None
                 #Take all the values
                 entry['value'] = [point.value for point in msg.points]

scripts/position2.csv

@@ -1 +1 @@
-2,10.0,0.0,0.5,0.4
+2,10.0,0.0,0.5,0.7

scripts/position3.csv

@@ -1 +1 @@
-3,0.0,5.0,0.5,0.7
+3,7.0,1.0,0.5,0.7

scripts/position4.csv

@@ -1 +1 @@
-4,0.0,-5.0,0.5,0.0
+4,5.0,-1.0,0.5,0.7

scripts/two_drones/session_multiple.yml

@@ -0,0 +1,121 @@
+# do not modify these
+root: ./
+name: simulation
+socket_name: mrs
+attach: false
+tmux_options: -f /etc/ctu-mrs/tmux.conf
+# you can modify these
+pre_window: export UAV_NAME=uav1; export RUN_TYPE=simulation; export UAV_TYPE=x500
+startup_window: status
+windows:
+  - roscore:
+      layout: tiled
+      panes:
+        - roscore
+  - gazebo:
+      layout: tiled
+      panes:
+        - waitForRos; roslaunch mrs_uav_gazebo_simulation simulation.launch world_name:=grass_plane gui:=true
+  - gz_rate:
+      layout: tiled
+      panes:
+        - waitForHw; sleep 5; gz physics -u 2000
+  - status:
+      layout: tiled
+      panes:
+        - export UAV_NAME=uav1; waitForHw; roslaunch mrs_uav_status status.launch
+        - export UAV_NAME=uav2; waitForHw; roslaunch mrs_uav_status status.launch
+        - export UAV_NAME=uav3; waitForHw; roslaunch mrs_uav_status status.launch
+        - export UAV_NAME=uav4; waitForHw; roslaunch mrs_uav_status status.launch
+  - spawn:
+      layout: tiled
+      panes:
+        - waitForGazebo; rosservice call /mrs_drone_spawner/spawn  "1 --$UAV_TYPE --pos-file $(rospack find uvdar_core)/scripts/position1.csv --enable-rangefinder --enable-ground-truth --enable-dual-uv-cameras calib_file:=$(rospack find uvdar_core)/config/ocamcalib/calib_results_bf_uv_fe.txt --enable-back-uv-camera calib_file:=$(rospack find uvdar_core)/config/ocamcalib/calib_results_bf_uv_fe.txt"
+        - waitForGazebo; sleep 12; rosservice call /mrs_drone_spawner/spawn  "2 --$UAV_TYPE --pos-file $(rospack find uvdar_core)/scripts/position2.csv --enable-rangefinder --enable-ground-truth --enable-uv-leds signal_id:=[0,1,2,3]"
+        - waitForGazebo; sleep 17; rosservice call /mrs_drone_spawner/spawn  "3 --$UAV_TYPE --pos-file $(rospack find uvdar_core)/scripts/position3.csv --enable-rangefinder --enable-ground-truth --enable-uv-leds signal_id:=[4,5,6,7]"
+        - waitForGazebo; sleep 22; rosservice call /mrs_drone_spawner/spawn  "4 --$UAV_TYPE --pos-file $(rospack find uvdar_core)/scripts/position4.csv --enable-rangefinder --enable-ground-truth --enable-uv-leds signal_id:=[8,9,10,11]"
+  - hw_api:
+      layout: tiled
+      panes:
+        - export UAV_NAME=uav1; waitForTime; roslaunch mrs_uav_px4_api api.launch
+        - export UAV_NAME=uav2; waitForTime; roslaunch mrs_uav_px4_api api.launch
+        - export UAV_NAME=uav3; waitForTime; roslaunch mrs_uav_px4_api api.launch
+        - export UAV_NAME=uav4; waitForTime; roslaunch mrs_uav_px4_api api.launch
+  - core:
+      layout: tiled
+      panes:
+        - export UAV_NAME=uav1; waitForHw; roslaunch mrs_uav_core core.launch
+          platform_config:=`rospack find mrs_uav_gazebo_simulation`/config/mrs_uav_system/$UAV_TYPE.yaml
+          custom_config:=./config/custom_config.yaml
+          world_config:=./config/world_config.yaml                   
+          network_config:=./config/network_config.yaml
+        - export UAV_NAME=uav2; waitForHw; roslaunch mrs_uav_core core.launch
+          platform_config:=`rospack find mrs_uav_gazebo_simulation`/config/mrs_uav_system/$UAV_TYPE.yaml
+          custom_config:=./config/custom_config.yaml
+          world_config:=./config/world_config.yaml
+          network_config:=./config/network_config.yaml
+        - export UAV_NAME=uav3; waitForHw; roslaunch mrs_uav_core core.launch
+          platform_config:=`rospack find mrs_uav_gazebo_simulation`/config/mrs_uav_system/$UAV_TYPE.yaml
+          custom_config:=./config/custom_config.yaml
+          world_config:=./config/world_config.yaml
+          network_config:=./config/network_config.yaml
+        - export UAV_NAME=uav4; waitForHw; roslaunch mrs_uav_core core.launch
+          platform_config:=`rospack find mrs_uav_gazebo_simulation`/config/mrs_uav_system/$UAV_TYPE.yaml
+          custom_config:=./config/custom_config.yaml
+          world_config:=./config/world_config.yaml
+          network_config:=./config/network_config.yaml
+  - takeoff:
+      layout: tiled
+      panes:
+        - export UAV_NAME=uav1; waitForHw; roslaunch mrs_uav_autostart automatic_start.launch
+        - 'export UAV_NAME=uav1; waitForControl; rosservice call /$UAV_NAME/hw_api/arming 1; sleep 2; rosservice call /$UAV_NAME/hw_api/offboard'
+        - export UAV_NAME=uav2; waitForHw; roslaunch mrs_uav_autostart automatic_start.launch
+        - 'export UAV_NAME=uav2; waitForControl; rosservice call /$UAV_NAME/hw_api/arming 1; sleep 2; rosservice call /$UAV_NAME/hw_api/offboard'
+        - export UAV_NAME=uav3; waitForHw; roslaunch mrs_uav_autostart automatic_start.launch
+        - 'export UAV_NAME=uav3; waitForControl; rosservice call /$UAV_NAME/hw_api/arming 1; sleep 2; rosservice call /$UAV_NAME/hw_api/offboard'
+        - export UAV_NAME=uav4; waitForHw; roslaunch mrs_uav_autostart automatic_start.launch
+        - 'export UAV_NAME=uav4; waitForControl; rosservice call /$UAV_NAME/hw_api/arming 1; sleep 2; rosservice call /$UAV_NAME/hw_api/offboard'
+  - uv_observer:
+      layout: even-vertical
+      panes:
+        - waitForCompile; gz world --pause=0; sleep 6;  export UAV_NAME=uav1; waitForControl; roslaunch uvdar_core sim_three_sided_combined_marlon.launch use_4DHT:=false
+  - slow_down:
+      layout: even-vertical
+      panes:
+        - waitForGazebo; waitForRos; sleep 20; gz physics -u 80
+  - uav1_waypoint_flier:
+      layout: tiled
+      panes:
+        - export UAV_NAME=uav1; waitForControl; roslaunch example_waypoint_flier example_waypoint_flier.launch
+        - 'history -s rosservice call /$UAV_NAME/example_waypoint_flier/fly_to_first_waypoint'
+        - 'history -s rosservice call /$UAV_NAME/example_waypoint_flier/start_waypoints_following'
+        - 'history -s rosservice call /$UAV_NAME/example_waypoint_flier/stop_waypoints_following'
+  - uav2_waypoint_flier:
+      layout: tiled
+      panes:
+        - export UAV_NAME=uav2; waitForControl; roslaunch example_waypoint_flier example_waypoint_flier_2.launch
+        - 'export UAV_NAME=uav2; history -s rosservice call /$UAV_NAME/example_waypoint_flier/fly_to_first_waypoint'
+        - 'export UAV_NAME=uav2; history -s rosservice call /$UAV_NAME/example_waypoint_flier/start_waypoints_following'
+        - 'export UAV_NAME=uav2; history -s rosservice call /$UAV_NAME/example_waypoint_flier/stop_waypoints_following'
+
+ #- trajectories:
+     #layout: tiled
+   # panes:
+   #     - export UAV_NAME=uav1; history -s rosservice call /'"$UAV_NAME"'/control_manager/start_trajectory_tracking; history -s rosservice call /'"$UAV_NAME"'/control_manager/goto_trajectory_start; history -s roslaunch uvdar_core load_trajectory.launch file:="tx1/line.txt" loop:=true
+   #    - export UAV_NAME=uav1; waitForControl; sleep 15;  ~/catkin_ws/src/uvdar_core/scripts/trajectory_generation.py; history -s rosservice call /'"$UAV_NAME"'/control_manager/goto_trajectory_start  
+        # - export UAV_NAME=uav3; history -s rosservice call /'"$UAV_NAME"'/control_manager/start_trajectory_tracking; history -s rosservice call /'"$UAV_NAME"'/control_manager/goto_trajectory_start; history -s roslaunch uvdar_core load_trajectory.launch file:="two_tx/tx2_fly_by.txt" loop:=true
+  - rviz:
+      layout: even-vertical
+      panes:
+      - waitForGazebo; rviz -d $(rospack find uvdar_core)/rviz/two_drone_visualization.rviz
+  - layout:
+      layout: tiled
+      panes:
+        - waitForControl; sleep 3; ~/.i3/layout_manager.sh ./layout.json
+  - record:
+      layout: even-vertical
+      panes:
+        - 'history -s rosbag record -O ~/Desktop/MRS_Master_Project/rosbags/simulation/static_multiple_topics.bag /uav1/uvdar/adaptive_logging_back /uav1/uvdar/adaptive_logging_left /uav1/uvdar/adaptive_logging_right /uav1/uvdar/blinkers_seen_back /uav1/uvdar/blinkers_seen_left /uav1/uvdar/blinkers_seen_right /uav1/control_manager/control_reference --duration=30s'   
+        #- rosbag record -O rosbags/first/omta_38.bag -e "(.*)_camp"
+        # - rosbag record -O rosbags/first/omta_39.bag -e "(.*)_camp"
+

src/detector.cpp

@@ -460,10 +460,10 @@ class UVDARDetector : public nodelet::Nodelet{
       return;
     }
 
-    //Print the adaptive points
+   /*  //Print the adaptive points
     for (int i = 0; i < static_cast<int>(adaptive_detected_points_[image_index].size()); i++) {
       ROS_INFO_STREAM("[UVDARDetector]: Adaptive detected point " << i << ": " << adaptive_detected_points_[image_index][i]);
-    }
+    } */
 
   }
   //}
@@ -508,10 +508,10 @@ class UVDARDetector : public nodelet::Nodelet{
       ROS_INFO_STREAM("[UVDARDetector]: No detected points.");
     }
 
-    // Print the points
+  /*   // Print the points
     for (size_t i = 0; i < detected_points.size(); i++) {
       ROS_INFO_STREAM("[UVDARDetector]: Detected point " << i << ": " << detected_points[i]);
-    }
+    } */
 
     uvdar_core::ImagePointsWithFloatStamped msg_detected;
     msg_detected.stamp = image->header.stamp;
@@ -566,10 +566,10 @@ class UVDARDetector : public nodelet::Nodelet{
       ROS_INFO_STREAM("[UVDARDetector]: Tracking points per camera: " << trackingPointsPerCamera[image_index].size()); 
       received_tracking_points_ = true;
 
-      //Print the points
+      /* //Print the points
       for (int i = 0; i < static_cast<int>(trackingPointsPerCamera[image_index].size()); i++) {
         ROS_INFO_STREAM("[UVDARDetector]: Tracking point " << i << ": " << trackingPointsPerCamera[image_index][i]);
-      }
+      } */
 
       {
         std::scoped_lock lock(mutex_camera_image_);
@@ -579,9 +579,9 @@ class UVDARDetector : public nodelet::Nodelet{
         adaptive_detected_points_[image_index].clear();
 
         //TODO: Check if its worth it, this to be able to detect new points that where not currently detected
-        //processStandard(image, image_index);
+        processStandard(image, image_index);
 
-        ROS_INFO_STREAM("[UVDARDetector]: Processing image with tracking points only.");
+        //ROS_INFO_STREAM("[UVDARDetector]: Processing image with tracking points only.");
         processAdaptive(image, image_index, trackingPointsPerCamera[image_index]);
 
         cv::Mat visualization_image;