Merge branch 'master' of github.com:ctu-mrs/uvdar_core

ctu-mrs · Apr 17, 2024 · 8e0c825 · 8e0c825
2 parents 1abbc4e + 7ba7af9
commit 8e0c825
Show file tree

Hide file tree

Showing 10 changed files with 1,305 additions and 749 deletions.
diff --git a/.gitman.yml b/.gitman.yml
@@ -4,11 +4,25 @@ sources:
     name: ami
     rev: master
     type: git
-    params:
     sparse_paths:
       -
     links:
       - source: ''
         target: include/ami
     scripts:
       - git submodule update --init --recursive
+sources_locked:
+  - repo: https://github.com/TimLakemann/ami.git
+    name: ami
+    rev: 59402d1844ea8ae24ef0dc4a5d9fce5bb55e80a4
+    type: git
+    sparse_paths:
+      -
+    links:
+      - source: ''
+        target: include/ami
+    scripts:
+      - git submodule update --init --recursive
+default_group: ''
+groups:
+  -
diff --git a/install/install_on_real_UAV.sh b/install/install_on_real_UAV.sh
@@ -214,41 +214,8 @@ then
 fi 
 ###############################################################
 
-##################### LED Configuration #######################
-read -n 2 -p $'\e[1;32mDo you want to test the LEDs? [y/n]\e[0m\n' resp_led
-response_led=`echo $resp_led | sed -r 's/(.*)$/\1=/'`
-if [[ $response_led =~ ^(y|Y)=$ ]]; then 
-    source $workspace/devel/setup.bash
-
-    echo "####################### LED Configuration #######################"
-    echo $'\e[0;33mLED testing works only with a battery as the power source!\e[0m'
-    echo $'\e[1;32mWhich module is the UVDAR board connected to?\e[0m'
-    echo "Enter:"
-    echo "1 = /dev/MRS_MODULE1"
-    echo "2 = /dev/MRS_MODULE2"
-    echo "3 = /dev/MRS_MODULE3" 
-    echo "4 = /dev/MRS_MODULE4"
-    read -n 2 resp_module 
-    echo "Starting with LED initialization on:/dev/MRS_MODULE$resp_module... This will take about 20 seconds."
-    path_to_led_config=/opt/ros/noetic/share/uvdar_core/config/blinking_sequences/test_assignment.txt
-    roslaunch uvdar_core led_manager.launch sequence_file:=$path_to_led_config portname:=/dev/MRS_MODULE$resp_module &> $tmp_file_LED_launch & 
-    pid_led_manager=$! 
-    sleep 5; rosservice call /$UAV_NAME/uvdar_led_manager_node/quick_start 0
-    sleep 2; rosservice call /$UAV_NAME/uvdar_led_manager_node/load_sequences
-    sleep 2; rosservice call /$UAV_NAME/uvdar_led_manager_node/select_sequences [0,1,2,3]
-    sleep 2; rosservice call /$UAV_NAME/uvdar_led_manager_node/set_frequency 1
-    sleep 5
-
-    # kill the LED manager and remove temporary file
-    kill -9 "$pid_led_manager"
-    rm $tmp_file_LED_launch
-    echo "##################### LED Configuration done! ###################"
-    echo $'\e[1;32mPlease verify that the LEDs are correctly wired!\e[0m'
-    echo "Blinking Pattern: Clockwise blinking circle starting at the left front arm!"
-    echo $'\e[0;33mIf the blinking pattern didn\'t change: Please shutdown the NUC, detach the battery, attach it again and call this script again!\e[0m'
-else
-    echo "OK. Exiting script..."
-fi 
+$(rospack find uvdar_core)/install/test_leds.sh
+
 ###############################################################
 
 exit 0
diff --git a/install/test_leds.sh b/install/test_leds.sh
@@ -0,0 +1,37 @@
+#!/bin/bash
+tmp_file_LED_launch="/tmp/led_manager.txt"
+##################### LED Configuration #######################
+read -n 2 -p $'\e[1;32mDo you want to test the LEDs? [y/n]\e[0m\n' resp_led
+response_led=`echo $resp_led | sed -r 's/(.*)$/\1=/'`
+if [[ $response_led =~ ^(y|Y)=$ ]]; then 
+    source $workspace/devel/setup.bash
+
+    echo "####################### LED Configuration #######################"
+    echo $'\e[0;33mLED testing works only with a battery as the power source!\e[0m'
+    echo $'\e[1;32mWhich module is the UVDAR board connected to?\e[0m'
+    echo "Enter:"
+    echo "1 = /dev/MRS_MODULE1"
+    echo "2 = /dev/MRS_MODULE2"
+    echo "3 = /dev/MRS_MODULE3" 
+    echo "4 = /dev/MRS_MODULE4"
+    read -n 2 resp_module 
+    echo "Starting with LED initialization on:/dev/MRS_MODULE$resp_module... This will take about 20 seconds."
+    path_to_led_config=/opt/ros/noetic/share/uvdar_core/config/blinking_sequences/test_assignment.txt
+    roslaunch uvdar_core led_manager.launch sequence_file:=$path_to_led_config portname:=/dev/MRS_MODULE$resp_module &> $tmp_file_LED_launch & 
+    pid_led_manager=$! 
+    sleep 5; rosservice call /$UAV_NAME/uvdar_led_manager_node/quick_start 0
+    sleep 2; rosservice call /$UAV_NAME/uvdar_led_manager_node/load_sequences
+    sleep 2; rosservice call /$UAV_NAME/uvdar_led_manager_node/select_sequences [0,1,2,3]
+    sleep 2; rosservice call /$UAV_NAME/uvdar_led_manager_node/set_frequency 1
+    sleep 5
+
+    # kill the LED manager and remove temporary file
+    kill -9 "$pid_led_manager"
+    rm $tmp_file_LED_launch
+    echo "##################### LED Configuration done! ###################"
+    echo $'\e[1;32mPlease verify that the LEDs are correctly wired!\e[0m'
+    echo "Blinking Pattern: Clockwise blinking circle starting at the left front arm!"
+    echo $'\e[0;33mIf the blinking pattern didn\'t change: Please shutdown the NUC, detach the battery, attach it again and call this script again!\e[0m'
+else
+    echo "OK. Exiting script..."
+fi 
diff --git a/launch/sim_three_sided.launch b/launch/sim_three_sided.launch
@@ -26,8 +26,8 @@
   <arg name="view" default="false"/>
   <arg name="calib" default="false"/>
 
-  <!-- <arg name="profiling" default="false"/> -->
-  <arg name="profiling" default="true"/>
+  <arg name="profiling" default="false"/>
+  <!-- <arg name="profiling" default="true"/> -->
   <arg name="debug" default="false"/>
   <arg name="visual_debug" default="false"/>
   <arg name="gui" default="true"/>

diff --git a/rviz/two_drone_visualization.rviz b/rviz/two_drone_visualization.rviz
@@ -6,9 +6,12 @@ Panels:
       Expanded:
         - /Global Options1
         - /Status1
+        - /Grid1
         - /Hypotheses1/Covariance1
+        - /Filtered1/Covariance1
+        - /Filtered1/Covariance1/Position1
       Splitter Ratio: 0.5
-    Tree Height: 701
+    Tree Height: 363
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -49,7 +52,7 @@ Visualization Manager:
         Y: 0
         Z: 0
       Plane: XY
-      Plane Cell Count: 10
+      Plane Cell Count: 20
       Reference Frame: <Fixed Frame>
       Value: true
     - Alpha: 1
@@ -79,15 +82,15 @@ Visualization Manager:
       Queue Size: 10
       Shaft Length: 1
       Shaft Radius: 0.05000000074505806
-      Shape: Arrow
+      Shape: Axes
       Topic: /uav1/uvdar/measuredPoses
       Unreliable: false
       Value: true
     - Alpha: 1
       Axes Length: 1
       Axes Radius: 0.10000000149011612
       Class: mrs_rviz_plugins/PoseWithCovarianceArray
-      Color: 255; 25; 0
+      Color: 255; 255; 255
       Covariance:
         Orientation:
           Alpha: 0.5
@@ -114,6 +117,37 @@ Visualization Manager:
       Topic: /uav1/uvdar/constituentHypotheses
       Unreliable: false
       Value: true
+    - Alpha: 0.5
+      Axes Length: 1
+      Axes Radius: 0.10000000149011612
+      Class: mrs_rviz_plugins/PoseWithCovarianceArray
+      Color: 255; 0; 0
+      Covariance:
+        Orientation:
+          Alpha: 0.5
+          Color: 255; 255; 127
+          Color Style: Unique
+          Frame: Local
+          Offset: 1
+          Scale: 1
+          Value: false
+        Position:
+          Alpha: 0.30000001192092896
+          Color: 204; 51; 204
+          Scale: 1
+          Value: false
+        Value: false
+      Enabled: true
+      Head Length: 0.029999999329447746
+      Head Radius: 0.10000000149011612
+      Name: HypothesesTentative
+      Queue Size: 10
+      Shaft Length: 0.30000001192092896
+      Shaft Radius: 0.029999999329447746
+      Shape: Arrow
+      Topic: /uav1/uvdar/constituentHypothesesTentative
+      Unreliable: false
+      Value: true
     - Alpha: 1
       Class: rviz/Axes
       Enabled: true
@@ -123,6 +157,37 @@ Visualization Manager:
       Reference Frame: uav1/fcu
       Show Trail: false
       Value: true
+    - Alpha: 1
+      Axes Length: 1
+      Axes Radius: 0.10000000149011612
+      Class: mrs_rviz_plugins/PoseWithCovarianceArray
+      Color: 255; 25; 0
+      Covariance:
+        Orientation:
+          Alpha: 0.5
+          Color: 255; 255; 127
+          Color Style: Unique
+          Frame: Local
+          Offset: 1
+          Scale: 1
+          Value: true
+        Position:
+          Alpha: 0.30000001192092896
+          Color: 85; 255; 0
+          Scale: 1
+          Value: true
+        Value: true
+      Enabled: true
+      Head Length: 0.30000001192092896
+      Head Radius: 0.10000000149011612
+      Name: Filtered
+      Queue Size: 10
+      Shaft Length: 1
+      Shaft Radius: 0.05000000074505806
+      Shape: Arrow
+      Topic: /uav1/uvdar/filteredPoses
+      Unreliable: false
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -150,42 +215,36 @@ Visualization Manager:
   Value: true
   Views:
     Current:
-      Class: rviz/Orbit
-      Distance: 13.165216445922852
+      Angle: -1.5709999799728394
+      Class: rviz/TopDownOrtho
       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: 0.23903301358222961
-        Y: 1.1742606163024902
-        Z: 0.38564643263816833
-      Focal Shape Fixed Size: true
-      Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.8952029347419739
+      Scale: 52.86750030517578
       Target Frame: <Fixed Frame>
-      Yaw: 3.4203882217407227
+      X: -0.4788692593574524
+      Y: 0.25147315859794617
     Saved: ~
 Window Geometry:
   Displays:
-    collapsed: false
-  Height: 999
-  Hide Left Dock: false
-  Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd00000004000000000000015600000348fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003e00000348000000cb00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000348fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003e00000348000000a500fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005000000003efc0100000002fb0000000800540069006d00650100000000000005000000033400fffffffb0000000800540069006d006501000000000000045000000000000000000000028f0000034800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+    collapsed: true
+  Height: 1401
+  Hide Left Dock: true
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd000000040000000000000156000003c7fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003e000003c7000000cb00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c000002610000000100000118000004dffc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003e000004df000000a500fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004f500000039fc0100000002fb0000000800540069006d00650100000000000004f50000033400fffffffb0000000800540069006d00650100000000000004500000000000000000000004f5000004df00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
     collapsed: false
   Tool Properties:
     collapsed: false
   Views:
-    collapsed: false
-  Width: 1280
-  X: 4480
-  Y: 0
+    collapsed: true
+  Width: 1269
+  X: 3202
+  Y: 5
diff --git a/scripts/two_drones/config/custom_config.yaml b/scripts/two_drones/config/custom_config.yaml
@@ -24,3 +24,10 @@ mrs_uav_managers:
         controller: "MpcController"
         # controller: "Se3Controller"
         tracker: "MpcTracker"
+
+mrs_uav_trackers:
+  mpc_tracker:
+    collision_avoidance:
+      radius: 2.0 # [m]
+      enabled: true
+
diff --git a/scripts/two_drones/session.yml b/scripts/two_drones/session.yml
@@ -6,7 +6,7 @@ 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
+startup_window: uv_observer
 windows:
   - roscore:
       layout: tiled

diff --git a/scripts/utilities/record_calibration.yml b/scripts/utilities/record_calibration.yml
@@ -10,20 +10,20 @@ windows:
   - roscore:
       layout: even-vertical
       panes:
-        - sleep 4; roscore
+        - sleep 5; roscore
   - directories:
       layout: even-vertical
       panes: 
-        - sleep 2; devnum=`cat devnum.txt`; mkdir ~/bag_files; mkdir ~/bag_files/uvdar_calib/; mkdir ~/bag_files/uvdar_calib/${devnum}
+        - sleep 6; devnum=`cat devnum.txt`; mkdir ~/bag_files; mkdir ~/bag_files/uvdar_calib/; mkdir ~/bag_files/uvdar_calib/${devnum}
   - bluefox:
       layout: even-vertical
       panes:
-        - waitForRos; devnum=`cat devnum.txt`; roslaunch bluefox2 single_nodelet.launch aec:=false agc:=false expose_us:=50 camera:=cam device:=${devnum} 
+        - sleep 6; devnum=`cat devnum.txt`; roslaunch bluefox2 single_nodelet.launch aec:=false agc:=false expose_us:=500 camera:=cam device:=${devnum} 
   - recording:
       layout: even-vertical
       panes:
-        - waitForRos; sleep 1; devnum=`cat devnum.txt`; rosbag record -a -x '(.*)compressed(.*)' -x '(.*)theora(.*)' -O ~/bag_files/uvdar_calib/${devnum}/C_${devnum}.bag
+        - sleep 6; sleep 1; devnum=`cat devnum.txt`; rosbag record -a -x '(.*)compressed(.*)' -x '(.*)theora(.*)' -O ~/bag_files/uvdar_calib/${devnum}/C_${devnum}.bag
   - termviz: 
       layout: even-vertical
       panes:
-        - waitForRos; sleep 2; termviz record_calibration_termviz.yml 
+        - sleep 6; sleep 2; termviz record_calibration_termviz.yml 
diff --git a/src/led_manager.cpp b/src/led_manager.cpp
@@ -183,6 +183,7 @@ namespace uvdar {
 
 
         unsigned char i = 0;
+        ros::Duration local_sleeper(0.25 + 0.1*(sequences_[0].size()));
         for (auto sq : sequences_){
           serial_msg.payload.clear();
           serial_msg.payload.push_back(0x99); //write sequences
@@ -196,7 +197,8 @@ namespace uvdar {
 
           /* if (i == 3) */
           baca_protocol_publisher.publish(serial_msg);
-          sleeper.sleep();
+
+          local_sleeper.sleep();
           i++;
         }
 
@@ -406,8 +408,19 @@ namespace uvdar {
         /* uvdar_core::SetIntIndex::Request ind_req; */
         /* uvdar_core::SetIntIndex::Response ind_res; */
         /* ind_req.value = req.value; */
-        callbackSelectSingleSequence(req,res);
+        /* callbackSelectSingleSequence(req,res); */
+        uvdar_core::SetInts::Request req_seqences;
+        uvdar_core::SetInts::Response res_seqences;
+        unsigned char val = (unsigned char)(req.value);
+        req_seqences.value.push_back(4*val+0);
+        req_seqences.value.push_back(4*val+1);
+        req_seqences.value.push_back(4*val+2);
+        req_seqences.value.push_back(4*val+3);
+        callbackSelectSequences(req_seqences, res_seqences);
 
+        res.success = true;
+        char message[100];
+        sprintf(message, "Quickstart done. Sequences set to [%d,%d,%d,%d].", 4*val+0, 4*val+1, 4*val+2, 4*val+3); 
         return true;
       }