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:
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_pointsLegend
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_pointsare 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_pointsmessage 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#L1023And 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-L1039Details
/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 ```