Skip to content
Snippets Groups Projects
Commit 3ccbbb82 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

Merge branch 'devel' of...

Merge branch 'devel' of ssh://gitlab.iri.upc.edu:2202/mobile_robotics/wolf_projects/wolf_ros/demos/wolf_demo_imu2d into devel
parents ca1b0e74 f6493e66
No related branches found
No related tags found
No related merge requests found
Showing
with 562 additions and 27 deletions
#!/bin/bash
BAG="ana_lab_2"
if [[ -z "${BAG}" ]]
then
BAG="ana_lab_2"
fi
rm $BAG\_filtered.bag
rm $BAG\_filtered_notf.bag
rosbag filter $BAG.bag $BAG\_filtered.bag "topic == '/ana/imu/data' or topic == '/ana/odom' or topic == '/ana/sensors/bno055_imu/imu' or topic == '/ana/sensors/scan' or topic == '/tf'"
python3 filter_tf.py $BAG\_filtered.bag $BAG\_filtered_notf.bag
#python3 filter_tf2.py -i $BAG\_filtered.bag -o $BAG\_filtered_notf.bag -f ana/front_left_axle
#!/bin/bash
BAG="ana_lab_2"
t0=1620123952.21
tend=1620123952.21
echo "Press CTRL+C to proceed."
trap "pkill -f 'sleep 1h'" INT
trap "set +x ; sleep 1h ; set -x" DEBUG
#roslaunch wolf_demo_imu2d imu2d_demo.launch bag:=$BAG\_filtered_notf test:=0 speed:=1 suffix:="_GT"
#rosbag filter trajectory_recording_GT.bag trajectory_recording_GT_tmp.bag "t.to_sec >= $tend"
#rostopic echo -b trajectory_recording_GT_tmp.bag /wolf_ros_node/trajectory -p > trajectory_$BAG\_exp_$i.csv
rm trajectory_recording_GT_tmp.bag
#python3 transpose_csv.py trajectory_$BAG\_exp_$i.csv trajectory_$BAG\_exp_$i\_transp.csv
for i in 10 20 30 40 50
if [[ -z "${BAG}" ]]
then
BAG="ana_lab_2"
fi
#yamlfiles are divided into two types:
# Starting with G ==> groundtruth, we extract the trajectory
# Starting with E ==> experiment, we extract the poses
for yamlfile in $(find ../yaml/trajectory_analysys -maxdepth 1 -type f -printf "%f\n")
do
filtertime=$t0+$i+30.0-1.0
roslaunch wolf_demo_imu2d imu2d_demo.launch bag:=$BAG\_shortened_$i test:=5 speed:=1 suffix:="_$i"
rostopic echo -b trajectory_recording_$i.bag /wolf_ros_node/pose_pose_with_cov -p > trajectory_$BAG\_exp_$i.csv
python3 transpose_csv.py trajectory_$BAG\_exp_$i.csv trajectory_$BAG\_exp_$i\_transp.csv
yamlname=${yamlfile%.*}
roslaunch wolf_demo_imu2d imu2d_analysys.launch bag:=$BAG\_filtered_notf test:=$yamlfile speed:=1 suffix:=$yamlname
if [[ ${yamlname:0:1} == 'G' ]]
then
tend=$(rosbag info -y -k end recordings/trajectory_recording$yamlname.bag)
tend=${tend%.*}
rosbag filter recordings/trajectory_recording$yamlname.bag recordings/trajectory_recording$yamlname\_tmp.bag "topic == '/wolf_ros_node/trajectory' and t.to_sec() >= $tend"
rm CSV/$yamlname.csv
rostopic echo -b recordings/trajectory_recording$yamlname\_tmp.bag /wolf_ros_node/trajectory -p > CSV/$yamlname.csv
rm recordings/trajectory_recording$yamlname\_tmp.bag
elif [[ ${yamlname:0:1} == 'E' ]]
then
rm CSV/$yamlname.csv
rostopic echo -b recordings/trajectory_recording$yamlname.bag /wolf_ros_node/pose_pose_with_cov -p > CSV/$yamlname.csv
fi
rm CSV/transp/$yamlname\_transp.csv
python3 transpose_csv.py CSV/$yamlname.csv CSV/transp/$yamlname\_transp.csv
done
#!/bin/bash
BAG="ana_lab_2"
tstartstr=$(rosbag info $BAG.bag | grep start)
t0=$(echo $tstr | cut -d "(" -f2 | cut -d ")" -f1 | bc -l)
if [[ -z "${BAG}" ]]
then
BAG="ana_lab_2"
fi
t0=$(rosbag info -y -k start $BAG\_filtered_notf.bag)
for i in 10 20 30 40 50
do
cuttime=$(($t0+$i))
cuttime=$( bc <<< "$t0+$i" )
echo $cuttime
endtime=$(($cuttime+30.0))
rosbag filter $BAG\_filtered_notf.bag $BAG\_shortened\_$i.bag "(topic != '/ana/sensors/scan' and t.to_sec() <=$endtime) or (topic == '/ana/sensors/scan' and t.to_sec() <= $cuttime)"
endtime=$(bc <<< "$cuttime+30")
rm $BAG\_shortened\_$i.bag
rosbag filter $BAG\_filtered_notf.bag $BAG\_shortened\_$i.bag "(topic != '/ana/sensors/scan' and t.to_sec() <= $endtime ) or (topic == '/ana/sensors/scan' and t.to_sec() <= $cuttime)"
done
<!-- -->
<launch>
<param name="use_sim_time" value="true" />
<arg name="speed" default="1" />
<arg name="sec" default="0" />
<arg name="profiling" default="false" />
<arg name="gdb" default="false" />
<arg name="test" default="" />
<arg name="bag" default="" />
<arg name="suffix" default="" />
<arg name="launch_pref" default="" unless="$(eval profiling or gdb)"/>
<arg name="launch_pref" value="valgrind --tool=callgrind --callgrind-out-file='callgrind.wolf.%p'" if="$(arg profiling)" />
<arg name="launch_pref" value="gdb -ex run --args" if="$(arg gdb)" />
<!-- using "clock" option to use the simulated time. Rosbag launch as the first node -->
<node pkg="rostopic" type="rostopic" name="" args='pub /helena/sensors/bno055_imu/imu/topic_enable std_msgs/Bool "data: True"'/>
<node pkg="rosbag" type="record" name="recorder" args='record -O $(find wolf_demo_imu2d)/bag/recordings/trajectory_recording$(arg suffix).bag "/wolf_ros_node/trajectory" "/wolf_ros_node/pose_pose_with_cov"'/>
<node pkg="tf"
type="static_transform_publisher"
name="static_tf"
args="0 0 0 0 0 0 /ana/base_footprint /ana/base_link 100"/>
<node pkg="tf"
type="static_transform_publisher"
name="static_tf2"
args="0 0 0 0 0 0 /ana/base_link /ana/imu_bno055 100"/>
<node pkg="tf"
type="static_transform_publisher"
name="static_tf3"
args="0 0 0 3.14159265 0 0 /ana/base_link /ana/velodyne 100"/>
<node pkg="tf"
type="static_transform_publisher"
name="static_tf4"
args="-0.11 0.01 0 0 0 0 /ana/base_link /ana/imu_microstrain 100"/>
<node pkg="rosbag"
type="play"
name="player"
required="true"
args="-s $(arg sec) -r $(arg speed) --clock $(find wolf_demo_imu2d)/bag/$(arg bag).bag"/>
<node type="rviz"
name="rviz"
pkg="rviz"
args="-d $(find wolf_demo_imu2d)/rviz/imu2d_demo.rviz"/>
<node type="wolf_ros_node"
name="wolf_ros_node"
pkg="wolf_ros_node"
required="true"
output="screen"
launch-prefix="$(arg launch_pref)">
<param name="~yaml_file_path" value="$(find wolf_demo_imu2d)/yaml/trajectory_analysys/$(arg test)" />
</node>
</launch>
......@@ -18,7 +18,7 @@
<node pkg="rostopic" type="rostopic" name="" args='pub /helena/sensors/bno055_imu/imu/topic_enable std_msgs/Bool "data: True"'/>
<group if="$(arg record)">
<node pkg="rosbag" type="record" name="recorder" args='record -O $(find wolf_demo_imu2d)/bag/trajectory_recording$(arg suffix).bag "/wolf_ros_node/trajectory" "/wolf_ros_node/pose_pose_with_cov"'/>
<node pkg="rosbag" type="record" name="recorder" args='record -O $(find wolf_demo_imu2d)/bag/recordings/trajectory_recording$(arg suffix).bag "/wolf_ros_node/trajectory" "/wolf_ros_node/pose_pose_with_cov"'/>
</group>
<node pkg="tf"
......
#microstrain
extrinsic:
pose: [0,0,0]
a_noise: 0.9 #0.9
a_noise: 0.05 #0.9
w_noise: 0.01 #0.01
ab_initial_stdev: 0.5 # m/s2 - initial bias
wb_initial_stdev: 0.1 # rad/sec - initial bias
ab_rate_stdev: 0.001 # m/s2/sqrt(s)
wb_rate_stdev: 0.001 # rad/s/sqrt(s)
ab_rate_stdev: 0.00001 # m/s2/sqrt(s)
wb_rate_stdev: 0.00001 # rad/s/sqrt(s)
orthogonal_gravity: true
......@@ -3,7 +3,7 @@ keyframe_vote:
min_features_for_keyframe: 10
min_dist: 999
min_angle: 999
min_time: 1
min_time: 0.1
min_error: 999
max_points: 0
max_new_features: 15
......
extrinsic:
pose: [0,0,0]
k_disp_to_disp: 0.08
k_rot_to_rot: 0.6
k_rot_to_rot: 0.6 #0.6
config:
debug:
profiling: true
profiling_file: "~/wolf_demo_profiling_imu2d_test5.txt"
print_problem: false
print_period: 2
print_depth: 4
print_constr_by: false
print_metric: true
print_state_blocks: true
problem:
tree_manager:
type: "none"
frame_structure: "POV"
dimension: 2
prior:
mode: "fix"
$state:
P: [0,0]
O: [0]
V: [0,0]
time_tolerance: 0.1
node_rate: 100
map:
type: "MapBase"
plugin: "core"
solver:
follow: "parameters/solver.yaml"
sensors:
-
type: "SensorLaser2d"
name: "scanner_front_left"
plugin: "laser"
follow: "parameters/test_laser_params.yaml"
-
type: "SensorImu2d"
name: "bno"
plugin: "imu"
follow: "parameters/test_imu_params_bno.yaml"
-
type: "SensorImu2d"
name: "microstrain"
plugin: "imu"
follow: "parameters/test_imu_params_microstrain.yaml"
processors:
-
type: "ProcessorOdomIcp"
name: "processorodomicp"
sensor_name: "scanner_front_left"
plugin: "laser"
keyframe_vote:
voting_active: true
min_features_for_keyframe: 10
min_dist: 999
min_angle: 999
min_time: 1
min_error: 999
max_points: 0
follow: "parameters/test_laser_processor.yaml"
#-
# type: "ProcessorLoopClosureIcp"
# name: "processorloopclosureicp"
# sensor_name: "scanner_front_left"
# plugin: "laser"
# time_tolerance: 0.1
# apply_loss_function: true
# keyframe_vote:
# voting_active: false
# recent_frames_ignored: 10
# frames_ignored_after_loop: 0
# max_error_threshold: 0.02
# min_points_percent: 40
# max_loops: 1
# max_candidates: 5
# max_attempts: 5
# candidate_generation: "random" # 'random' or 'tree'
# icp:
# follow: "parameters/csm.yaml"
-
type: "ProcessorImu2d"
name: "processorimu2dbno"
sensor_name: "bno"
plugin: "imu"
follow: "parameters/test_imu_processor_bno.yaml"
-
type: "ProcessorImu2d"
name: "processorimu2dmicro"
sensor_name: "microstrain"
plugin: "imu"
follow: "parameters/test_imu_processor_microstrain.yaml"
ROS subscriber:
-
package: "wolf_ros_laser"
type: "SubscriberLaser2d"
topic: "/ana/sensors/scan"
sensor_name: "scanner_front_left"
load_params_from_msg: true
-
package: "wolf_ros_imu"
type: "SubscriberImuEnableable"
topic: "/ana/sensors/bno055_imu/imumal"
sensor_name: "bno"
follow: "parameters/test_imu_subscriber_bno.yaml"
-
package: "wolf_ros_imu"
type: "SubscriberImuEnableable"
topic: "/ana/imu/data"
sensor_name: "microstrain"
follow: "parameters/test_imu_subscriber_microstrain.yaml"
ROS publisher:
-
package: "wolf_ros_node"
type: "PublisherTf"
topic: " "
period: 0.1
follow: "parameters/test_publisher_ros_node.yaml"
-
package: "wolf_ros_node"
type: "PublisherGraph"
topic: "graph"
period: 1
viz_overlapped_factors: true
-
package: "wolf_ros_node"
type: "PublisherTrajectory"
topic: "trajectory"
period: 1
frame_id: "map"
-
package: "wolf_ros_node"
type: "PublisherPose"
topic: "pose"
extrinsics: false
period: 1
frame_id: "map"
-
package: "wolf_ros_laser"
type: "PublisherLaserMap"
topic: "map"
period: 1
map_frame_id: "map"
update_dist_th: 0.05
update_angle_th: 0.05
max_n_cells: 1000000
grid_size: 0.1
p_free: 0.3
p_obst: 0.8
p_free_th: 0.2
p_obst_th: 0.9
discard_max_range: true
config:
debug:
profiling: true
profiling_file: "~/wolf_demo_profiling_imu2d_test0.txt"
print_problem: false
print_period: 2
print_depth: 4
print_constr_by: false
print_metric: true
print_state_blocks: true
problem:
tree_manager:
type: "none"
frame_structure: "POV"
dimension: 2
prior:
mode: "fix"
$state:
P: [0,0]
O: [0]
V: [0,0]
time_tolerance: 0.1
node_rate: 100
map:
type: "MapBase"
plugin: "core"
solver:
follow: "parameters/solver.yaml"
sensors:
-
type: "SensorLaser2d"
name: "scanner_front_left"
plugin: "laser"
follow: "parameters/test_laser_params.yaml"
processors:
-
type: "ProcessorOdomIcp"
name: "processorodomicp"
sensor_name: "scanner_front_left"
plugin: "laser"
voting_active: true
min_features_for_keyframe: 10
min_dist: 999
min_angle: 999
min_time: 1
min_error: 999
max_points: 0
follow: "parameters/test_laser_processor.yaml"
#-
# type: "ProcessorLoopClosureIcp"
# name: "processorloopclosureicp"
# sensor_name: "scanner_front_left"
# plugin: "laser"
# time_tolerance: 0.1
# apply_loss_function: true
# keyframe_vote:
# voting_active: false
# recent_frames_ignored: 3
# frames_ignored_after_loop: 0
# max_error_threshold: 0.02
# min_points_percent: 40
# max_loops: 3
# max_candidates: 5
# max_attempts: 15
# candidate_generation: "random" # 'random' or 'tree'
# icp:
# follow: "parameters/csm.yaml"
ROS subscriber:
-
package: "wolf_ros_laser"
type: "SubscriberLaser2d"
topic: "/ana/sensors/scan"
sensor_name: "scanner_front_left"
load_params_from_msg: true
ROS publisher:
-
package: "wolf_ros_node"
type: "PublisherTf"
topic: " "
period: 0.1
follow: "parameters/test_publisher_ros_node.yaml"
-
package: "wolf_ros_node"
type: "PublisherGraph"
topic: "graph"
period: 1
viz_overlapped_factors: true
-
package: "wolf_ros_node"
type: "PublisherTrajectory"
topic: "trajectory"
period: 1
frame_id: "map"
-
package: "wolf_ros_laser"
type: "PublisherLaserMap"
topic: "map"
period: 1
map_frame_id: "map"
update_dist_th: 0.05
update_angle_th: 0.05
max_n_cells: 1000000
grid_size: 0.1
p_free: 0.3
p_obst: 0.8
p_free_th: 0.2
p_obst_th: 0.9
discard_max_range: true
verbose: false # prints debug messages
# ALGORITHM OPTIONS
use_point_to_line_distance: true # use PlICP (true) or use vanilla ICP (false).
max_angular_correction_deg: 180 # Maximum angular displacement between scans (deg)
max_linear_correction: 10 # Maximum translation between scans (m)
max_correspondence_dist: 0.5 # Maximum distance for a correspondence to be valid
use_corr_tricks: true # Use smart tricks for finding correspondences. Only influences speed; not convergence.
debug_verify_tricks: false # Checks that find_correspondences_tricks give the right answer
# STOP CRITERIA
max_iterations: 50 # maximum iterations
epsilon_xy: 1e-4 # distance change
epsilon_theta: 1e-3 # angle change
# RESTART
restart: false # Restart algorithm
restart_threshold_mean_error: 0 # Threshold for restarting
restart_dt: 0 # Displacement for restarting
restart_dtheta: 0 # Displacement for restarting
# DISCARDING POINTS/CORRESPONDENCES
min_reading: 0.023 # discard rays outside of this interval
max_reading: 60 # discard rays outside of this interval
outliers_maxPerc: 0.9 # Percentage of correspondences to consider: if 0.9, always discard the top 10% of correspondences with more error
outliers_adaptive_order: 0.7 # Parameters describing a simple adaptive algorithm for discarding
outliers_adaptive_mult: 1.5 # Parameters describing a simple adaptive algorithm for discarding
outliers_remove_doubles: false # Do not allow two different correspondences to share a point
do_visibility_test: false # If initial guess, visibility test can discard points that are not visible
do_alpha_test: false # Discard correspondences based on the angles
do_alpha_test_thresholdDeg: 10 #
# POINT ORIENTATION
clustering_threshold: 0.5 # Max-distance clustering for point orientation
orientation_neighbourhood: 4 # Number of neighbour rays used to estimate the orientation
# WEIGHTS
use_ml_weights: false # weight the impact of each correspondence. This works fabolously if the first scan has no noise.
use_sigma_weights: false # If the field "readings_sigma" is used to weight the correspondence by 1/sigma^2
sigma: 0.2 # Noise of the scan
# COVARIANCE
do_compute_covariance: true
cov_factor: 5 # Factor multiplying the cov output of csm
verbose: false # prints debug messages
# ALGORITHM OPTIONS
use_point_to_line_distance: true # use PlICP (true) or use vanilla ICP (false).
max_angular_correction_deg: 180 # Maximum angular displacement between scans (deg)
max_linear_correction: 10 # Maximum translation between scans (m)
max_correspondence_dist: 0.5 # Maximum distance for a correspondence to be valid
use_corr_tricks: true # Use smart tricks for finding correspondences. Only influences speed; not convergence.
debug_verify_tricks: false # Checks that find_correspondences_tricks give the right answer
# STOP CRITERIA
max_iterations: 50 # maximum iterations
epsilon_xy: 1e-4 # distance change
epsilon_theta: 1e-3 # angle change
# RESTART
restart: false # Restart algorithm
restart_threshold_mean_error: 0 # Threshold for restarting
restart_dt: 0 # Displacement for restarting
restart_dtheta: 0 # Displacement for restarting
# DISCARDING POINTS/CORRESPONDENCES
min_reading: 0.023 # discard rays outside of this interval
max_reading: 60 # discard rays outside of this interval
outliers_maxPerc: 0.9 # Percentage of correspondences to consider: if 0.9, always discard the top 10% of correspondences with more error
outliers_adaptive_order: 0.7 # Parameters describing a simple adaptive algorithm for discarding
outliers_adaptive_mult: 1.5 # Parameters describing a simple adaptive algorithm for discarding
outliers_remove_doubles: false # Do not allow two different correspondences to share a point
do_visibility_test: false # If initial guess, visibility test can discard points that are not visible
do_alpha_test: false # Discard correspondences based on the angles
do_alpha_test_thresholdDeg: 10 #
# POINT ORIENTATION
clustering_threshold: 0.5 # Max-distance clustering for point orientation
orientation_neighbourhood: 4 # Number of neighbour rays used to estimate the orientation
# WEIGHTS
use_ml_weights: false # weight the impact of each correspondence. This works fabolously if the first scan has no noise.
use_sigma_weights: false # If the field "readings_sigma" is used to weight the correspondence by 1/sigma^2
sigma: 0.2 # Noise of the scan
# COVARIANCE
do_compute_covariance: true
cov_factor: 9999999999999999999999
minimizer: LEVENBERG_MARQUARDT
max_num_iterations: 100
verbose: 0
period: 0.0
n_threads: 2
interrupt_on_problem_change: false
min_num_iterations: 5 #if update immediately
compute_cov: false
cov_enum: 3 # if compute_cov
cov_period: 1 # if compute_cov
function_tolerance: 1e-9
gradient_tolerance: 1e-10
use_nonmonotonic_steps: false
#bno
extrinsic:
pose: [0,0,0]
a_noise: 0.096 #best: 0.96
w_noise: 0.02
ab_initial_stdev: 5 # m/s2 - initial bias
wb_initial_stdev: 1 # rad/sec - initial bias
ab_rate_stdev: 0.0001 # m/s2/sqrt(s)
wb_rate_stdev: 0.0001 # rad/s/sqrt(s)
orthogonal_gravity: true
#microstrain
extrinsic:
pose: [0,0,0]
a_noise: 0.05 #0.9
w_noise: 0.01 #0.01
ab_initial_stdev: 0.5 # m/s2 - initial bias
wb_initial_stdev: 0.1 # rad/sec - initial bias
ab_rate_stdev: 0.00001 # m/s2/sqrt(s)
wb_rate_stdev: 0.00001 # rad/s/sqrt(s)
orthogonal_gravity: true
time_tolerance: 0.008 #Half the sample time, bno
unmeasured_perturbation_std: 0.1
keyframe_vote:
voting_active: false
max_time_span: 1
max_buff_length: 1000000
dist_traveled: 10
angle_turned: 3.14
state_getter: true
state_priority: 1
apply_loss_function: false
use_gravity_grid: false
time_tolerance: 0.005 #Half the sample time, microstrain
unmeasured_perturbation_std: 0.1
keyframe_vote:
voting_active: false
max_time_span: 1
max_buff_length: 1000000
dist_traveled: 10
angle_turned: 3.14
state_getter: true
state_priority: 0
apply_loss_function: false
use_gravity_grid: false
#bno:
dt: 0.016
imu_x_axis: 3
imu_y_axis: -2
imu_z_axis: 1
cov_source: "sensor"
in_degrees: true
#all
topic_enable: "/helena/sensors/bno055_imu/imu/topic_enable"
static_init_duration: 5
lowpass_filter: true
lowpass_cutoff_freq: 5
#microstrain:
dt: 0.01
imu_x_axis: 1
imu_y_axis: -2
imu_z_axis: -3
cov_source: "sensor"
in_degrees: false
#all
topic_enable: "/helena/sensors/bno055_imu/imu/topic_enable"
static_init_duration: 3
lowpass_filter: false
lowpass_cutoff_freq: 5
extrinsic:
pose: [0,0,3.1415926535] #Last value was pi
LaserScanParams:
angle_min: -3.14159274101
angle_max: 3.14159274101
angle_step: 0.007 #0.00700000021607
scan_time: 0.0
range_min: 0.0
range_max: 200
range_std_dev: 0.2
angle_std_dev: 0.01
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment