Support orientation on custom via points

[doisyg]

Hello,
The ability to send custom via points is great to force a robot to pass through predefined set of points.
Via points are currently defined only by their x and y. It would be great to able to constraints the robot to pass through these points with a specific orientation.
How hard that would be to implement this behavior in teb_local_planner? Any pointers. I may try.
Best

[croesmann]

Hi,
this might be a good idea and I don't think that it is hard to implement.
Just create a copy of the via point cost function (edge) (in edge_via_point.h).

Change the edge to a 2 dimensional error and to accept a PoseSE2 as via-point:

class EdgeViaPointSE2 : public BaseTebUnaryEdge<2, const PoseSE2*, VertexPose> {...}

The error might then be computed using the following distance metric:

 _error[0] = (bandpt->position() - _measurement->position()).norm(); // as in EdgeViaPoint
 _error[1] = g2o::normalize_theta(bandpt->theta() - _measurement->theta());

Afterward, you must add this particular edge creation case to optimal_planner.cpp.
You also need to add a weight for the via-point orientation (see the other edge creation routines for examples).
We could think of changing the API, in particular, ViaPointContainer to a container of PoseSE2.

If you need further hints, let me know.

[doisyg]

Thanks for the pointers. I will try to have a look at it in the following weeks.

[doisyg]

Before modifying the present implementation I am trying to understand how it works. What is suppose to be the workflow for sending a robot to custom waypoints?
Currently, this is what I do:

1. With the robot stoped, generate a nav_msgs/Path
2. Publish the generated path on /move_base/TebLocalPlannerROS/via_points
   Nothing happens, so I tried:
3. Send a goal to move base:
   The via points sent through the topics appear in the Teb markers, the robot starts to follow a path going to the goal and passing by the generated via points, it reached the goal, but then instead of stopping, it goes back on forth indefinitly between the center of the waypoints (it seems) and the goal:

Maybe I miss something?

[croesmann]

according to your animation, you probably have the use case to provide via-points using an external script.
To be honest, this is a use case I have not yet played a lot with, so there might be some things to improve.
The most common via-point use case is to obtain them from the global plan in every planning cycle (which is indeed implemented).
But of course, it should be possible to also add them via a topic (I added this due to some user request #63 in merge request #74).
However, I just had a quick look at void TebLocalPlannerROS::customViaPointsCB(const nav_msgs::Path::ConstPtr& via_points_msg) and I am obviously not transforming the via points to the planning frame (usually map). Hence, we require to provide the via points already in the correct coordinate systems a-priori (but this does not seem to be the issue in your example).

In your animation, we can see, that the planner does not take the via-points into account correctly.
This is because it is quite difficult to reach via-points which are not within the line of sight towards the goal. The trajectory needs to be extended which is done in the following manner:
Each via-point is associated with the closest pose of the trajectory (in void TebOptimalPlanner::AddEdgesViaPoints()). The weight of the via-point cost function implies an attraction force resulting in extending the trajectory slightly. The autoResize algorithm then adds new poses. For complex via-point scenarios, it might be really difficult to converge properly.

Currently, I ignore via-points behind the start or beyond the goal to avoid such situations.
This is implicitly done in void TebOptimalPlanner::AddEdgesViaPoints(). If the closest pose to a via-point is the start or the goal pose, it does not have any effect on the optimization result, since the start and goal are fixed during optimization.
You can see some magic, that I force the algorithm to attach via-points with at least the second vertex in case parameter via_points_ordered=True. You could try to repeat your scenario from the animation with this option turned on. I am really interested in how it looks like.
Otherwise, please have a look at void TebOptimalPlanner::AddEdgesViaPoints().
It can definitely be improved. However, the best would be to consider the via-points also in the TEB initialization. If we would initialize the TEB along the via-points, even the current implementation of void TebOptimalPlanner::AddEdgesViaPoints() should work fine to my mind.

[doisyg]

Thanks a lot for your detailed answer (as always). I was indeed adding waypoints through the viapoint topic. But I realized that it wasn't the cleanest way to do it, which you confirmed later in your answer.
I managed to achieve the desired behavior (make the robot navigate through a series of waypoint) by using move base flex and the mbf_msgs::ExePath action (which has a path field of type nav_msgs::Path)

Note: In my implementation, I actually use the mbf_msgs::MoveBase action to go to the first waypoint and then use the ExePath for the rest of my waypoints (EDIT: the path actually include the first waypoint). I can conveniently use two different controllers (ie local planner) with two different sets of parameters thanks to move base flex.

I noticed that if the global_plan_viapoint_sep is inferior (or negative) to the distance between my waypoints, I get the expected behavior, see below with global_plan_viapoint_sep: 0.25, weight_viapoint: 1000 and a distance between two waypoint of 0.5.

Otherwise, if global_plan_viapoint_sep is superior to the distance between my waypoints, it seems that teb is generating them on the fly. Here with global_plan_viapoint_sep: 0.25, weight_viapoint: 1000 and a distance between two waypoint of 0.1.

Anyway, I reached the desired behavior and don't need to implement immediately the orientation constraints to the viapoints. But it is definitely something I will consider in the next few months

[zerodamage]

Hello,

Has there been any progress on this enhancement?
If not, we have a use-case that requires this, and I will therefore begin implementing based on @croesmann 's suggestions above

Best regards

[zerodamage]

#109

[coco721]

hi @croesmann @doisyg
this behaviour is really useful. Can teb_local planner avoid obstacle when the obstacle occure on the path that given by myself ? Actually, I do the similar job. I replace the "global_planner" with a "waypoint_planner"(i send a fixed global path to local_planner). I would like to use "teb_local_planner" to avoid obstacle when obstacle occure in local cost map (the global waypoints may through obstacle,so teb_local_planner has to generated a new local path to avoid collision without following global path),after pass the obstacle succefully, the local path should back to global watpoint path.

However, when i try this in gazebo, the teb_local_planner's global plan(not waypoint path given by myself,but teb_local_planner generated to let local plan follow) will never change and always following my waypoints global path.

is there any suggestion for my problem??

here is my teb_planner.yamls:
`

TebLocalPlannerROS:

odom_topic: /GTR/odom

Trajectory

teb_autosize: True
dt_ref: 0.3
dt_hysteresis: 0.1
global_plan_overwrite_orientation: True
allow_init_with_backwards_motion: false #当前没有rear sensor
max_global_plan_lookahead_dist: 3.0
feasibility_check_no_poses: 2
publish_feedback: true

global_plan_viapoint_sep: 1

Robot

max_vel_x: 1.5
max_vel_x_backwards: 0.5
max_vel_y: 0.0
max_vel_theta: 1.6 # the angular velocity is also bounded by min_turning_radius in case of a carlike robot (r = v / omega)
acc_lim_x: 0.5
acc_lim_theta: 0.5

********************** Carlike robot parameters ********************

min_turning_radius: 0.1 # Min turning radius of the carlike robot (compute value using a model or adjust with rqt_reconfigure manually)
wheelbase: 1.3 # Wheelbase of our robot
cmd_angle_instead_rotvel: True # stage simulator takes the angle instead of the rotvel as input (twist message)

********************************************************************

footprint_model: # types: "point", "circular", "two_circles", "line", "polygon"
type: "line"
radius: 0.2 # for type "circular"
line_start: [-1.5, 0.0] # for type "line"
line_end: [1.5, 0.0] # for type "line"
front_offset: 0.2 # for type "two_circles"
front_radius: 0.2 # for type "two_circles"
rear_offset: 0.2 # for type "two_circles"
rear_radius: 0.2 # for type "two_circles"
vertices: [ [0.25, -0.05], [0.18, -0.05], [0.18, -0.18], [-0.19, -0.18], [-0.25, 0], [-0.19, 0.18], [0.18, 0.18], [0.18, 0.05], [0.25, 0.05] ] # for type "polygon"

GoalTolerance

xy_goal_tolerance: 0.2
yaw_goal_tolerance: 0.1
free_goal_vel: False

Obstacles

min_obstacle_dist: 4 # default=0.27 This value must also include our robot's expansion, since footprint_model is set to "line".
include_costmap_obstacles: True
costmap_obstacles_behind_robot_dist: 1.0
obstacle_poses_affected: 30
costmap_converter_plugin: ""
costmap_converter_spin_thread: True
costmap_converter_rate: 3

Optimization

no_inner_iterations: 5
no_outer_iterations: 4
optimization_activate: True
optimization_verbose: False
penalty_epsilon: 0.1
weight_max_vel_x: 2
weight_max_vel_theta: 1
weight_acc_lim_x: 1
weight_acc_lim_theta: 1
weight_kinematics_nh: 1000
weight_kinematics_forward_drive: 1
weight_kinematics_turning_radius: 1
weight_optimaltime: 1
weight_obstacle: 50
weight_dynamic_obstacle: 10 # not in use yet

Homotopy Class Planner

enable_homotopy_class_planning: true
enable_multithreading: True
simple_exploration: False
max_number_classes: 4
selection_cost_hysteresis: 1.0
selection_obst_cost_scale: 1.0
selection_alternative_time_cost: False
roadmap_graph_no_samples: 15
roadmap_graph_area_width: 5
h_signature_prescaler: 0.5
h_signature_threshold: 0.1
obstacle_keypoint_offset: 0.1
obstacle_heading_threshold: 0.45
visualize_hc_graph: False
`

Thanks
with reagrds

[zerodamage]

@guochence I can't immediately see the issue with your settings, other than the difference that I do not use a costmap converter...

I can tell you that in my case, even with global paths through obstacles, it is possible for my TEB local path to avoid the obstacles (if they are sufficiently small)

[coco721]

@zerodamage
thanks for your reply. May i ask few questions about yamls?
1.about "global_plan_viapoint_sep", did u set this parameter? or just =-1 to disable it?
2.About costmap, which plugins do u use in your global_costmap and local_costmap respectively？ Do i need use "inflation_layer" and "static_layer" in local_costmap while teb_planner has "min_obstacle_dist" and "inflation_dist"?
3.About "min_obstacle_dist" in teb_planner. I do not understand why the planner need this params while local_cost_map can apply "inflation_layer" .

thanks for you patient.
with regards

[zerodamage]

@guochence

1. is set to a distance smaller than my global plan spacing (0.1m)
2. while I still use inflation layer in the local costmap, I dont believe it is actually necessary, because only lethal obstacles are used by teb_local_planner
3. same answer as above, there is no need to apply the inflation layer, because the non-lethal results of it are not used for anything