Custom via points are not transformed to correct frame (shifted by map->odom)

[Combinacijus]

ROS Noetic (Images below)

What I do

I publish custom equally spaced via-points between previous and current waypoints (blue line images below) in "map" frame to the /move_base/TebLocalPlannerROS/via_points

Legend

Magenta arrows: **via_points (the ones I publish) Blue cubes teb_markers, ns: ViaPoints (the ones TEB follows) Blue lines: lines between waypoints that need to be followed as close as possible

What's wrong

My published via_points (magenta arrows) stick to the blue line perfectly but teb_markers (blue cubes) are shifted by map->odom transform that means TEB doesn't follow blue line but follow the cubes as show in images below. TebLocalPlannerROS/via_points are used in "odom" frame but are not transformed to "odom" frame, only header frame_id is changed. So via points published in "map" frame that TEB follows inherits accumulated error between map->odom.

What I would like to achieve

I would like that TebLocalPlanner would transform messages of "TebLocalPlannerROS/via_points" from given frame to "odom" frame and therefore robot would follow blue line as intended

Current workaround

To publish custom via points in "map" frame first you need to apply map->odom transform to said via points before publishing.

References for a fix

I couldn't locate where exactly via points transformation should happen but there some anchor points:

This is /move_base/TebLocalPlannerROS/via_points message callback aka the first place where via_points in "map" frame arrives: https://github.com/rst-tu-dortmund/teb_local_planner/blob/f22b10ac977d4c7020ea99bfe11e9db9d2bf48d5/src/teb_local_planner_ros.cpp#L1023

And here via_points with swapped "odom" frame (without transformation) are published as marker for visualization: https://github.com/rst-tu-dortmund/teb_local_planner/blob/f22b10ac977d4c7020ea99bfe11e9db9d2bf48d5/src/visualization.cpp#L337-L343

I couldn't find where these via points are used for navigation tho

Ideas how to fix it

In via_points callback function before appending to via_points_ x and y could be transformed (translation and rotation) from msg header to "odom" frame. Note my use of "odom" frame here I'm not sure about implementation detail on what frame teb planner actually uses is it "odom" or cfg_->map_frame as used for visualization or other https://github.com/rst-tu-dortmund/teb_local_planner/blob/f22b10ac977d4c7020ea99bfe11e9db9d2bf48d5/src/teb_local_planner_ros.cpp#L1035-L1039

Details

/move_base/TebLocalPlannerROS/via_points

header: 
  seq: 5640
  stamp: 
    secs: 1734
    nsecs: 705000000
  frame_id: "map"
poses: 
  - 
    header: 
      seq: 0
      stamp: 
        secs: 1734
        nsecs: 705000000
      frame_id: "map"
    pose: 
      position: teb_markers
        x: 5.790121363547482
        y: -7.767302333976653
        z: 0.0
      orientation: 
        x: 0.0
        y: 0.0
        z: 0.9926209046673173
        w: 0.12125897747151136
  - 
    header: 
      seq: 0
      stamp: 
        secs: 1734
        nsecs: 705000000
      frame_id: "map"
    pose: 
      position: 
        x: 5.226292848549462
        y: -7.833518089135186
        z: 0.0
      orientation: 
        x: 0.0
        y: 0.0
        z: 0.9926209046673173
        w: 0.12125897747151136

/move_base/TebLocalPlannerROS/teb_markers

header: 
  seq: 17092
  stamp: 
    secs: 1734
    nsecs: 910000000
  frame_id: "odom"
ns: "ViaPoints"
id: 0
type: 8
action: 0
pose: 
  position: 
    x: 0.0
    y: 0.0
    z: 0.0
  orientation: 
    x: 0.0
    y: 0.0
    z: 0.0
    w: 0.0
scale: 
  x: 0.1
  y: 0.1
  z: 0.0
color: 
  r: 0.0
  g: 0.0
  b: 1.0
  a: 1.0
lifetime: 
  secs: 2
  nsecs:         0
frame_locked: False
points: 
  - 
    x: 5.790121363547482
    y: -7.767302333976653
    z: 0.0
  - 
    x: 5.226292848549462
    y: -7.833518089135186
    z: 0.0
colors: []
text: ''
mesh_resource: ''
mesh_use_embedded_materials: False

teb_local_planner_params.yaml

base_local_planner: teb_local_planner/TebLocalPlannerROS

TebLocalPlannerROS:
  odom: /odometry/filtered
  odom_topic: /odometry/filtered
  map_frame: map

  # HCPlanning
  enable_multithreading: true
  max_number_classes: 5
  selection_cost_hysteresis: 1.0
  selection_prefer_initial_plan: 0.95
  selection_obst_cost_scale: 2.0
  selection_viapoint_cost_scale: 1.0
  selection_alternative_time_cost: false
  switching_blocking_period: 0.0
  roadmap_graph_no_samples: 15
  roadmap_graph_area_width: 5.0
  roadmap_graph_area_length_scale: 1.0
  h_signature_prescaler: 1.0
  h_signature_threshold: 0.1
  obstacle_heading_threshold: 0.45
  viapoints_all_candidates: true
  visualize_hc_graph: false

  # Recovery
  shrink_horizon_backup: true
  oscillation_recovery: true

  # Optimization
  no_inner_iterations: 5
  no_outer_iterations: 4
  optimization_activate: true
  optimization_verbose: false
  penalty_epsilon: 0.1
  weight_max_vel_x: 2.0
  weight_max_vel_y: 0.0
  weight_max_vel_theta: 1.0
  weight_acc_lim_x: 1.0
  weight_acc_lim_y: 1.0
  weight_acc_lim_theta: 1.0
  weight_kinematics_nh: 1000.0

  weight_kinematics_forward_drive: 1.0
  weight_kinematics_turning_radius: 1.0
  
  weight_optimaltime: 10.0
  weight_shortest_path: 1.0
  weight_obstacle: 50.0
  weight_inflation: 0.1
  weight_dynamic_obstacle: 50.0
  weight_dynamic_obstacle_inflation: 0.1
  weight_viapoint: 10.0  # Important for better following global path
  weight_adapt_factor: 2.0
  obstacle_cost_exponent: 1.0

  # Trajectory
  teb_autosize: true
  dt_ref: 0.3
  dt_hysteresis: 0.03
  global_plan_overwrite_orientation: false
  allow_init_with_backwards_motion: true
  max_global_plan_lookahead_dist: 3.0
  force_reinit_new_goal_dist: 0.0
  feasibility_check_no_poses: 5
  exact_arc_length: false
  publish_feedback: false
  visualize_with_time_as_z_axis_scale: 0.0

  # Obstacles
  min_obstacle_dist: 0.5
  inflation_dist: 0.6
  dynamic_obstacle_inflation_dist: 0.6
  include_dynamic_obstacles: true
  include_costmap_obstacles: true
  legacy_obstacle_association: false
  obstacle_association_force_inclusion_factor: 1.5
  obstacle_association_cutoff_factor: 5.0
  costmap_obstacles_behind_robot_dist: 1.5
  obstacle_poses_affected: 30

  # Robot
  max_vel_x: 0.5
  max_vel_x_backwards: 0.5
  max_vel_theta: 1.
  acc_lim_x: 1.
  acc_lim_theta: 1.
  is_footprint_dynamic: false

  # Carlike
  min_turning_radius: 0.0
  wheelbase: 0.5
  cmd_angle_instead_rotvel: false

  # Omnidirectional
  max_vel_y: 0.0
  acc_lim_y: 0.0

  # GoalTolerance
  xy_goal_tolerance: 0.02
  yaw_goal_tolerance: 3.2
  free_goal_vel: false

  # ViaPoints
  global_plan_viapoint_sep: -0.2  # Negative means disabled so that custom vias could be used
  via_points_ordered: false