Add possibility to weight viapoint orientations #109
Conversation
modified: cfg/TebLocalPlannerReconfigure.cfg modified: include/teb_local_planner/g2o_types/edge_via_point.h modified: include/teb_local_planner/optimal_planner.h modified: include/teb_local_planner/teb_config.h modified: src/homotopy_class_planner.cpp modified: src/optimal_planner.cpp modified: src/teb_config.cpp modified: src/test_optim_node.cpp modified: src/visualization.cpp
modified: src/optimal_planner.cpp modified: src/teb_local_planner_ros.cpp
modified: CMakeLists.txt
new file: src/test_optim_node.cpp
modified: src/test_optim_node.cpp
|
Hey Christian ;-) |
| @@ -295,7 +295,7 @@ void HomotopyClassPlanner::renewAndAnalyzeOldTebs(bool delete_detours) | |||
|
|
|||
| void HomotopyClassPlanner::updateReferenceTrajectoryViaPoints(bool all_trajectories) | |||
| { | |||
| if ( (!all_trajectories && !initial_plan_) || !via_points_ || via_points_->empty() || cfg_->optim.weight_viapoint <= 0) | |||
| if ( (!all_trajectories && !initial_plan_) || !via_points_ || via_points_->empty() || (cfg_->optim.weight_viapoint <= 0 && cfg_->optim.weight_viapoint_orientation)) | |||
There was a problem hiding this comment.
This probably needs some work...
|
|
||
| int index = teb_.findClosestTrajectoryPose(*vp_it, NULL, start_pose_idx); | ||
| int index = teb_.findClosestTrajectoryPose(temp.position(), NULL, start_pose_idx); |
There was a problem hiding this comment.
Don't know if this could be done in a more optimal code style
|
Hi Christoph :) There are some minor things as noted above, but otherwise, it should be good to go. In the documentation, it might be worth noting that very high weights combined with sharp corners can cause oscillation or stuck behaviors. Best, @zerodamage |
|
@croesmann Hi, what the status of this pr? |
|
The changes are most easily observed in: Adding the line: Wherein the error in orientation is added to the graph for viapoints. That along with all the other changes to make the orientation available throughout the code as seen in the change from Vector2 to PoseSE. For now the orientation is calculated using atan2 between adjacent points. Buyer beware: if the orientation weight is set very high on a non-holonomic vehicle, the solver can get stuck, due to the conflicting constraints on position and orientation. |
|
Thanks! I see, one more question here about error value range, as in this optimization problem we try to minimize the error - push it to zero, correct? I tried to run your changes locally, I observed g2o::normalize_theta(bandpt->theta() - _measurement->theta()); outputting negative values as well, which shouldn't happen, or am I missing something here? |
Right! I've apparently missed the norm/absolute of the error in that line |
|
Do you plan for this pr to be merged into teb_local_planner ros2-master? |
For issue/enhancement of via-point following