Skip to content

Commit

Permalink
Merge branch 'sans_sun_points' of github.com:ctu-mrs/uvdar_core into …
Browse files Browse the repository at this point in the history 
…sans_sun_points
  • Loading branch information
@ViktorWalter
ViktorWalter committed Apr 20, 2024
2 parents 3fa5a4c + 697a8ce commit bb53094
Show file tree
Hide file tree
Showing 8 changed files with 102 additions and 74 deletions.
37 changes: 2 additions & 35 deletions install/install_on_real_UAV.sh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
37 changes: 37 additions & 0 deletions install/test_leds.sh
View file
Original file line number Diff line number Diff line change
@@ -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
25 changes: 22 additions & 3 deletions launch/rw_three_sided.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,7 @@

<arg name="calibrations_folder" default="$(find mrs_uav_deployment)/config/uvdar_calibrations"/>

<arg name="camera_rate" default="60"/>
<arg name="camera_rate" default="59.90"/>
<arg name="fps" default="$(arg camera_rate)"/>
<arg name="idpf" default="0"/>
<arg name="aec" default="false"/>
Expand All @@ -38,6 +38,15 @@
<arg name="jpeg_quality" default="90"/>

<arg name="blink_process_rate" default="10"/>
<arg name="max_px_shift_y" default="3"/>
<arg name="max_px_shift_x" default="3"/>
<arg name="max_zeros_consecutive" default="10"/>
<arg name="stored_seq_len_factor" default="20"/>
<arg name="max_buffer_length" default="5000"/>
<arg name="poly_order" default="4"/>
<arg name="decay_factor" default="0.1"/>
<arg name="confidence_probability" default="95.0"/>
<arg name="allowed_BER_per_seq" default="0"/>

<arg name="accumulator_length" default="14"/>
<arg name="pitch_steps" default="16"/>
Expand Down Expand Up @@ -253,8 +262,8 @@
<node name="blink_processor" pkg="nodelet" type="nodelet" args="$(arg nodelet) uvdar/UVDARBlinkProcessor $(arg nodelet_manager)" output="screen" respawn="true">
<!-- <node name="blink_processor" pkg="nodelet" type="nodelet" args="$(arg nodelet) uvdar/UVDARBlinkProcessor $(arg nodelet_manager)" output="screen" respawn="true" launch-prefix="roslaunch_debug"> -->
<param name="uav_name" type = "string" value="$(arg uav_name)"/>
<!-- <param name="debug" type="bool" value="$(arg debug)"/> -->
<param name="debug" type="bool" value="false"/>
<param name="debug" type="bool" value="$(arg debug)"/>
<!-- <param name="debug" type="bool" value="true"/> -->
<param name="visual_debug" type="bool" value="$(arg visual_debug)"/>
<param name="gui" type="bool" value="$(arg gui)"/>
<param name="publish_visualization" type="bool" value="$(arg publish_visualization)"/>
Expand All @@ -266,6 +275,16 @@

<param name="blink_process_rate" type="int" value="$(arg blink_process_rate)"/>

<param name="max_px_shift_x" type="int" value="$(arg max_px_shift_x)"/>
<param name="max_px_shift_y" type="int" value="$(arg max_px_shift_y)"/>
<param name="max_zeros_consecutive" type="int" value="$(arg max_zeros_consecutive)"/>
<param name="stored_seq_len_factor" type="int" value="$(arg stored_seq_len_factor)"/>
<param name="max_buffer_length" type="int" value="$(arg max_buffer_length)"/>
<param name="poly_order" type="int" value="$(arg poly_order)"/>
<param name="decay_factor" type="double" value="$(arg decay_factor)"/>
<param name="confidence_probability" type="double" value="$(arg confidence_probability)"/>
<param name="allowed_BER_per_seq" type="int" value="$(arg allowed_BER_per_seq)"/>

<param name="accumulator_length" type="int" value="$(arg accumulator_length)"/>
<param name="pitch_steps" type="int" value="$(arg pitch_steps)"/>
<param name="yaw_steps" type="int" value="$(arg yaw_steps)"/>
Expand Down
21 changes: 3 additions & 18 deletions launch/rw_three_sided_basler.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,11 +44,10 @@
<!--AMI values-->
<arg name="max_px_shift_y" default="3"/>
<arg name="max_px_shift_x" default="3"/>
<arg name="max_zeros_consecutive" default="2"/>
<arg name="max_ones_consecutive" default="2"/>
<arg name="stored_seq_len_factor" default="1"/>
<arg name="max_zeros_consecutive" default="10"/>
<arg name="stored_seq_len_factor" default="20"/>
<arg name="max_buffer_length" default="5000"/>
<arg name="poly_order" default="2"/>
<arg name="poly_order" default="4"/>
<arg name="decay_factor" default="0.1"/>
<arg name="confidence_probability" default="95.0"/>
<arg name="allowed_BER_per_seq" default="0"/>
Expand Down Expand Up @@ -91,19 +90,6 @@

<arg name="sequence_file" default="$(find uvdar_core)/config/selected.txt"/>

<!-- Params for Tim's BP_Processor-->
<arg name="max_px_shift_x" default="2"/>
<arg name="max_px_shift_y" default="2"/>
<arg name="max_zeros_consecutive" default="2"/>
<arg name="max_ones_consecutive" default="2"/>
<arg name="stored_seq_len_factor" default="15"/>
<arg name="poly_order" default="4"/>
<arg name="decay_factor" default="0.01"/>
<arg name="confidence_probability" default="95.0"/>
<arg name="frame_tolerance" default="5"/>
<arg name="allowed_BER_per_seq" default="0"/>


<!-- Model settings: -->

<arg name="use_masks" default="true"/>
Expand Down Expand Up @@ -238,7 +224,6 @@
<param name="max_px_shift_x" type="int" value="$(arg max_px_shift_x)"/>
<param name="max_px_shift_y" type="int" value="$(arg max_px_shift_y)"/>
<param name="max_zeros_consecutive" type="int" value="$(arg max_zeros_consecutive)"/>
<param name="max_ones_consecutive" type="int" value="$(arg max_ones_consecutive)"/>
<param name="stored_seq_len_factor" type="int" value="$(arg stored_seq_len_factor)"/>
<param name="max_buffer_length" type="int" value="$(arg max_buffer_length)"/>
<param name="poly_order" type="int" value="$(arg poly_order)"/>
Expand Down
8 changes: 3 additions & 5 deletions launch/sim_three_sided.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -57,11 +57,10 @@

<arg name="max_px_shift_y" default="3"/>
<arg name="max_px_shift_x" default="3"/>
<arg name="max_zeros_consecutive" default="2"/>
<arg name="max_ones_consecutive" default="2"/>
<arg name="stored_seq_len_factor" default="1"/>
<arg name="max_zeros_consecutive" default="10"/>
<arg name="stored_seq_len_factor" default="20"/>
<arg name="max_buffer_length" default="5000"/>
<arg name="poly_order" default="2"/>
<arg name="poly_order" default="4"/>
<arg name="decay_factor" default="0.1"/>
<arg name="confidence_probability" default="95.0"/>
<arg name="allowed_BER_per_seq" default="0"/>
Expand Down Expand Up @@ -171,7 +170,6 @@
<param name="max_px_shift_x" type="int" value="$(arg max_px_shift_x)"/>
<param name="max_px_shift_y" type="int" value="$(arg max_px_shift_y)"/>
<param name="max_zeros_consecutive" type="int" value="$(arg max_zeros_consecutive)"/>
<param name="max_ones_consecutive" type="int" value="$(arg max_ones_consecutive)"/>
<param name="stored_seq_len_factor" type="int" value="$(arg stored_seq_len_factor)"/>
<param name="max_buffer_length" type="int" value="$(arg max_buffer_length)"/>
<param name="poly_order" type="int" value="$(arg poly_order)"/>
Expand Down
6 changes: 3 additions & 3 deletions src/blink_processor.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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));
Expand Down
17 changes: 15 additions & 2 deletions src/led_manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -196,7 +197,8 @@ namespace uvdar {

/* if (i == 3) */
baca_protocol_publisher.publish(serial_msg);
sleeper.sleep();

local_sleeper.sleep();
i++;
}

Expand Down Expand Up @@ -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;
}

Expand Down
25 changes: 17 additions & 8 deletions src/pose_calculator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)

Expand Down Expand Up @@ -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<static_hypothesis_count; i++){
Expand Down

0 comments on commit bb53094

Please sign in to comment.