[sf_carto_bag_debug] Paper tuning

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,

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

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
 

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

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

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]

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
+          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"