Upload fortis_unity_ws setup

.gitignore

+.catkin_tools
+.vscode
+build
+logs
+devel
+src/*
+
+

README.md

 # fortis_unity_ws
 
-Development workspace for  fortis project with unity sample
\ No newline at end of file
+Development workspace for fortis project with unity sample
+
+# Requirements:
+
+## install:
+
+#### install GTSAM lib for LIO-SAM
+
+`sudo apt-get install -y software-properties-common`
+
+`sudo add-apt-repository ppa:borglab/gtsam-release-4.0`
+
+```bash
+sudo apt install -y \
+        libgtsam-dev \
+        libgtsam-unstable-dev
+```
+
+#### install Move Base Flex dependency
+
+`sudo apt install ros-noetic-costmap-2d`
+`sudo apt install ros-noetic-nav-core`
+
+`sudo apt install ros-noetic-move-base-flex`
+
+#### install Open3D
+
+# Execute:
+
+`./tmux/execute_sim.sh`
+
+#### PIP install
+
+tsp_solver2

frames.gv

+digraph G {
+"base_link" -> "manitou_imu"[label="Broadcaster: /unity_endpoint_lucas_Victus_40937_4809900339950455392\nAverage rate: 8.163 Hz\nMost recent transform: 109.720 ( 0.080 sec old)\nBuffer length: 4.900 sec\n"];
+"map" -> "base_link"[label="Broadcaster: /unity_endpoint_lucas_Victus_40937_4809900339950455392\nAverage rate: 10.204 Hz\nMost recent transform: 109.720 ( 0.080 sec old)\nBuffer length: 4.900 sec\n"];
+"base_link" -> "top_left_lidar"[label="Broadcaster: /unity_endpoint_lucas_Victus_40937_4809900339950455392\nAverage rate: 10.204 Hz\nMost recent transform: 109.720 ( 0.080 sec old)\nBuffer length: 4.900 sec\n"];
+"base_link" -> "top_rigth_lidar"[label="Broadcaster: /unity_endpoint_lucas_Victus_40937_4809900339950455392\nAverage rate: 10.204 Hz\nMost recent transform: 109.720 ( 0.080 sec old)\nBuffer length: 4.900 sec\n"];
+"base_link" -> "top_center_lidar"[label="Broadcaster: /unity_endpoint_lucas_Victus_40937_4809900339950455392\nAverage rate: 10.204 Hz\nMost recent transform: 109.720 ( 0.080 sec old)\nBuffer length: 4.900 sec\n"];
+"base_link" -> "camera_frame"[label="Broadcaster: /unity_endpoint_lucas_Victus_40937_4809900339950455392\nAverage rate: 10.204 Hz\nMost recent transform: 109.720 ( 0.080 sec old)\nBuffer length: 4.900 sec\n"];
+"base_link" -> "wheel_BL"[label="Broadcaster: /unity_endpoint_lucas_Victus_40937_4809900339950455392\nAverage rate: 10.204 Hz\nMost recent transform: 109.720 ( 0.080 sec old)\nBuffer length: 4.900 sec\n"];
+"base_link" -> "wheel_BR"[label="Broadcaster: /unity_endpoint_lucas_Victus_40937_4809900339950455392\nAverage rate: 10.204 Hz\nMost recent transform: 109.720 ( 0.080 sec old)\nBuffer length: 4.900 sec\n"];
+"base_link" -> "wheel_FR"[label="Broadcaster: /unity_endpoint_lucas_Victus_40937_4809900339950455392\nAverage rate: 10.204 Hz\nMost recent transform: 109.720 ( 0.080 sec old)\nBuffer length: 4.900 sec\n"];
+"base_link" -> "wheel_FL"[label="Broadcaster: /unity_endpoint_lucas_Victus_40937_4809900339950455392\nAverage rate: 10.204 Hz\nMost recent transform: 109.720 ( 0.080 sec old)\nBuffer length: 4.900 sec\n"];
+"map" -> "odom"[label="Broadcaster: /unity_endpoint_lucas_Victus_40937_4809900339950455392\nAverage rate: 10.204 Hz\nMost recent transform: 109.720 ( 0.080 sec old)\nBuffer length: 4.900 sec\n"];
+edge [style=invis];
+ subgraph cluster_legend { style=bold; color=black; label ="view_frames Result";
+"Recorded at time: 109.800"[ shape=plaintext ] ;
+ }->"map";
+}
\ No newline at end of file

ros_config/dm_run_behavior/fortis_fsm.yaml

+behavior_name: fortis_fsm
+
+args:
+  # Init wait time
+  waiting_time: 1

ros_config/dm_run_behavior/fortis_load_pallet.yaml

+behavior_name: fortis_load_pallet
+
+args:
+  # Init wait time
+  waiting_time: 2
+  n_iterations: 4
+  base_frame: "base_link"
+  offset_front_pallet: 2.5

ros_config/dm_run_behavior/fortis_nav.yaml

+behavior_name: fortis_nav
+
+args:
+  # Init wait time
+  waiting_time: 2
+
+  # # Waypoint navigation action node
+  # waypoint_navigation_topic: waypoint_navigation
+  # waypoint_controller: ""
+  # waypoint_planner: ""
+  # waypoint_recovery_behaviors: ""
+  waypoint_start_index: 10
+  
+  # # Database access
+  # db_dsn: postgresql://root@127.0.0.1:26257/nakama?sslmode=disable
+  # db_storage: storage
+
+  # # Use geographic coordinates as waypoints?
+  ll_coords_data: 0
+  
+  # # (ll_coords_data == false)
+  # db_path_collection: Paths
+  
+  # # (ll_coords_data == true)
+  # db_ll_coord_collection: GeoCoordinates
+  # ll_conv_srv_topic: fromLL
+  # ll_coords_map_frame: map
+
+  # # set datum before executing? 
+  # # This is used to make sure that the dispatched coordinates are relative to the correct 
+  # # geographic reference (saved during the recording of cartesian coordinates).
+  # set_datum: 0
+  # set_datum_srv_topic: set_datum
+
+inputs:
+  waypoints_id: wpn_001 #""
\ No newline at end of file

ros_config/dm_run_behavior/fortis_task.yaml

+behavior_name: fortis_task
+
+args:
+  # Init wait time
+  waiting_time: 10

ros_config/dm_run_behavior/fortis_unload_pallet.yaml

+behavior_name: fortis_unload_pallet
+
+args:
+  base_frame: "base_link"
+  map_frame: "map"

ros_config/dm_run_behavior/record_cartesian_coords.yaml

+behavior_name: record_cartesian_coords
+
+args:
+
+  # Capture info
+  coord_topic: odom
+  num_of_coords: 4
+  min_allowed_trust: 0.0
+  tf_poses: 1
+  tf_poses_frame: map
+  
+  # Database access
+  dsn: postgresql://root@127.0.0.1:26257/nakama?sslmode=disable
+  storage: storage
+  path_collection: Paths
+  create_if_empty: 1
+  overwrite_if_exists: 1
+
+inputs:
+  path_id: wpn_001 #""

ros_config/dm_run_behavior/record_discrete_cartesian_coords.yaml

+behavior_name: record_discrete_cartesian_coords
+
+args:
+
+  # Capture info
+  coord_topic: /waypoints #/odom #/lio_sam/mapping/odometry #ekf/local_odom #ekf/global_odom
+  num_of_coords: 40
+  min_allowed_trust: 0.0
+  tf_poses: 1
+  tf_poses_frame: map
+  
+  # Database access
+  dsn: postgresql://root@127.0.0.1:26257/nakama?sslmode=disable
+  storage: storage
+  path_collection: Paths
+  create_if_empty: 1
+  overwrite_if_exists: 1
+
+inputs:
+  path_id: wpn_001 #""

ros_config/dm_run_behavior/record_geographic_coords.yaml

+behavior_name: record_geographic_coords
+
+args:
+
+  # Capture info
+  navsatfix_topic: /gps/fix
+  num_of_coords: 5 #1
+  min_allowed_trust: 0.9
+  
+  # Database access
+  dsn: postgresql://root@127.0.0.1:26257/nakama?sslmode=disable
+  storage: storage
+  ll_coords_collection: GeoCoordinates
+  create_if_empty: 1
+  overwrite_if_exists: 0
+  
+
+inputs:
+  ll_coords_id: rrt_exploration_vertices #""

ros_config/dm_run_behavior/waypoint_navigation_procedure.yaml

+behavior_name: waypoint_navigation_procedure
+
+args:
+
+  # Waypoint navigation action node
+  waypoint_navigation_topic: waypoint_navigation
+  waypoint_controller: ""
+  waypoint_planner: ""
+  waypoint_recovery_behaviors: ""
+  waypoint_start_index: 0
+  
+  # Database access
+  db_dsn: postgresql://root@127.0.0.1:26257/nakama?sslmode=disable
+  db_storage: storage
+
+  # Use geographic coordinates as waypoints?
+  ll_coords_data: 0
+  
+  # (ll_coords_data == false)
+  db_path_collection: Paths
+  
+  # (ll_coords_data == true)
+  db_ll_coord_collection: GeoCoordinates
+  ll_conv_srv_topic: fromLL
+  ll_coords_map_frame: map
+
+  # set datum before executing? 
+  # This is used to make sure that the dispatched coordinates are relative to the correct 
+  # geographic reference (saved during the recording of cartesian coordinates).
+  set_datum: 0
+  set_datum_srv_topic: set_datum
+
+inputs:
+  waypoints_id: wpn_001 #""

ros_config/loc_lio_sam.yaml

+lio_sam:
+  # Topics
+  pointCloudTopic: "front_lslidar_point_cloud_filtered" # "back_lslidar_point_cloud_filtered" # "front_lslidar_point_cloud_filtered" "fused_point_cloud"  # Point cloud data
+  imuTopic: "imu/wit/imu" # IMU data
+  odomTopic: "odom" # IMU pre-preintegration odometry, same frequency as IMU
+  gpsTopic: "odometry/gps" # GPS odometry topic from navsat, see module_navsat.launch file
+
+  # Topics - Ingeniarius' version at: https://gitlab.ingeniarius.pt/ingeniarius_common/perception/lio-sam.git
+  gpsFixTopic: "/loader/ugv1/gps/fix" # GPS navsatfix topic from sensor, used to determine wether to apply GPS factor or not
+  gpsFixFilteredTopic: "" # Its subscribed but not used for anything yet
+
+  # Frames
+  lidarFrame: "base_link"
+  baselinkFrame: "base_link"
+  odometryFrame: "odom"
+  mapFrame: "map"
+
+  # Frames - Ingeniarius
+  mapFrameAsChild: false # Pass the map_frame as child of base_link, following the CTU MRS approach (https://ctu-mrs.github.io/docs/system/frames_of_reference.html)
+
+  # Ingeniarius' version at: https://gitlab.ingeniarius.pt/ingeniarius_common/perception/lio-sam.git
+  liosamMode: 0 # LIO-SAM Mode 0: SLAM, Mode 1: Localization, Mode 2: Application
+  gpsInitialLocalization: true
+  gpsUpatedDistFix: 2.0 # Min update distance for liosam update is mapframe went it have GPSfix, status = 2
+
+  # GPS Settings
+  useImuHeadingInitialization: true # if using GPS data, set to "true"
+  useGpsElevation: true # if GPS elevation is bad, set to "false"
+  gpsCovThreshold: 2.0 # m^2, threshold for using GPS data
+  poseCovThreshold: 25.0 # m^2, threshold for using GPS data
+
+  # Export settings
+  savePCD: false # https://github.com/TixiaoShan/LIO-SAM/issues/3
+  savePCDDirectory: "/scliosam/data/" # use global path, and end with "/". Warning: if you have already data in the above savePCDDirectory, it will all remove and remake them. Thus, backup is recommended if pre-made data exist.
+
+  # Ingeniarius' version at: https://gitlab.ingeniarius.pt/ingeniarius_common/perception/lio-sam.git
+  eraseLOAMFolder: false
+  saveRawCloud: false
+  graphTest: false # Save factor graph drawing
+  saveGTSAM: false # Save graph
+  saveGTSAMDirectory: "gtsam/" # Directory of gtsam
+  # Analysis Localization
+  buildConfusionMatrix: false # Build a confusion matrix
+  savePCDDirectory_Q: "/scliosam/" # Directory to save the matrix
+  # Quality of localization
+  covarianceMedium: 1.0
+  covarianceHigh: 2.0
+
+  # Sensor Settings
+  sensor: velodyne # lidar sensor type, 'velodyne' or 'ouster' or 'livox' or 'leishen'
+  N_SCAN: 16 # number of lidar channel (i.e., 16, 32, 64, 128)
+  Horizon_SCAN: 2000 # lidar horizontal resolution (Velodyne:1800, Ouster:512,1024,2048)
+  downsampleRate: 1 # default: 1. Downsample your data if too many points. i.e., 16 = 64 / 4, 16 = 16 / 1
+  lidarMinRange: 1.0 # default: 1.0, minimum lidar range to be used
+  lidarMaxRange: 1000.0 # default: 1000.0, maximum lidar range to be used
+
+  # IMU Settings
+  imuAccNoise: 3.9939570888238808e-03
+  imuGyrNoise: 1.5636343949698187e-03
+  imuAccBiasN: 6.4356659353532566e-05
+  imuGyrBiasN: 3.5640318696367613e-05
+  imuGravity: 9.80511
+  imuRPYWeight: 0.01
+
+  # Extrinsics (lidar -> IMU)
+  extrinsicTrans: [0.0, 0.0, 0.0]
+  extrinsicRot: [1, 0, 0, 0, 1, 0, 0, 0, 1]
+  extrinsicRPY: [1, 0, 0, 0, 1, 0, 0, 0, 1]
+
+  # LOAM feature threshold
+  edgeThreshold: 1.0
+  surfThreshold: 0.1
+  edgeFeatureMinValidNum: 10
+  surfFeatureMinValidNum: 100
+
+  # voxel filter paprams
+  odometrySurfLeafSize: 0.2 # default: 0.4 - outdoor, 0.2 - indoor
+  mappingCornerLeafSize: 0.1 # default: 0.2 - outdoor, 0.1 - indoor
+  mappingSurfLeafSize: 0.2 # default: 0.4 - outdoor, 0.2 - indoor
+
+  # robot motion constraint (in case you are using a 2D robot)
+  z_tollerance: 1000 # meters
+  rotation_tollerance: 1000 # radians
+
+  # CPU Params
+  numberOfCores: 4 # number of cores for mapping optimization
+  mappingProcessInterval: 0.15 # seconds, regulate mapping frequency
+
+  # Surrounding map
+  surroundingkeyframeAddingDistThreshold: 1.0 # meters, regulate keyframe adding threshold
+  surroundingkeyframeAddingAngleThreshold: 0.2 # radians, regulate keyframe adding threshold
+  surroundingKeyframeDensity: 2.0 # meters, downsample surrounding keyframe poses
+  surroundingKeyframeSearchRadius: 50.0 # meters, within n meters scan-to-map optimization (when loop closure disabled)
+
+  # Loop closure
+  loopClosureEnableFlag: true
+  loopClosureFrequency: 1.0 # Hz, regulate loop closure constraint add frequency
+  surroundingKeyframeSize: 50 # submap size (when loop closure enabled)
+  historyKeyframeSearchRadius: 15.0 # meters, key frame that is within n meters from current pose will be considerd for loop closure
+  historyKeyframeSearchTimeDiff: 30.0 # seconds, key frame that is n seconds older will be considered for loop closure
+  historyKeyframeSearchNum: 25 # number of hostory key frames will be fused into a submap for loop closure
+  historyKeyframeFitnessScore: 0.3 # icp threshold, the smaller the better alignment
+
+  # Ingeniarius' version at: https://gitlab.ingeniarius.pt/ingeniarius_common/perception/lio-sam.git
+  performRSLoopClosureFlag: true # Perform RS Loop Closure
+  performSCLoopClosureFlag: false # Perform SC Loop Closure
+
+  # Visualization
+  globalMapVisualizationSearchRadius: 1000.0 # meters, global map visualization radius
+  globalMapVisualizationPoseDensity: 1.0 #10.0 # meters, global map visualization keyframe density
+  globalMapVisualizationLeafSize: 0.4 #0.20 # meters, global map visualization cloud density
+
+  # Ingeniarius' version at: https://gitlab.ingeniarius.pt/ingeniarius_common/perception/lio-sam.git
+  # ICP Localization
+  icpLocalizationStep: 1 # Step
+  icpLocalizationAscendingOrder: false # Ascending order
+  icpLocalizationPreliminary: false # ICP Preliminary Localization
+  # PCD Localization
+  pcdInitialLocalization: false # Initial localization using PCD
+  pcdLocalizationThreshold: 0.0
+  # SCD Localization
+  scdInitialLocalization: true # Initial localization using SCD
+  scdPCNumRing: 20
+  scdPCNumSector: 60
+  scdPCMaxRadius: 80.0
+  scdInput: single_scan # single_scan single_feature multi_scan
+
+  numberGpsOdometryMessageToInitialize: 30 # minimum number of navsat's odometry messages in order to initialize the GPS factor
+
+  # Relocalization
+  relocalizeMode: 3 # 0 - Rviz, 1 - trajectory, 2 - searching
+  relocalizeThreadFrequency: 1.0 # the frequency of the relocalization thread logic
+  relocalizeGlobalTimeout: 10.0 # global relocalization timeout. The period at which a better localization for the odom frame is looked.
+  relocalizeHistoryKeyframeFitnessScore: 1.0 # the fitness score threshold for relocalization.
+  setMaxCorrespondenceDistance:
+    100 # Set the maximum distance threshold between two correspondent points in source <-> target
+    # If the distance is larger than this threshold, the points will be ignored in the alignment process
+  setMaximumIterations: 100 # Set the maximum number of iterations the internal optimization should run for
+  setTransformationEpsilon: 0.000001 # The maximum difference between two consecutive transformations in order to consider convergence
+  setEuclideanFitnessEpsilon:
+    0.000001 # Set the maximum allowed Euclidean error between two consecutive steps in the ICP loop, before the
+    # algorithm is considered to have converged. The error is estimated as the sum of the differences
+    # between correspondences in an Euclidean sense, divided by the number of correspondences;
+  setRANSACIterations: 0 # Set the number of iterations RANSAC should run for.
+  numberOfAttempts: 10
+  useICPscanMatchGlobal: false
+
+  initMapGPS: false
+  numFramesEKF: 10
+
+# Navsat (convert GPS coordinates to Cartesian)
+navsat:
+  frequency: 50
+  wait_for_datum: true
+  delay: 0.0
+  magnetic_declination_radians: 0
+  yaw_offset: 0
+  zero_altitude: true
+  altitude: 0.0
+  broadcast_utm_transform: false
+  broadcast_utm_transform_as_parent_frame: false
+  publish_filtered_gps: true
+
+# EKF for Navsat
+ekf_gps:
+  publish_tf: false
+  map_frame: map
+  odom_frame: odom
+  base_link_frame: base_link
+  world_frame: map
+
+  frequency: 50
+  two_d_mode: false
+  sensor_timeout: 0.01
+  # -------------------------------------
+  # External IMU:
+  # -------------------------------------
+  imu0: "imu/wit/imu"
+  # make sure the input is aligned with ROS REP105. "imu_correct" is manually transformed by myself. EKF can also transform the data using tf between your imu and base_link
+  imu0_config:
+    [
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      true,
+      true,
+      true,
+      true,
+    ]
+  imu0_differential: false
+  imu0_queue_size: 50
+  imu0_remove_gravitational_acceleration: true
+  # -------------------------------------
+  # Odometry (From Navsat):
+  # -------------------------------------
+  odom0: odometry/gps
+  odom0_config:
+    [
+      true,
+      true,
+      true,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+      false,
+    ]
+  odom0_differential: false
+  odom0_queue_size: 10
+
+  #                            x     y     z     r     p     y   x_dot  y_dot  z_dot  r_dot p_dot y_dot x_ddot y_ddot z_ddot
+  process_noise_covariance:
+    [
+      1.0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      1.0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      10.0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0.03,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0.03,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0.1,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0.25,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0.25,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0.04,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0.01,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0.01,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0.5,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0.01,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0.01,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0,
+      0.015,
+    ]
+
+  # initial_estimate_covariance: [1e-1, 0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
+  #                               0,    1e-1, 0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
+  #                               0,    0,    1e-1, 0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
+  #                               0,    0,    0,    1e-1, 0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
+  #                               0,    0,    0,    0,    1e-1, 0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
+  #                               0,    0,    0,    0,    0,    1e-1, 0,    0,    0,    0,     0,     0,     0,    0,    0,
+  #                               0,    0,    0,    0,    0,    0,    1e-1, 0,    0,    0,     0,     0,     0,    0,    0,
+  #                               0,    0,    0,    0,    0,    0,    0,    1e-1, 0,    0,     0,     0,     0,    0,    0,
+  #                               0,    0,    0,    0,    0,    0,    0,    0,    1e-1, 0,     0,     0,     0,    0,    0,
+  #                               0,    0,    0,    0,    0,    0,    0,    0,    0,    1e-1,  0,     0,     0,    0,    0,
+  #                               0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     1e-1,  0,     0,    0,    0,
+  #                               0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     1e-1,  0,    0,    0,
+  #                               0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     1e-1, 0,    0,
+  #                               0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    1e-1, 0,
+  #                               0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    1e-1]

ros_config/nav_move_base_flex/common_costmap_params.yaml

+# footprint: [[0.94, -0.84], [0.94, 0.84], [-1.66, 0.84], [-1.66, -0.84]] # without tool
+# footprint: [[1.59, -0.84], [1.59, 0.84], [-1.66, 0.84], [-1.66, -0.84]] # with fork
+footprint: [[2.7, -0.84], [2.7, 0.84], [-1.66, 0.84], [-1.66, -0.84]] # with tool 2.30
+footprint_padding: 0.0
+make_plan_clear_costmap: false # parameters of make_plan service
+make_plan_add_unreachable_goal: false # true, parameters of make_plan service
+
+global_static_layer:
+  enabled: true # Whether to apply this plugin or not
+  first_map_only: false # Only subscribe to the first message on the map topic, ignoring all subsequent messages
+  unknown_cost_value: -1 # The value for which a cost should be considered unknown when reading in a map from the map server. If the costmap is not tracking unknown space, costs of this value will be considered occupied. A value of zero also results in this parameter being unused.
+  lethal_cost_threshold: 100 # The threshold value at which to consider a cost lethal when reading in a map from the map server.
+  subscribe_to_updates: true # In addition to map_topic, also subscribe to map_topic + "_updates"
+  track_unknown_space: true # If true, unknown values in the map messages are translated directly to the layer. Otherwise, unknown values in the map message are translated as FREE_SPACE in the layer.
+  use_maximum: false # Only matters if the static layer is not the bottom layer. If true, only the maximum value will be written to the master costmap
+  trinary_costmap: true # If true, translates all map message values to NO_INFORMATION/FREE_SPACE/LETHAL_OBSTACLE (three values). If false, a full spectrum of intermediate values is possible.
+  map_topic: /loader/ugv1/lio_sam/mapping/map_global/map #The topic that the costmap subscribes to for the static map. This parameter is useful when you have multiple costmap instances within a single node that you want to use different static maps.
+
+global_inflation_layer:
+  enabled: true # Whether to apply this plugin or not
+  cost_scaling_factor: 3.0 # exponential rate at which the obstacle cost drops off (default: 10)
+  inflation_radius: 3.0 # 1.0m out of 1.6m of robot's width (the robot is actually wider)
+
+global_obstacle_layer_0:
+  enabled: true
+  voxel_decay: 10 # seconds if linear, e^n if exponential
+  decay_model: 0 # 0=linear, 1=exponential, -1=persistent
+  voxel_size: 0.05 # meters
+  track_unknown_space: true # default space is known
+  mark_threshold: 0 # voxel height
+  update_footprint_enabled: true
+  combination_method: 1 # 1=max, 0=override
+  origin_z: 0.0 # meters
+  publish_voxel_map: true # default off
+  transform_tolerance: 0.6 # seconds
+  mapping_mode: false # default off, saves map not for navigation
+  map_save_duration: 60 # default 60s, how often to autosave
+  observation_sources: pc1_mark pc1_clear
+  pc1_mark:
+    data_type: PointCloud2
+    topic: /loader/ugv1/lio_sam/deskew/cloud_deskewed
+    marking: true
+    clearing: false
+    obstacle_range: 3.0 # meters
+    min_obstacle_height: 0.3 # default 0, meters
+    max_obstacle_height: 2.0 # default 3, meters
+    expected_update_rate: 0.0 # default 0, if not updating at this rate at least, remove from buffer
+    observation_persistence: 0.0 # default 0, use all measurements taken during now-value, 0=latest
+    inf_is_valid: false # default false, for laser scans
+    filter: "passthrough" # default passthrough, apply "voxel", "passthrough", or no filter to sensor data, recommend on
+    voxel_min_points: 0 # default 0, minimum points per voxel for voxel filter
+    clear_after_reading: true # default false, clear the buffer after the layer gets readings from it
+  pc1_clear:
+    data_type: PointCloud2
+    topic: /loader/ugv1/lio_sam/deskew/cloud_deskewed
+    marking: false
+    clearing: true
+    max_z: 7.0 # default 0, meters
+    min_z: 0.1 # default 10, meters (the back lidar has 4m blind spot)
+    vertical_fov_angle: 0.4 #523   # default 0.7, radians. For 3D lidars it's the symmetric FOV about the planar axis.
+    vertical_fov_padding: 0.05 # 3D Lidar only. Default 0, in meters
+    horizontal_fov_angle: 6.29 # 3D lidar scanners like the VLP16 have 360 deg horizontal FOV.
+    decay_acceleration: 1.0 # default 0, 1/s^2.
+    model_type: 1 # default 0, model type for frustum. 0=depth camera, 1=3d lidar like VLP16 or similar
+
+global_obstacle_layer_1:
+  enabled: false
+  voxel_decay: 60 # seconds if linear, e^n if exponential
+  decay_model: 0 # 0=linear, 1=exponential, -1=persistent
+  voxel_size: 0.2 # meters
+  track_unknown_space: true # default space is known
+  max_obstacle_height: 2.5 # meters
+  unknown_threshold: 15 # voxel height
+  mark_threshold: 3 # voxel height
+  update_footprint_enabled: true
+  combination_method: 0 # 1=max, 0=override
+  obstacle_range: 5.0 # meters
+  origin_z: 0.0 # meters
+  publish_voxel_map: false # default off
+  transform_tolerance: 0.6 # seconds
+  mapping_mode: false # default off, saves map not for navigation
+  map_save_duration: 60 # default 60s, how often to autosave
+  observation_sources: pc2_mark pc2_clear
+  pc2_mark:
+    data_type: PointCloud2
+    topic: /traversability_map_visualization/traversability_cloud/passed
+    marking: true
+    clearing: false
+    min_obstacle_height: 0.0 # default 0, meters (initial: 0.05)
+    max_obstacle_height: 2.5 # default 3, meters
+    expected_update_rate: 0.0 # default 0, if not updating at this rate at least, remove from buffer
+    observation_persistence: 0.0 # default 0, use all measurements taken during now-value, 0=latest
+    inf_is_valid: false # default false, for laser scans
+    filter: "passthrough" # default passthrough, apply "voxel", "passthrough", or no filter to sensor data, recommend on
+    voxel_min_points: 0 # default 0, minimum points per voxel for voxel filter
+    clear_after_reading: true # default false, clear the buffer after the layer gets readings from it
+  pc2_clear:
+    enabled: true #default true, can be toggled on/off with associated service call
+    data_type: PointCloud2
+    topic: /traversability_map_visualization/traversability_cloud/passed
+    marking: false
+    clearing: true
+    max_z: 5.0 # default 0, meters
+    min_z: 0.0 # default 10, meters (the back lidar has 4m blind spot)
+    vertical_fov_angle: 0.523 # default 0.7, radians. For 3D lidars it's the symmetric FOV about the planar axis.
+    vertical_fov_padding: 0.05 # 3D Lidar only. Default 0, in meters
+    horizontal_fov_angle: 6.29 # 3D lidar scanners like the VLP16 have 360 deg horizontal FOV.
+    decay_acceleration: 5.0 # default 0, 1/s^2.
+    model_type: 1 # default 0, model type for frustum. 0=depth camera, 1=3d lidar like VLP16 or similar
+
+local_static_layer_0:
+  enabled: true
+  first_map_only: false
+  unknown_cost_value: -1
+  lethal_cost_threshold: 100
+  subscribe_to_updates: true
+  track_unknown_space: true
+  use_maximum: false
+  trinary_costmap: true
+  map_topic: /loader/ugv1/lio_sam/mapping/map_global/map
+
+local_static_layer_1:
+  enabled: true
+  first_map_only: false
+  unknown_cost_value: -1
+  lethal_cost_threshold: 100
+  subscribe_to_updates: false
+  track_unknown_space: true
+  use_maximum: false
+  trinary_costmap: true
+  map_topic: /traversability_map_visualization/traversability_map
+
+local_inflation_layer:
+  enabled: true # Whether to apply this plugin or not
+  cost_scaling_factor: 10 # exponential rate at which the obstacle cost drops off (default: 10)
+  inflation_radius: 1.0 # max. distance from an obstacle at which costs are incurred for planning paths (min » costmap resolution)
+
+local_obstacle_layer_0:
+  enabled: true
+  voxel_decay: 2 # seconds if linear, e^n if exponential
+  decay_model: 0 # 0=linear, 1=exponential, -1=persistent
+  voxel_size: 0.1 # meters
+  track_unknown_space: true # default space is known
+  max_obstacle_height: 5.0 # meters
+  unknown_threshold: 15 # voxel height
+  mark_threshold: 3 # voxel height
+  update_footprint_enabled: true
+  combination_method: 1 # 1=max, 0=override
+  obstacle_range: 10.0 # meters
+  origin_z: 0.0 # meters
+  publish_voxel_map: false # default off
+  transform_tolerance: 0.6 # seconds
+  mapping_mode: false # default off, saves map not for navigation
+  map_save_duration: 60 # default 60s, how often to autosave
+  observation_sources: pc1_mark pc1_clear
+  pc1_mark:
+    data_type: PointCloud2
+    topic: /loader/ugv1/lio_sam/deskew/cloud_deskewed
+    marking: true
+    clearing: false
+    min_obstacle_height: 0.2 # default 0, meters
+    max_obstacle_height: 2.0 # default 3, meters
+    expected_update_rate: 0.0 # default 0, if not updating at this rate at least, remove from buffer
+    observation_persistence: 0.0 # default 0, use all measurements taken during now-value, 0=latest
+    inf_is_valid: false # default false, for laser scans
+    filter: "passthrough" # default passthrough, apply "voxel", "passthrough", or no filter to sensor data, recommend on
+    voxel_min_points: 0 # default 0, minimum points per voxel for voxel filter
+    clear_after_reading: true # default false, clear the buffer after the layer gets readings from it
+  pc1_clear:
+    enabled: true #default true, can be toggled on/off with associated service call
+    data_type: PointCloud2
+    topic: /loader/ugv1/lio_sam/deskew/cloud_deskewed
+    marking: false
+    clearing: true
+    max_z: 10.0 # default 0, meters
+    min_z: 0.0 # default 10, meters (the back lidar has 4m blind spot)
+    vertical_fov_angle: 0.523 # default 0.7, radians. For 3D lidars it's the symmetric FOV about the planar axis.
+    vertical_fov_padding: 0.05 # 3D Lidar only. Default 0, in meters
+    horizontal_fov_angle: 6.29 # 3D lidar scanners like the VLP16 have 360 deg horizontal FOV.
+    decay_acceleration: 5.0 # default 0, 1/s^2.
+    model_type: 1 # default 0, model type for frustum. 0=depth camera, 1=3d lidar like VLP16 or similar
+
+local_obstacle_layer_1:
+  enabled: false
+  voxel_decay: 15 # seconds if linear, e^n if exponential
+  decay_model: 0 # 0=linear, 1=exponential, -1=persistent
+  voxel_size: 0.2 # meters
+  track_unknown_space: true # default space is known
+  max_obstacle_height: 2.5 # meters
+  unknown_threshold: 15 # voxel height
+  mark_threshold: 3 # voxel height
+  update_footprint_enabled: true
+  combination_method: 0 # 1=max, 0=override
+  obstacle_range: 5.0 # meters
+  origin_z: 0.0 # meters
+  publish_voxel_map: false # default off
+  transform_tolerance: 0.6 # seconds
+  mapping_mode: false # default off, saves map not for navigation
+  map_save_duration: 60 # default 60s, how often to autosave
+  observation_sources: pc2_mark pc2_clear
+  pc2_mark:
+    data_type: PointCloud2
+    topic: /traversability_map_visualization/traversability_cloud/passed
+    marking: true
+    clearing: false
+    min_obstacle_height: 0.0 # default 0, meters (initial: 0.05)
+    max_obstacle_height: 2.5 # default 3, meters
+    expected_update_rate: 0.0 # default 0, if not updating at this rate at least, remove from buffer
+    observation_persistence: 0.0 # default 0, use all measurements taken during now-value, 0=latest
+    inf_is_valid: false # default false, for laser scans
+    filter: "passthrough" # default passthrough, apply "voxel", "passthrough", or no filter to sensor data, recommend on
+    voxel_min_points: 0 # default 0, minimum points per voxel for voxel filter
+    clear_after_reading: true # default false, clear the buffer after the layer gets readings from it
+  pc2_clear:
+    enabled: true #default true, can be toggled on/off with associated service call
+    data_type: PointCloud2
+    topic: /traversability_map_visualization/traversability_cloud/passed
+    marking: false
+    clearing: true
+    max_z: 5.0 # default 0, meters
+    min_z: 0.0 # default 10, meters (the back lidar has 4m blind spot)
+    vertical_fov_angle: 0.523 # default 0.7, radians. For 3D lidars it's the symmetric FOV about the planar axis.
+    vertical_fov_padding: 0.05 # 3D Lidar only. Default 0, in meters
+    horizontal_fov_angle: 6.29 # 3D lidar scanners like the VLP16 have 360 deg horizontal FOV.
+    decay_acceleration: 5.0 # default 0, 1/s^2.
+    model_type: 1 # default 0, model type for frustum. 0=depth camera, 1=3d lidar like VLP16 or similar

ros_config/nav_move_base_flex/global_costmap_params.yaml

+global_costmap:
+  global_frame: map
+  robot_frame: base_link
+  robot_base_frame: base_link
+  transform_tolerance: 1.0
+  update_frequency: 2.0
+  publish_frequency: 2.0
+  rolling_window: false
+  width: 30.0
+  height: 30.0
+  resolution: 0.2
+  origin_x: 0.0
+  origin_y: 0.0
+  always_send_full_costmap: true
+
+  plugins:
+    - { name: global_static_layer, type: "costmap_2d::StaticLayer" }
+    - { name: global_inflation_layer, type: "costmap_2d::InflationLayer" }
+    # - { name: global_obstacle_layer_0, type: "spatio_temporal_voxel_layer/SpatioTemporalVoxelLayer" }

ros_config/nav_move_base_flex/local_costmap_params.yaml

+1local_costmap:
+  global_frame: map
+  robot_frame: base_link
+  robot_base_frame: base_link
+  transform_tolerance: 1.0
+  update_frequency: 2.0
+  publish_frequency: 2.0
+  rolling_window: true
+  width: 30.0
+  height: 30.0
+  resolution: 0.2
+  origin_x: 0.0
+  origin_y: 0.0
+  always_send_full_costmap: true
+
+  plugins:
+    - { name: local_static_layer_0, type: "costmap_2d::StaticLayer" }
+    - { name: local_inflation_layer, type: "costmap_2d::InflationLayer" }
+    # - { name: local_obstacle_layer_0, type: "spatio_temporal_voxel_layer/SpatioTemporalVoxelLayer" }
+

ros_config/nav_move_base_flex/mbf_costmap_nav.yaml

+#########################
+### Static Parameters ###
+#########################
+robot_frame: base_link # the frame of the robot, which will be used to determine its position
+map_frame: map          # the global frame the robot is controlling in
+global_frame: map
+# force_stop_at_goal: false   # force move base flex to stop the robot once the goal is reached
+# force_stop_on_cancel: false # force move base flex to stop the robot on navigation cancellation
+# mbf_tolerance_check: false  # force move base flex to check for the goal tolerance
+# dist_tolerance: 0.1     # distance tolerance to the given goal pose
+# angle_tolerance: 0.175  # angle tolerance to the given goal pose
+# tf_timeout: 1.0         # time before a timeout used for tf requests
+
+#################################################
+### Dynamically Reconfigurable MBF Parameters ###
+#################################################
+# Planners 
+planners: # global planner, e.g. navfn/NavfnROS
+  - name: GlobalPlanner
+    type: global_planner/GlobalPlanner
+
+planner_frequency: $(arg planner_frequency) # the rate in Hz at which to run the planning loop
+planner_max_retries: $(arg planner_max_retries) # how many times we will recall the planner in an attempt to find a valid plan before giving up
+planner_patience: $(arg planner_patience) # how long the planner will wait in seconds in an attempt to find a valid plan before giving up
+
+# Controllers
+controllers: # list of controller, e.g. eband_local_planner/EBandPlannerROS
+  - name: TebLocalPlannerROS
+    type: teb_local_planner/TebLocalPlannerROS
+  # - name: GoToPlanner
+  #   type: teb_local_planner/TebLocalPlannerROS
+  # - name: UnloadPallet
+  #   type: teb_local_planner/TebLocalPlannerROS
+
+controller_frequency: $(arg controller_frequency) # the rate in Hz at which to run the control loop and send velocity commands to the base
+controller_max_retries: $(arg controller_max_retries) # how many times we will recall the controller in an attempt to find a valid command before giving up
+controller_patience: $(arg controller_patience) # how long the controller will wait in seconds without receiving a valid control before giving up
+
+# Oscillation
+oscillation_timeout: 0.0 # how far in meters the robot must move to be considered not to be oscillating
+oscillation_distance: 0.5 # how long in seconds to allow for oscillation before executing recovery behaviors
+
+# Other
+recovery_enabled: $(arg recovery_enabled) # enable the move_base_flex recovery behaviors to attempt to clear out space (for some reason the modern name does not work)
+recovery_patience: 15.0 # how much time we allow recovery behaviors to complete before canceling (or stopping if cancel fails).
+restore_defaults: false # restore to the original configuration
+shutdown_costmaps: false # shutdown the costmaps of the node when move_base_flex is in an inactive state.
+shutdown_costmaps_delay: 1.0 # how long in seconds to wait after last action before shutting down the costmaps.
+
+# the remaining parameters do not seem to exist here: http://wiki.ros.org/move_base_flex#Navigation_Servers
+# <param name="switch_controller_keyword_name" value="Docking"/> <!--the recovery will search for controllers with this name inside-->
+
+# NOTES:
+# - parameter sources: https://uos.github.io/mbf_docs/tutorials/beginner/parameters/mbf_parameters/ and
+# https://wiki.ros.org/move_base_flex#Navigation_Servers.
+#
+# - controller_frequency can be set to a lower value (than the 10.0 as default) to prevent
+# an high cmd_vel rate, which caused the robot to some discontinuous movements.
+#
+# - planner_frequency can be set to 0.0 in order to make the global_planner to run only when 
+# a new goal is received(it helps the robot to ignore drift during execution).
+#
+# - legacy "max_planning_retries" = mbf "planner_max_retries".
+#
+# - oscillation_timeout set to 0.0 in order to avoid the robot to get stuck when some oscillations are present.
+#
+# - planner_max_retries, controller_max_retries and recovery_enabled, these parameters were never set explicitly by us 
+# (source: https://wiki.ros.org/move_base_flex#Navigation_Servers and https://uos.github.io/mbf_docs/tutorials/beginner/parameters/mbf_parameters/)
+#
+# - shutdown_costmaps and shutdown_costmaps_delay, never set explecitily set by us before. Allow to shutdown costmaps,
+# when move base is in an inactive state (might be good for optimisation).
+#
+# - legacy "recovery_behavior_enabled" = mbf "recovery_enabled".
+#
+# - there were legacy move base parameters in previous configuration: "conservative_reset_dist" and "clearing_rotation_allowed".
+# Check the relay for more information https://github.com/magazino/move_base_flex/blob/master/mbf_costmap_nav/scripts/move_base_legacy_relay.py.
+#

ros_config/nav_move_base_flex/planners.yaml

+GlobalPlanner:
+  # Lethal Cost (dynamic reconfigure)
+  lethal_cost: 253  # --- default ---
+  # Neutral Cost (dynamic reconfigure)
+  neutral_cost: 50  # --- default ---
+  # Factor to multiply each cost from costmap by (dynamic reconfigure)
+  cost_factor: 3.0  # --- default ---
+  # Specifies whether or not to visualize the potential area computed via a PointCloud2.
+  visualize_potential: true # --- default ---
+  # Publish Potential Costmap (dynamic reconfigure)
+  publish_potential: true # --- default ---
+  # How to set the orientation of each point (dynamic reconfigure)
+  # None=0, Forward=1, Interpolate=2, ForwardThenInterpolate=3, 
+  # Backward=4, Leftward=5, Rightward=6
+  orientation_mode: 0 # --- default ---
+  # What window to use to determine the orientation based on the 
+  # position derivative specified by the orientation mode (dynamic reconfigure) 
+  orientation_window_size: 1  # --- default ---
+  # If for some reason, you want global_planner to exactly 
+  # mirror the behavior of navfn, set this to true 
+  old_navfn_behavior: false # --- default ---
+  # If true, create a path that follows the grid boundaries. 
+  # Otherwise, use a gradient descent method. 
+  use_grid_path: false  # --- default ---
+  # If true, use the quadratic approximation of the potential. 
+  # Otherwise, use a simpler calculation. 
+  use_quadratic: true
+  # If true, use dijkstra's algorithm. 
+  # Otherwise, A*.
+  use_dijkstra: true  # --- default ---
+  # A tolerance on the goal point for the planner. 
+  # The planner will attempt to create a plan that is as close to 
+  # the specified goal as possible but no further than default_tolerance away
+  default_tolerance: 0.0
+  # Specifies whether or not to allow the planner to create plans that traverse unknown space. NOTE: if you are using a layered costmap_2d costmap with a voxel or obstacle layer, you must also set the track_unknown_space param for that layer to be true, or it will convert all your unknown space to free space (which planner will then happily go right through).
+  allow_unknown: false  # *** change ***
+  # Outlines the global costmap with lethal obstacles. 
+  # For the usage of a non static (rolling window) global costmap 
+  # this needs to be set to fals
+  outline_map: true # --- default ---