From bd0d97022128d4737ba1a38c6c2d60a751a32587 Mon Sep 17 00:00:00 2001 From: viktor Date: Tue, 16 Apr 2024 11:08:38 +0200 Subject: [PATCH 1/6] Adjusting wait times in led config, and separating the led config test script --- install/install_on_real_UAV.sh | 37 ++-------------------------------- install/test_leds.sh | 37 ++++++++++++++++++++++++++++++++++ src/led_manager.cpp | 4 +++- 3 files changed, 42 insertions(+), 36 deletions(-) create mode 100755 install/test_leds.sh diff --git a/install/install_on_real_UAV.sh b/install/install_on_real_UAV.sh index 159b6c2..0b6bf87 100755 --- 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 new file mode 100755 index 0000000..c02ebc1 --- /dev/null +++ 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/src/led_manager.cpp b/src/led_manager.cpp index 44a4281..93f8d79 100644 --- 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++; } From e4dbc6b3840d7102cb294d7ef56f81b4b1355a58 Mon Sep 17 00:00:00 2001 From: viktor Date: Tue, 16 Apr 2024 11:46:15 +0200 Subject: [PATCH 2/6] Made the quick start command select different signal --- src/led_manager.cpp | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/src/led_manager.cpp b/src/led_manager.cpp index 93f8d79..a9c4cde 100644 --- a/src/led_manager.cpp +++ b/src/led_manager.cpp @@ -408,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; } From 7ba7af903809caca19a5c6a34735d52b3415e521 Mon Sep 17 00:00:00 2001 From: viktor Date: Tue, 16 Apr 2024 13:34:01 +0200 Subject: [PATCH 3/6] Accelerated initialization for erroneous observations --- src/pose_calculator.cpp | 25 +++++++++++++++++-------- 1 file changed, 17 insertions(+), 8 deletions(-) diff --git a/src/pose_calculator.cpp b/src/pose_calculator.cpp index e12672e..8381452 100644 --- a/src/pose_calculator.cpp +++ b/src/pose_calculator.cpp @@ -76,7 +76,7 @@ #define MAX_HYPOTHESIS_AGE 1.5 #define MAX_INIT_ITERATIONS 10000 -#define MAX_MUTATION_REFINE_ITERATIONS 10000 +#define MAX_MUTATION_REFINE_ITERATIONS 1000 #define SIMILAR_ERRORS_THRESHOLD sqr(1) @@ -2432,13 +2432,22 @@ namespace uvdar { auto [hypotheses_init, errors_init] = getViableInitialHyptheses(points, furthest_position, target, image_index, fromcam_tf, tocam_tf, INITIAL_ROUGH_HYPOTHESIS_COUNT, time); profiler.addValue("InitialHypotheses"); - - auto hypotheses_refined = refineByMutation(model_, points, hypotheses_init, tocam_tf, image_index, target, ERROR_THRESHOLD_MUTATION_1(image_index), 1.0, 1.0, INITIAL_HYPOTHESIS_COUNT); - profiler.addValue("Initial Mutation 1"); - hypotheses_refined = refineByMutation(model_, points, hypotheses_refined, tocam_tf, image_index, target, ERROR_THRESHOLD_MUTATION_2(image_index), 1.0, 1.0, INITIAL_HYPOTHESIS_COUNT); - profiler.addValue("Initial Mutation 2"); - hypotheses_refined = refineByMutation(model_, points, hypotheses_refined, tocam_tf, image_index, target, ERROR_THRESHOLD_MUTATION_3(image_index), 1.0, 1.0, INITIAL_HYPOTHESIS_COUNT); - profiler.addValue("Initial Mutation 3"); + double ratio_found = (double)(hypotheses_init.size()) / (double)(INITIAL_ROUGH_HYPOTHESIS_COUNT); + ROS_INFO(" Ratio Init: %f", 100*ratio_found ); + + int desired_count = (int)(ratio_found*INITIAL_HYPOTHESIS_COUNT); + auto hypotheses_refined = refineByMutation(model_, points, hypotheses_init, tocam_tf, image_index, target, ERROR_THRESHOLD_MUTATION_1(image_index), 1.0, 1.0, desired_count); + /* profiler.addValue("Initial Mutation 1"); */ + ratio_found = (double)(hypotheses_refined.size()) / desired_count; + ROS_INFO(" Ratio Refin 1: %f", 100*ratio_found ); + desired_count = (int)(ratio_found*desired_count); + hypotheses_refined = refineByMutation(model_, points, hypotheses_refined, tocam_tf, image_index, target, ERROR_THRESHOLD_MUTATION_2(image_index), 1.0, 1.0, desired_count); + /* profiler.addValue("Initial Mutation 2"); */ + ratio_found = (double)(hypotheses_refined.size()) / desired_count; + ROS_INFO(" Ratio Refin 2: %f", 100*ratio_found ); + desired_count = (int)(ratio_found*desired_count); + hypotheses_refined = refineByMutation(model_, points, hypotheses_refined, tocam_tf, image_index, target, ERROR_THRESHOLD_MUTATION_3(image_index), 1.0, 1.0, desired_count); + /* profiler.addValue("Initial Mutation 3"); */ int static_hypothesis_count = (int)(hypotheses_refined.size()); for (int i=0; i Date: Wed, 17 Apr 2024 12:02:21 +0200 Subject: [PATCH 4/6] ami default params changed --- launch/rw_three_sided.launch | 9 +++++++++ launch/rw_three_sided_basler.launch | 21 +++------------------ launch/sim_three_sided.launch | 8 +++----- src/blink_processor.cpp | 6 +++--- 4 files changed, 18 insertions(+), 26 deletions(-) diff --git a/launch/rw_three_sided.launch b/launch/rw_three_sided.launch index 18b757e..ccca5b1 100644 --- a/launch/rw_three_sided.launch +++ b/launch/rw_three_sided.launch @@ -38,6 +38,15 @@ + + + + + + + + + diff --git a/launch/rw_three_sided_basler.launch b/launch/rw_three_sided_basler.launch index 3b266b4..8c8fe4e 100644 --- a/launch/rw_three_sided_basler.launch +++ b/launch/rw_three_sided_basler.launch @@ -44,11 +44,10 @@ - - - + + - + @@ -91,19 +90,6 @@ - - - - - - - - - - - - - @@ -238,7 +224,6 @@ - diff --git a/launch/sim_three_sided.launch b/launch/sim_three_sided.launch index aed367d..824288e 100644 --- a/launch/sim_three_sided.launch +++ b/launch/sim_three_sided.launch @@ -57,11 +57,10 @@ - - - + + - + @@ -171,7 +170,6 @@ - diff --git a/src/blink_processor.cpp b/src/blink_processor.cpp index 0362b60..c3b9d0e 100755 --- a/src/blink_processor.cpp +++ b/src/blink_processor.cpp @@ -310,10 +310,10 @@ namespace uvdar{ /***** AMI params *****/ param_loader.loadParam("max_px_shift_x", _max_px_shift_.x, int(2)); param_loader.loadParam("max_px_shift_y", _max_px_shift_.y, int(2)); - param_loader.loadParam("max_zeros_consecutive", _max_zeros_consecutive_, int(2)); - param_loader.loadParam("stored_seq_len_factor", _stored_seq_len_factor_, int(15)); + param_loader.loadParam("max_zeros_consecutive", _max_zeros_consecutive_, int(10)); + param_loader.loadParam("stored_seq_len_factor", _stored_seq_len_factor_, int(20)); param_loader.loadParam("max_buffer_length", _max_buffer_length_, int(1000)); - param_loader.loadParam("poly_order", _poly_order_, int(2)); + param_loader.loadParam("poly_order", _poly_order_, int(4)); param_loader.loadParam("decay_factor", _decay_factor_, float(0.1)); param_loader.loadParam("confidence_probability", _conf_probab_percent_, double(75.0)); param_loader.loadParam("allowed_BER_per_seq", _allowed_BER_per_seq_, int(0)); From 003dc79a041c2fc5ef09769c319405ff38dc047d Mon Sep 17 00:00:00 2001 From: Tim Lakemann Date: Thu, 18 Apr 2024 21:53:10 +0200 Subject: [PATCH 5/6] ami params missing --- launch/rw_three_sided.launch | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/launch/rw_three_sided.launch b/launch/rw_three_sided.launch index ccca5b1..372ad63 100644 --- a/launch/rw_three_sided.launch +++ b/launch/rw_three_sided.launch @@ -275,6 +275,16 @@ + + + + + + + + + + From 697a8ceb85f5beca557915da9329c294f23952d5 Mon Sep 17 00:00:00 2001 From: viktor Date: Fri, 19 Apr 2024 17:45:43 +0200 Subject: [PATCH 6/6] Changed the nominal camera framerate to 59.9 to reduce the length of 'edge sampling' situations --- launch/rw_three_sided.launch | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/launch/rw_three_sided.launch b/launch/rw_three_sided.launch index 372ad63..2efbd54 100644 --- a/launch/rw_three_sided.launch +++ b/launch/rw_three_sided.launch @@ -22,7 +22,7 @@ - + @@ -262,8 +262,8 @@ - - + +