Custom via points are not transformed to correct frame (shifted by map->odom)
Combinacijus opened this issue · 0 comments
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:
teb_local_planner/src/teb_local_planner_ros.cpp
Line 1023 in f22b10a
And here via_points with swapped "odom" frame (without transformation) are published as marker for visualization:
teb_local_planner/src/visualization.cpp
Lines 337 to 343 in f22b10a
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
teb_local_planner/src/teb_local_planner_ros.cpp
Lines 1035 to 1039 in f22b10a
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