From e7f3c401123d80850ffd2dafb5b98876d29b0233 Mon Sep 17 00:00:00 2001 From: Lovro Markovic Date: Wed, 8 Sep 2021 17:39:26 +0200 Subject: [PATCH] [sf_carto_bag_debug] Paper tuning --- src/sensor.h | 7 ++- .../sf_carto_bag_debug/bag_setup_danieli.sh | 1 + .../bag_setup_danieli_localization.sh | 4 +- .../bag_setup_danieli_paper.sh | 11 +++- .../custom_config/ouster_local.lua | 56 +++++++++++++++++++ .../sensor_client_paper_config.yaml | 14 ++--- startup/sf_carto_bag_debug/session.yml | 17 +++++- 7 files changed, 95 insertions(+), 15 deletions(-) create mode 100644 startup/sf_carto_bag_debug/custom_config/ouster_local.lua diff --git a/src/sensor.h b/src/sensor.h index 7e2f6f3..7c33eea 100644 --- a/src/sensor.h +++ b/src/sensor.h @@ -238,7 +238,12 @@ class Sensor // Drift position checks if (estimateDrift()) { // TODO(lmark): Add drifted position outlier - checks.drifted_position_outlier = false; + auto abs_error = (position - m_sensor_transformed_position).cwiseAbs(); + checks.drifted_position_outlier = + abs_error.x() > m_sensor_params.position_outlier_lim.x() + || abs_error.y() > m_sensor_params.position_outlier_lim.y() + || abs_error.z() > m_sensor_params.position_outlier_lim.z(); + if (checks.drifted_position_outlier) { ROS_WARN_STREAM_THROTTLE( 2.0, diff --git a/startup/sf_carto_bag_debug/bag_setup_danieli.sh b/startup/sf_carto_bag_debug/bag_setup_danieli.sh index 66192ff..4efb095 100644 --- a/startup/sf_carto_bag_debug/bag_setup_danieli.sh +++ b/startup/sf_carto_bag_debug/bag_setup_danieli.sh @@ -1,6 +1,7 @@ #!/bin/bash export UAV_NAMESPACE=danieli2 +export TF_PREFIX=danieli2 export BAG_NAME=$HOME/Bags/danieli1_compassless/sensor_fusion_ouster_slow.bag export CONFIGURATION_BASENAME=ouster_pozyx.lua export CONFIGURATION_DIRECTORY=$(pwd)/custom_config diff --git a/startup/sf_carto_bag_debug/bag_setup_danieli_localization.sh b/startup/sf_carto_bag_debug/bag_setup_danieli_localization.sh index 5a2a450..3ea805f 100644 --- a/startup/sf_carto_bag_debug/bag_setup_danieli_localization.sh +++ b/startup/sf_carto_bag_debug/bag_setup_danieli_localization.sh @@ -1,12 +1,12 @@ #!/bin/bash export UAV_NAMESPACE=danieli2 -export BAG_NAME=$HOME/FER/cartographer_experiments/Danieli/Localization/localization_2.bag +export TF_PREFIX=danieli2 +export BAG_NAME=$HOME/Bags/danieli1_compassless/sensor_fusion_ouster_slow.bag export CONFIGURATION_BASENAME=localization_pozyx.lua export CONFIGURATION_DIRECTORY=$(pwd)/custom_config export CARTO_URDF=$(rospack find uav_ros_general)/urdf/ouster-imu.urdf.xacro export SF_CONFIG=$(realpath custom_config/sensor_client_carto_config.yaml) - export PURE_LOCALIZATION=true export PBSTREAM_NAME=$(pwd)/custom_config/hangar_map_1.pbstream diff --git a/startup/sf_carto_bag_debug/bag_setup_danieli_paper.sh b/startup/sf_carto_bag_debug/bag_setup_danieli_paper.sh index bee48d4..4363ed7 100644 --- a/startup/sf_carto_bag_debug/bag_setup_danieli_paper.sh +++ b/startup/sf_carto_bag_debug/bag_setup_danieli_paper.sh @@ -1,11 +1,14 @@ #!/bin/bash export UAV_NAMESPACE=danieli2 -export BAG_NAME=$HOME/Bags/danieli1_compassless/clanak/measurements_2021-09-06-20-17-17.bag -export CONFIGURATION_BASENAME=ouster_pozyx.lua +export TF_PREFIX=red +export CARTO_NAMESPACE=red +export BAG_NAME=/usr/local/games/compassless_t265_carto_pozyx_2021-09-06-16-21-18.bag +export CONFIGURATION_BASENAME=ouster_local.lua export CONFIGURATION_DIRECTORY=$(pwd)/custom_config export CARTO_URDF=$(rospack find uav_ros_general)/urdf/ouster-imu.urdf.xacro export SF_CONFIG=$(realpath custom_config/sensor_client_paper_config.yaml) +export CARTO_POSE=/$UAV_NAMESPACE/pproc/cartographer/pose export PURE_LOCALIZATION=false export PBSTREAM_NAME= @@ -21,10 +24,12 @@ export BAG_IMU_TOPIC=/red/mavros/imu/data export BAG_TRANSFORM_TOPIC=/red/pozyx/measured export BAG_CAMERA_ODOM_TOPIC=/red/camera/odom/sample export BAG_CARTO_TOPIC=/red/uav/cartographer/pose +export BAG_EKF_TOPIC=/red/es_ekf/odom # Define cartographer topics export POINT_TOPIC=/$UAV_NAMESPACE/os_cloud_node/points export IMU_TOPIC=/$UAV_NAMESPACE/mavros/imu/data export TRANSFORM_TOPIC=/$UAV_NAMESPACE/pozyx/measured export CAMERA_ODOM_TOPIC=/$UAV_NAMESPACE/camera/odom/sample -export CARTO_TOPIC=/$UAV_NAMESPACE/uav/cartographer/pose +export CARTO_TOPIC=/unused/uav/cartographer/pose +export EKF_TOPIC=/unused/es_ekf_odom \ No newline at end of file diff --git a/startup/sf_carto_bag_debug/custom_config/ouster_local.lua b/startup/sf_carto_bag_debug/custom_config/ouster_local.lua new file mode 100644 index 0000000..3cb4955 --- /dev/null +++ b/startup/sf_carto_bag_debug/custom_config/ouster_local.lua @@ -0,0 +1,56 @@ +-- Copyright 2016 The Cartographer Authors +-- +-- Licensed under the Apache License, Version 2.0 (the "License"); +-- you may not use this file except in compliance with the License. +-- You may obtain a copy of the License at +-- +-- http://www.apache.org/licenses/LICENSE-2.0 +-- +-- Unless required by applicable law or agreed to in writing, software +-- distributed under the License is distributed on an "AS IS" BASIS, +-- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +-- See the License for the specific language governing permissions and +-- limitations under the License. + +include "ouster_pozyx.lua" +namespace = os.getenv("TF_PREFIX") + +options = { + map_builder = MAP_BUILDER, + trajectory_builder = TRAJECTORY_BUILDER, + map_frame = namespace.."/map", + tracking_frame = namespace.."/base_link", + published_frame = namespace.."/base_link", + odom_frame = namespace.."/odom", + provide_odom_frame = false, + publish_frame_projected_to_2d = false, + use_pose_extrapolator = true, + use_odometry = false, + use_nav_sat = false, + use_position_sensor = false, -- use POZYX + position_translation_weight = 1.2, -- 1e0, ----------------- POZYX + nav_sat_use_predefined_enu_frame = false, + nav_sat_predefined_enu_frame_lat_deg = 45.813902, + nav_sat_predefined_enu_frame_lon_deg = 16.038766, + nav_sat_predefined_enu_frame_alt_m = 168.259294525, + nav_sat_translation_weight = 1., + nav_sat_inverse_covariance_weight = 0.4, + nav_sat_inverse_covariance_bias = 0, + use_landmarks = false, + num_laser_scans = 0, + num_multi_echo_laser_scans = 0, + num_subdivisions_per_laser_scan = 1, + num_point_clouds = 1, + lookup_transform_timeout_sec = 0.2, + submap_publish_period_sec = 0.3, + pose_publish_period_sec = 2e-2, + trajectory_publish_period_sec = 1e0, + rangefinder_sampling_ratio = 1., + odometry_sampling_ratio = 1., + fixed_frame_pose_sampling_ratio = 1., + imu_sampling_ratio = 1., + landmarks_sampling_ratio = 1., +} + +POSE_GRAPH.optimize_every_n_nodes = 0 -- 320 --480 +return options diff --git a/startup/sf_carto_bag_debug/custom_config/sensor_client_paper_config.yaml b/startup/sf_carto_bag_debug/custom_config/sensor_client_paper_config.yaml index e7e35a7..441a110 100644 --- a/startup/sf_carto_bag_debug/custom_config/sensor_client_paper_config.yaml +++ b/startup/sf_carto_bag_debug/custom_config/sensor_client_paper_config.yaml @@ -36,19 +36,19 @@ camera_is_velocity_sensor: false camera_estimate_drift: true camera_topic: camera/odom/sample camera_msg_type: 0 -camera_R_pose: [0.05, 0.05, 0.01] +camera_R_pose: [0.2, 0.2, 0.2] camera_R_angle: [0.2, 0.2, 0.2] camera_rotation: [1,0,0,0,1,0,0,0,1] camera_translation: [0,0,0] camera_origin_at_first_measurement: false -camera_position_outlier_lim: [10, 10, 10] +camera_position_outlier_lim: [1, 1, 2] camera_orientation_outlier_lim: [0, 0, 0] #pozyx -pozyx_R_pose: [1, 1, 3] +pozyx_R_pose: [0.15, 0.15, 0.2] pozyx_R_angle: [0.01, 0.01, 0.01] pozyx_is_orientation_sensor: false -pozyx_estimate_drift: false +pozyx_estimate_drift: true pozyx_is_velocity_sensor: false pozyx_topic: pozyx/measured pozyx_msg_type: 1 @@ -59,15 +59,15 @@ pozyx_position_outlier_lim: [1, 1, 2] pozyx_orientation_outlier_lim: [0, 0, 0] #carto -carto_R_pose: [0.5, 0.5, 1] +carto_R_pose: [0.3, 0.3, 0.5] carto_R_angle: [0.5, 0.5, 0.5] carto_is_orientation_sensor: true carto_is_velocity_sensor: false carto_estimate_drift: true -carto_topic: uav/cartographer/pose +carto_topic: pproc/cartographer/pose carto_msg_type: 2 carto_rotation: [0.0, -1.0, 0.0, 1.0, 0.0, 0, 0, 0.0, 1.0] carto_translation: [-4.048,3.654,0.176] carto_origin_at_first_measurement: true -carto_position_outlier_lim: [10, 10, 10] +carto_position_outlier_lim: [1, 1, 2] carto_orientation_outlier_lim: [0, 0, 0] \ No newline at end of file diff --git a/startup/sf_carto_bag_debug/session.yml b/startup/sf_carto_bag_debug/session.yml index 5b6456d..825a4db 100644 --- a/startup/sf_carto_bag_debug/session.yml +++ b/startup/sf_carto_bag_debug/session.yml @@ -12,12 +12,13 @@ windows: - waitForRos; sleep 5; rosparam set use_sim_time true; rosbag play $BAG_NAME --clock $BAG_POINT_TOPIC:=$POINT_TOPIC $BAG_IMU_TOPIC:=$IMU_TOPIC $BAG_TRANSFORM_TOPIC:=$TRANSFORM_TOPIC $BAG_CAMERA_ODOM_TOPIC:=$CAMERA_ODOM_TOPIC $BAG_CARTO_TOPIC:=$CARTO_TOPIC + $BAG_EKF_TOPIC:=$EKF_TOPIC /tf:=/tf_unused /tf_static:=/tf_static_unused - slam: layout: tiled panes: - waitForRos; rosparam set use_sim_time true; roslaunch uav_ros_general robot_state_publisher.launch - urdf:=$CARTO_URDF + urdf:=$CARTO_URDF namespace:=$TF_PREFIX - waitForRos; rosparam set use_sim_time true; roslaunch uav_ros_general cartographer.launch simulation:=false @@ -32,6 +33,8 @@ windows: transform:=$TRANSFORM_TOPIC cartographer_state_filename:=$PBSTREAM_NAME pure_localization:=$PURE_LOCALIZATION + namespace:=$CARTO_NAMESPACE + carto_pose:=$CARTO_POSE - waitForRos; rosrun cartographer_ros cartographer_occupancy_grid_node __ns:=$UAV_NAMESPACE - waitForRos; sed "s/PLACEHOLDER_NS/$UAV_NAMESPACE/g" custom_config/display_bag.rviz > custom_config/.display_bag.rviz; @@ -67,4 +70,14 @@ windows: panes: - waitForRos; waitFor /$UAV_NAMESPACE/ekf_euler; sed "s/PLACEHOLDER_NS/$UAV_NAMESPACE/g" custom_config/sf_debug.perspective > custom_config/.sf_debug.perspective; - rqt --perspective-file custom_config/.sf_debug.perspective \ No newline at end of file + rqt --perspective-file custom_config/.sf_debug.perspective + - history -s "rosbag record + /danieli2/es_ekf/odom + /danieli2/carto_transformed_pose + /danieli2/carto_state + /danieli2/pozyx_transformed_pose + /danieli2/pozyx_state + /danieli2/camera_transformed_pose + /danilei2/camera_state + /base_link/vrpn_client/estimated_odometry + -o $HOME/Bags/danieli1_compassless/clanak/pprocessed_measurements" \ No newline at end of file