diff --git a/.gitignore b/.gitignore index 011aa7b2..50c5a58a 100644 --- a/.gitignore +++ b/.gitignore @@ -5,3 +5,4 @@ *.swp *.swo .gitman +.tmuxinator.yml diff --git a/.gitman.yml b/.gitman.yml index c45aa5c4..0b454f84 100644 --- a/.gitman.yml +++ b/.gitman.yml @@ -4,6 +4,7 @@ sources: name: ami rev: master type: git + params: sparse_paths: - links: @@ -14,8 +15,9 @@ sources: sources_locked: - repo: https://github.com/TimLakemann/ami.git name: ami - rev: 59402d1844ea8ae24ef0dc4a5d9fce5bb55e80a4 + rev: f9b44c56297effa62badf9cab835b06a6528e074 type: git + params: sparse_paths: - links: diff --git a/CMakeLists.txt b/CMakeLists.txt index 98f1238f..e09ad950 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -6,6 +6,8 @@ set(CMAKE_CXX_STANDARD_REQUIRED ON) add_definitions(-Wall) add_definitions(-Wextra) +add_definitions(-fno-omit-frame-pointer) +# add_definitions(-O0) if(NOT MRS_ENABLE_TESTING) message(WARNING "relwithdebinfo profile detected, building with -O3") diff --git a/config/blinking_sequences/mixed_L18_p0.444_HD2_NO5_NZ2_Na6.txt b/config/blinking_sequences/mixed_L18_p0.444_HD2_NO5_NZ2_Na6.txt new file mode 100644 index 00000000..523eaf15 --- /dev/null +++ b/config/blinking_sequences/mixed_L18_p0.444_HD2_NO5_NZ2_Na6.txt @@ -0,0 +1,6 @@ +0,0,1,0,0,1,0,0,1,0,1,1,0,1,1,0,1,0 +0,0,1,0,0,1,0,0,1,0,0,1,1,0,1,0,1,1 +0,0,1,0,0,1,0,1,0,1,0,1,0,1,1,0,0,1 +0,0,1,0,0,1,0,0,1,0,1,1,1,1,1,0,1,1 +0,0,1,0,0,1,0,1,1,1,1,1,0,1,1,0,1,1 +0,0,1,0,0,1,0,1,1,1,0,1,1,1,1,0,1,1 \ No newline at end of file diff --git a/config/camera_config/basler_grayscale.yaml b/config/camera_config/basler_grayscale.yaml index ec3a8119..29dafecd 100644 --- a/config/camera_config/basler_grayscale.yaml +++ b/config/camera_config/basler_grayscale.yaml @@ -29,8 +29,8 @@ image_encoding: "mono8" # (width / binning_x) x (height / binning_y). # The default values binning_x = binning_y = 0 are considered the same # as binning_x = binning_y = 1 (no subsampling). -binning_x: 1 -binning_y: 1 +binning_x: 2 +binning_y: 2 # The desired publisher frame rate if listening to the topics. # This parameter can only be set once at startup diff --git a/config/models/quadrotor_foursided_px4_motor_aligned_charlotte.txt b/config/models/quadrotor_foursided_px4_motor_aligned_charlotte.txt new file mode 100644 index 00000000..878b7a0a --- /dev/null +++ b/config/models/quadrotor_foursided_px4_motor_aligned_charlotte.txt @@ -0,0 +1,9 @@ +# X Y Z Type Pitch Yaw signal_id +0.212132 0.212132 0 0 0.0 0 2 +0.212132 0.212132 0 0 0.0 1.57079632679 2 +0.212132 -0.212132 0 0 0.0 0 0 +0.212132 -0.212132 0 0 0.0 -1.57079632679 0 +-0.212132 -0.212132 0 0 0.0 -1.57079632679 3 +-0.212132 -0.212132 0 0 0.0 -3.14159265359 3 +-0.212132 0.212132 0 0 0.0 1.57079632679 1 +-0.212132 0.212132 0 0 0.0 3.14159265359 1 diff --git a/launch/baslertest.launch b/launch/baslertest.launch new file mode 100644 index 00000000..b48567ee --- /dev/null +++ b/launch/baslertest.launch @@ -0,0 +1,253 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + ["$(arg calibrations_folder)/masks/$(arg mrs_id)_$(arg BASLER_LEFT_ID).png", "$(arg calibrations_folder)/masks/$(arg mrs_id)_$(arg BASLER_RIGHT_ID).png", "$(arg calibrations_folder)/masks/$(arg mrs_id)_$(arg BASLER_BACK_ID).png"] + + [ "camera_right"] + [ "points_seen_right"] + + + + + + + + + + + + + + + + + + + + + + + + [$(arg id1), $(arg id2), $(arg id3), $(arg id4), $(arg id5), $(arg id6), $(arg id7), $(arg id8), $(arg id9), $(arg id10), $(arg id11), $(arg id12), $(arg id13), $(arg id14), $(arg id15), $(arg id16), $(arg id17), $(arg id18), $(arg id19), $(arg id20), $(arg id21)] + + + + + + + + + + + + + + + + + + + + + + + + + [ "camera_right"] + [ "points_seen_right"] + [ "blinkers_seen_right"] + [ "estimated_framerate_right"] + + [ "ami_logging_right"] + [ "ami_all_seq_info_right"] + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/rw_three_sided_basler.launch b/launch/rw_three_sided_basler.launch index a5205623..6d6c264f 100644 --- a/launch/rw_three_sided_basler.launch +++ b/launch/rw_three_sided_basler.launch @@ -31,7 +31,7 @@ - + @@ -146,7 +146,7 @@ - + @@ -158,7 +158,7 @@ - + @@ -169,7 +169,7 @@ - + @@ -269,9 +269,17 @@ + + + + + - - + + @@ -298,7 +306,7 @@ - ["$(arg calibrations_folder)/camera_calibrations/calib_results_bs_uv_$(arg left).txt", "$(arg calibrations_folder)/camera_calibrations/calib_results_bs_uv_$(arg right).txt", "$(arg calibrations_folder)/camera_calibrations/calib_results_bs_uv_$(arg back).txt"] + ["$(arg calibrations_folder)/camera_calibrations/calib_results_bs_uv_$(arg BASLER_LEFT_ID).txt", "$(arg calibrations_folder)/camera_calibrations/calib_results_bs_uv_$(arg BASLER_RIGHT_ID).txt", "$(arg calibrations_folder)/camera_calibrations/calib_results_bs_uv_$(arg BASLER_BACK_ID).txt"] diff --git a/launch/testing/camera_only_three_sided_basler.launch b/launch/testing/camera_only_three_sided_basler.launch index dd30b930..a0a2a4d5 100644 --- a/launch/testing/camera_only_three_sided_basler.launch +++ b/launch/testing/camera_only_three_sided_basler.launch @@ -29,7 +29,7 @@ - + @@ -132,7 +132,7 @@ - + @@ -144,7 +144,7 @@ - + @@ -155,7 +155,7 @@ - + diff --git a/scripts/basler_tests/config/custom_config.yaml b/scripts/basler_tests/config/custom_config.yaml new file mode 100644 index 00000000..50166037 --- /dev/null +++ b/scripts/basler_tests/config/custom_config.yaml @@ -0,0 +1,17 @@ +mrs_uav_managers: + + estimation_manager: + + # loaded state estimator plugins + # available in mrs_uav_state_estimators: gps_garmin, gps_baro, rtk, aloam, ground_truth, dummy + state_estimators: [ + "gps_baro", + "gps_garmin", + # "rtk", + # "aloam", + # "ground_truth", + # "dummy" + ] + + initial_state_estimator: "gps_garmin" # will be used as the first state estimator + agl_height_estimator: "garmin_agl" # only slightly filtered height for checking min height (not used in control feedback) diff --git a/scripts/basler_tests/config/network_config.yaml b/scripts/basler_tests/config/network_config.yaml new file mode 100644 index 00000000..2f2c9381 --- /dev/null +++ b/scripts/basler_tests/config/network_config.yaml @@ -0,0 +1,18 @@ +# 1. This list is used by NimbroNetwork for mutual communication of the UAVs +# The names of the robots have to match hostnames described in /etc/hosts. +# +# 2. This list is used by MpcTracker for mutual collision avoidance of the UAVs. +# The names should match the true "UAV_NAMES" (the topic prefixes). +# +# network_config:=~/config/network_config.yaml +# +# to the core.launch and nimbro.launch. + +network: + + robot_names: [ + uav1, + uav2, + uav3, + uav4, + ] diff --git a/scripts/basler_tests/record.sh b/scripts/basler_tests/record.sh new file mode 100755 index 00000000..28eda59d --- /dev/null +++ b/scripts/basler_tests/record.sh @@ -0,0 +1,65 @@ +#!/bin/bash + +path="/home/\$(optenv USER mrs)/bag_files/latest/" + +# By default, we record everything. +# Except for this list of EXCLUDED topics: +exclude=( + +# IN GENERAL, DON'T RECORD CAMERAS +# +# If you want to record cameras, create a copy of this script +# and place it at your tmux session. +# +# Please, seek an advice of a senior researcher of MRS about +# what can be recorded. Recording too much data can lead to +# ROS communication hiccups, which can lead to eland, failsafe +# or just a CRASH. + +# Every topic containing "compressed" +'(.*)compressed(.*)' +# Every topic containing "image_raw" +'(.*)image_raw(.*)' +# Every topic containing "theora" +'(.*)theora(.*)' +# Every topic containing "h264" +'(.*)h264(.*)' + +) + +# file's header +filename=`mktemp` +echo "" > "$filename" +echo "" >> "$filename" +echo "" >> "$filename" + +echo -n "> "$filename" + +# if there is anything to exclude +if [ "${#exclude[*]}" -gt 0 ]; then + + echo -n " -x " >> "$filename" + + # list all the string and separate the with | + for ((i=0; i < ${#exclude[*]}; i++)); + do + echo -n "${exclude[$i]}" >> "$filename" + if [ "$i" -lt "$( expr ${#exclude[*]} - 1)" ]; then + echo -n "|" >> "$filename" + fi + done + +fi + +echo "\">" >> "$filename" + +echo "" >> "$filename" +echo "" >> "$filename" + +# file's footer +echo "" >> "$filename" +echo "" >> "$filename" +echo "" >> "$filename" + +cat $filename +roslaunch $filename diff --git a/scripts/basler_tests/tmux.sh b/scripts/basler_tests/tmux.sh new file mode 100755 index 00000000..c4b48f31 --- /dev/null +++ b/scripts/basler_tests/tmux.sh @@ -0,0 +1,175 @@ +#!/bin/bash +### BEGIN INIT INFO +# Provides: tmux +# Required-Start: $local_fs $network dbus +# Required-Stop: $local_fs $network +# Default-Start: 2 3 4 5 +# Default-Stop: 0 1 6 +# Short-Description: start the uav +### END INIT INFO +if [ "$(id -u)" == "0" ]; then + exec sudo -u mrs "$0" "$@" +fi + +source $HOME/.bashrc + +# location for storing the bag files +# * do not change unless you know what you are doing +MAIN_DIR=~/"bag_files" + +# the project name +# * is used to define folder name in ~/$MAIN_DIR +PROJECT_NAME=basler_uvdar_test + +# the name of the TMUX session +# * can be used for attaching as 'tmux a -t ' +SESSION_NAME=mav + +# following commands will be executed first in each window +# * do NOT put ; at the end +pre_input="" + +# define commands +# 'name' 'command' +# * DO NOT PUT SPACES IN THE NAMES +# * "new line" after the command => the command will be called after start +# * NO "new line" after the command => the command will wait for user's +input=( + 'Rosbag' 'waitForOffboard; ./record.sh +' + 'Sensors' 'waitForTime; roslaunch mrs_uav_deployment sensors.launch +' + 'Luxonis' 'waitForTime; roslaunch ~/git/luxonis.launch +' + 'Nimbro' 'waitForTime; rosrun mrs_uav_deployment run_nimbro.py ./config/network_config.yaml `rospack find mrs_uav_deployment`/config/communication_config.yaml +' + 'HwApi' 'waitForTime; roslaunch mrs_uav_px4_api api.launch +' + 'Status' 'waitForHw; roslaunch mrs_uav_status status.launch +' + 'Core' 'waitForTime; roslaunch mrs_uav_core core.launch platform_config:=`rospack find mrs_uav_deployment`/config/mrs_uav_system/$UAV_TYPE.yaml world_config:=`rospack find mrs_uav_deployment`/config/worlds/world_$WORLD_NAME.yaml custom_config:=./config/custom_config.yaml network_config:=./config/network_config.yaml +' + 'AutoStart' 'waitForHw; roslaunch mrs_uav_autostart automatic_start.launch +' +# do NOT modify the command list below + 'EstimDiag' 'waitForHw; rostopic echo /'"$UAV_NAME"'/estimation_manager/diagnostics +' + 'kernel_log' 'tail -f /var/log/kern.log -n 100 +' + 'roscore' 'roscore +' + 'simtime' 'waitForRos; rosparam set use_sim_time false +' + 'UVDAR' 'waitForRos; sleep 10; roslaunch uvdar_core rw_three_sided_basler.launch +' +) + +# the name of the window to focus after start +init_window="Status" + +# automatically attach to the new session? +# {true, false}, default true +attach=true + +########################### +### DO NOT MODIFY BELOW ### +########################### + +export TMUX_BIN="/usr/bin/tmux -L mrs -f /etc/ctu-mrs/tmux.conf" + +# find the session +FOUND=$( $TMUX_BIN ls | grep $SESSION_NAME ) + +if [ $? == "0" ]; then + echo "The session already exists" + $TMUX_BIN -2 attach-session -t $SESSION_NAME + exit +fi + +# Absolute path to this script. /home/user/bin/foo.sh +SCRIPT=$(readlink -f $0) +# Absolute path this script is in. /home/user/bin +SCRIPTPATH=`dirname $SCRIPT` + +TMUX= $TMUX_BIN new-session -s "$SESSION_NAME" -d +echo "Starting new session." + +# get the iterator +ITERATOR_FILE="$MAIN_DIR/$PROJECT_NAME"/iterator.txt +if [ -e "$ITERATOR_FILE" ] +then + ITERATOR=`cat "$ITERATOR_FILE"` + ITERATOR=$(($ITERATOR+1)) +else + echo "iterator.txt does not exist, creating it" + mkdir -p "$MAIN_DIR/$PROJECT_NAME" + touch "$ITERATOR_FILE" + ITERATOR="1" +fi +echo "$ITERATOR" > "$ITERATOR_FILE" + +# create file for logging terminals' output +LOG_DIR="$MAIN_DIR/$PROJECT_NAME/" +SUFFIX=$(date +"%Y_%m_%d_%H_%M_%S") +SUBLOG_DIR="$LOG_DIR/"$ITERATOR"_"$SUFFIX"" +TMUX_DIR="$SUBLOG_DIR/tmux" +mkdir -p "$SUBLOG_DIR" +mkdir -p "$TMUX_DIR" + +# link the "latest" folder to the recently created one +rm "$LOG_DIR/latest" > /dev/null 2>&1 +rm "$MAIN_DIR/latest" > /dev/null 2>&1 +ln -sf "$SUBLOG_DIR" "$LOG_DIR/latest" +ln -sf "$SUBLOG_DIR" "$MAIN_DIR/latest" + +# create arrays of names and commands +for ((i=0; i < ${#input[*]}; i++)); +do + ((i%2==0)) && names[$i/2]="${input[$i]}" + ((i%2==1)) && cmds[$i/2]="${input[$i]}" +done + +# run tmux windows +for ((i=0; i < ${#names[*]}; i++)); +do + $TMUX_BIN new-window -t $SESSION_NAME:$(($i+1)) -n "${names[$i]}" +done + +sleep 3 + +# start loggers +for ((i=0; i < ${#names[*]}; i++)); +do + $TMUX_BIN pipe-pane -t $SESSION_NAME:$(($i+1)) -o "ts | cat >> $TMUX_DIR/$(($i+1))_${names[$i]}.log" +done + +# send commands +for ((i=0; i < ${#cmds[*]}; i++)); +do + $TMUX_BIN send-keys -t $SESSION_NAME:$(($i+1)) "cd $SCRIPTPATH; ${pre_input}; ${cmds[$i]}" +done + +# identify the index of the init window +init_index=0 +for ((i=0; i < ((${#names[*]})); i++)); +do + if [ ${names[$i]} == "$init_window" ]; then + init_index=$(expr $i + 1) + fi +done + +$TMUX_BIN select-window -t $SESSION_NAME:$init_index + +if $attach; then + + if [ -z ${TMUX} ]; + then + $TMUX_BIN -2 attach-session -t $SESSION_NAME + else + tmux detach-client -E "tmux -L mrs a -t $SESSION_NAME" + fi +else + echo "The session was started" + echo "You can later attach by calling:" + echo " tmux -L mrs a -t $SESSION_NAME" +fi diff --git a/scripts/utilities/basler_get_cam.sh b/scripts/utilities/basler_get_cam.sh new file mode 100755 index 00000000..6527cdaf --- /dev/null +++ b/scripts/utilities/basler_get_cam.sh @@ -0,0 +1,4 @@ +#!/bin/bash + +printout=`timeout 10s roslaunch uvdar_core cam_only_basler.launch` +echo "$printout" | grep Found | grep -oP -m1 'Serial\sNumber\s+\K\S+(?=:)' diff --git a/scripts/utilities/record_calibration_basler.yml b/scripts/utilities/record_calibration_basler.yml index 65f171b5..07d9a73e 100644 --- a/scripts/utilities/record_calibration_basler.yml +++ b/scripts/utilities/record_calibration_basler.yml @@ -1,27 +1,30 @@ name: uvdar_calib_record root: ./ startup_window: termviz -pre_window: devnum=$BASLER_UV_RIGHT - +pre_window: windows: + - devnum: + layout: even-vertical + panes: + - ./basler_get_cam.sh > devnum.txt - roscore: layout: even-vertical panes: - - sleep 4; roscore + - sleep 12; roscore - directories: layout: even-vertical panes: - - sleep 2; mkdir ~/bag_files; mkdir ~/bag_files/uvdar_calib/; mkdir ~/bag_files/uvdar_calib/${devnum} + - sleep 12; devnum=`cat devnum.txt`; mkdir ~/bag_files; mkdir ~/bag_files/uvdar_calib/; mkdir ~/bag_files/uvdar_calib/${devnum} - basler: 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} - - waitForRos; roslaunch uvdar_core cam_only_basler.launch expose_us:=1000 FRAME_RATE:=5 BASLER_ID:=$devnum + - sleep 12; waitForRos; roslaunch uvdar_core cam_only_basler.launch expose_us:=1000 FRAME_RATE:=5 - recording: layout: even-vertical panes: - - waitForRos; sleep 1; rosbag record -a -x '(.*)compressed(.*)' -x '(.*)theora(.*)' -O ~/bag_files/uvdar_calib/${devnum}/C_${devnum}.bag + - sleep 12; devnum=`cat devnum.txt`; waitForRos; sleep 1; 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 12; waitForRos; termviz record_calibration_termviz.yml diff --git a/src/blink_processor.cpp b/src/blink_processor.cpp index c3b9d0e5..72a60ccf 100755 --- a/src/blink_processor.cpp +++ b/src/blink_processor.cpp @@ -216,6 +216,7 @@ namespace uvdar{ // for extracting the received sequences from the AMI/4DHT struct BlinkData{ + bool valid = false; ros::Time last_sample_time; ros::Time last_sample_time_diagnostic; unsigned int sample_count = -1; @@ -573,6 +574,7 @@ namespace uvdar{ } blink_data_[img_index].last_sample_time = pts_msg->stamp; + blink_data_[img_index].valid = true; if (_debug_) { ROS_INFO_STREAM("[UVDARBlinkProcessor]: Received contours: " << pts_msg->points.size()); @@ -736,6 +738,11 @@ namespace uvdar{ } if(_use_4DHT_){ + + if (!(blink_data_[image_index].valid)){ + return; + } + uvdar_core::ImagePointsWithFloatStamped msg; if (_debug_){ diff --git a/src/pose_calculator.cpp b/src/pose_calculator.cpp index 6b47fea5..bc87b8da 100644 --- a/src/pose_calculator.cpp +++ b/src/pose_calculator.cpp @@ -2433,18 +2433,18 @@ 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"); double ratio_found = (double)(hypotheses_init.size()) / (double)(INITIAL_ROUGH_HYPOTHESIS_COUNT); - ROS_INFO(" Ratio Init: %f", 100*ratio_found ); + //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 ); + //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 ); + //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"); */