I am using ROS kinetic and followed the navigation stack's tutorial on writing a global path planner. Most of the time its working great but on special occasions its behavior seems kinda odd. The global planner aims to follow a path derived from a tree built by my exploration algorithm. The path is not optimal so far but the focus lies on the robot closely following the edges connecting the tree nodes as they were cleared of 3D obstacles beforehand.
An example path calculated by the global planner can be seen in the image below (green lines show the path, purple arrows the orientation along the path's nodes, the red arrow marks the goal node and the hand-drawn red arrow shows where the robot is moving in a circular pattern. In this example the robot started from the first node on the right and then got stuck in the circle for quite some repititions until it finally (somehow) manages to reach the goal.
Now my question: Is the global planner giving out paths which nodes are too far apart or might there be a problem with the orientation of said nodes? Right now all nodes along the path point towards the next node, the goal node points towards the exploration goal. Or is the actual problem the configuration of my local planner (DWA, parameters shown below)? I've played around with some of its parameters (path_distance_bias and goal_distance_bias) but this did not seem to stop the robot from driving in circles.
Thanks in advance,
Edit 2: Since I'm working on a VM and I had my issues recording the screen via video, I made a gif which you can see below. The robot is moving in circles again in another example. Or to be precise, it looks more like an outward spiral. This time you can see the tree structure (blue lines and spheres). The glboal path can still be identified by the purple arrows at the path's nodes (green line is hardly visible).
Edit 3: I've changed 3 parameters and messed around with them for a bit. I think oscillation_reset_dist has no impact on this matter since the robot's behavior did not change on altering the parameter. the parameters path_distance_bias and goal_distance_bias actually led to a different behavior. The higher path_distance_bias was compared to goal_distance_bias the closer the robot followed the path up to the goal. When nearly reaching it, he made a sharp turn for the start as can be seen in the gif below ( with path_distance_bias: 50, goal_distance_bias: 10). The other way around leads to the robot just going from the start straight to the goal and turning around when nearly there.
(Please ignore the red current goal marker here, it remained at the start node and was not set to the goal node. This happens sometimes)
It seems like the path is calculated anew when nearly there but I added console output to the methods and can verify that the respective method is not called at these points. I also got an output showing me which nodes were added to the path, so I can see the goal node is actually the final node. You can see an exemplary output below:
[ INFO] [1596090677.425945079, 3219.610000000]: Current goal node set to 58
[ INFO] [1596090678.174113157, 3220.400000000]: Global planner make plan
[ INFO] [1596090678.186692274, 3220.420000000]: calculate path from 46 to 58
[ INFO] [1596090678.186773130, 3220.420000000]: 44 added to start_path
[ INFO] [1596090678.186894367, 3220.420000000]: 57 added to goal_path
[ INFO] [1596090678.186932376, 3220.420000000]: 43 added to start_path
[ INFO] [1596090678.186967423, 3220.420000000]: 56 added to goal_path
[ INFO] [1596090678.187001735, 3220.420000000]: 38 added to start_path
[ INFO] [1596090678.187095070, 3220.420000000]: 52 added to goal_path
[ INFO] [1596090678.187121173, 3220.420000000]: 26 added to start_path
[ INFO] [1596090678.187884568, 3220.420000000]: 48 added to goal_path
[ INFO] [1596090678.187962311, 3220.420000000]: 14 added to start_path
[ INFO] [1596090678.189614117, 3220.420000000]: 40 added to goal_path
[ INFO] [1596090678.189682727, 3220.420000000]: 12 added to start_path
[ INFO] [1596090678.190048167, 3220.420000000]: 39 added to goal_path
[ INFO] [1596090678.190090085, 3220.420000000]: 5 added to start_path
[ INFO] [1596090678.194444984, 3220.430000000]: 38 added to goal_path
[ INFO] [1596090678.194814828, 3220.430000000]: Found in start_path!
[ INFO] [1596090678.198484899, 3220.430000000]: Nodes in path:
[ INFO] [1596090678.198591926, 3220.430000000]: 46: x: 4.48426, y: 6.13291
[ INFO] [1596090678.199153917, 3220.440000000]: 44: x: 2.81383, y: 6.64129
[ INFO] [1596090678.199209663, 3220.440000000]: 43: x: 2.05619, y: 5.42951
[ INFO] [1596090678.199260024, 3220.440000000]: 38: x: 0.620106, y: 4.88587
[ INFO] [1596090678.199303066, 3220.440000000]: 39: x: 1.07314, y: 3.5379
[ INFO] [1596090678.199340956, 3220.440000000]: 40: x: 1.9226, y: 2.1106
[ INFO] [1596090678.199376052, 3220.440000000]: 48: x: 3.6461, y: 2.15701
[ INFO] [1596090678.199545138, 3220.440000000]: 52: x: 4.98731, y: 2.61824
[ INFO] [1596090678.199597048, 3220.440000000]: 56: x: 5.63163, y: 4.26897
[ INFO] [1596090678.199639214, 3220.440000000]: 57: x: 7.07973, y: 4.8549
[ INFO] [1596090678.199775672, 3220.440000000]: 58: x: 6.16029, y: 5.91454
[ INFO] [1596090678.375787127, 3220.600000000]: Got new plan
Edit 4: These loops don't just appear for the whole path but also for small segments which contain neither the start nor the goal position. Unfortunately, I was unable to record this happening in time.
Furthermore, I've added path nodes in between two nodes to have more anchoring points for the path and also removed the start node from the path as I was suspecting it to be the problem. But neither approach helped. I'm still kinda clueless what is causing this behavior.
Edit 5: I think, I finally found out what is causing the robot to move in circles. It appears that the local planner has a different global plan to follow than the global path shown in RViz. See the two below pictures for clarification.
Here you can see the tree (blue lines and spheres) as well as the global path published by my own global planner (purple arrows).
This image shows the visualization (red lines) of the portion of the (Edit) global plan (/move_base/DWAPlannerROS/global_plan) published by the local planner. It seems to be quite different from the global plan from my global planner. Also, it goes along the tree's edges starting from the tree node closest to the robot (green square).
It seems to me that the local planner is receiving newly made plans from the global planner which logs are somehow not displayed in the terminal. This leads me to my following questions:
- Why is the repeated calculation of the global path not displayed in the terminal? (Maybe called as a service)
- Is it possible to prohibit this behavior with setting certain parameters or to identify when the global path planning is rerun/called from the local planner?
On a side note, is it possible to add collapsible sections in this forum (like in markdown)?
planner.yaml (Edit: Removed TrajectoryPlanner parameters)
controller_frequency: 5.0 recovery_behaviour_enabled: true clearing_rotation_allowed: false max_planning_retries: 1 planner_patience: 1 base_global_planner: rrt_nbv_exploration::RneGlobalPlanner base_local_planner: dwa_local_planner/DWAPlannerROS DWAPlannerROS: # Robot configuration parameters acc_lim_x: 2.5 acc_lim_y: 0 acc_lim_th: 3.2 max_vel_x: 0.5 min_vel_x: 0.0 max_vel_y: 0 min_vel_y: 0 max_trans_vel: 0.5 min_trans_vel: 0.1 max_rot_vel: 1.0 min_rot_vel: 0.2 # Goal Tolerance Parameters yaw_goal_tolerance: 0.2 xy_goal_tolerance: 0.2 latch_xy_goal_tolerance: false # # Forward Simulation Parameters # sim_time: 2.0 # sim_granularity: 0.02 # vx_samples: 6 # vy_samples: 0 # vtheta_samples: 20 # penalize_negative_x: true # # Trajectory scoring parameters # path_distance_bias: 32.0 # The weighting for how much the controller should stay close to the path it was given # goal_distance_bias: 24.0 # The weighting for how much the controller should attempt to reach its local goal, also controls speed # occdist_scale: 0.01 # The weighting for how much the controller should attempt to avoid obstacles # forward_point_distance: 0.325 # The distance from the center point of the robot to place an additional scoring point, in meters # stop_time_buffer: 0.2 # The amount of time that the robot must stThe absolute value of the veolicty at which to start scaling the robot's footprint, in m/sop before a collision in order for a trajectory to be considered valid in seconds # scaling_speed: 0.25 # The absolute value of the veolicty at which to start scaling the robot's footprint, in m/s # max_scaling_factor: 0.2 # The maximum factor to scale the robot's footprint by # # Oscillation Prevention Parameters # oscillation_reset_dist: 0.25 #How far the robot must travel in meters before oscillation flags are reset (double, default: 0.05)
Post score: 1
Comment by Dragonslayer on 2020-07-29:
A video would be nice. How accurate is the red circle? Does the robot ever come close to the cost area of the obstacle or does it turn way before? In general the localplanner "drives" the robot the global planner just tells him in what direction. Analog to a car the localplanner is the driver the global planner the navigation system.
Maybe its even the high oscillation_reset_dist that disallows the robot to take the right turn away from the obstacle and forces it left into it, evading even more left. Video would really help here.
Comment by stevemacenski on 2020-07-29:
Also depends on the trajectory planner and how its setup.
Comment by Dragonslayer on 2020-07-29:
@stevemacenski I almost fell for it too. He says he is using dwaplanner, and in the yaml you can see that below the TrajectoryPlannerROS: parameters there is actually a section DWAPlannerROS: with all the parameters. Tricky post, as lots of people will check the Trajecotry planner stuff only :)
Comment by gvdhoorn on 2020-07-29:\
On a side note, is it possible to add collapsible sections in this forum
No, it isn't.
Comment by MarcoStb on 2020-07-30:
@Dragonslayer: I've added a gif, since I haven't found a program able to record the screen without artifacts on my VM. As you can see in the gif, this also happens when there are no obstacles nearby. I'll try to fiddle with the oscillation_reset_dist parameter as well. @gvdhoorn: Shame, but thanks for the info.
Comment by Dragonslayer on 2020-07-30:
Strange, I have to take a closer look later and see if I can get any concret idea what might be going on, but as it seems to "redo" the plan somehow, maybe try link text:
~/prune_plan (bool, default: true)
Defines whether or not to eat up the plan as the robot moves along the path. If set to true, points will fall off the end of the plan once the robot moves 1 meter past them.
I like the high path_distance_bias better as it clearly shows when things start to go wrong. Yes, it seems with the given plan the robot doesn reach the goal and just starts over again. Your idea that "your" global plan is different from dwa´s global plan might be missleading as the wiki states "The PORTION of the global plan that the local planner is currently attempting to follow... " It seems the "import" of the next "portion" isnt happening. Maybe the "plan" messege or how ever you publish it is missing something?
Comment by stevemacenski on 2020-07-30:
That's a pretty fun bug to have in the scheme of possible bugs!
Comment by MarcoStb on 2020-07-31:
@Dragonslayer: Unfortunately, I already tried the prune_plan parameter to no avail. When I set it to false, it seems to be causing more problems finding a smooth path.
Yes, you're right. I read that but formulated it a bit unclear in my own words. I feel like it is not importing the next portion from the original plan but repeatedly calling the global planner's plugin method makePlan which in my case returns a new global plan starting at the tree node closest to the robot. In the "bug" case this tree node must be outside the remaining path which leads to the robot running in circles. But on the other hand, when looking at the gifs I posted, the robot seems not to be heading towards the nearest node in the tree.
Anyway, it would help me a lot to get clarity on how local and global planner work together inside of the move_base package since that seems to cause my problem. I was not able to find any documentation regarding this.
@stevemacenski: Unless you have to debug it ... :D
Comment by Dragonslayer on 2020-07-31:
It seems the portion of global_plan the local planner shows is the portion inside the local costmap. What happens if you set a goal inside the initial (from starting point) local costmap? Does it show the complete path now? Secondly what happens if you set path_distance_bias to NULL/0 ? How globalplanner and localplanner really interact I dont know exactly, but how I understand them its really just that the global planner sends a path and the local planner uses it as a guidance. However I just found this link text, might be worth a read, people experimenting with planners use it.
Comment by MarcoStb on 2020-08-04:
This seems to confirm your point. If the goal is inside the local costmap, the complete path is shown.
With path_distance set to 0, the robot drives straight to the goal. I did not observe any weird behavior in this case.
Might be worth a try, thanks for the link.
Comment by Dragonslayer on 2020-08-06:
In the link there is this exposed action exe_path (asume it means execute). What might happen with the circle motion is that this action is retriggered for some unknown reason making the robot go back to the starting point again, resulting in this circling motion. This might have something to do with recovery behaviours or some prune_plan eat up plan function(reseting starting point to near the robot), where with the reexecute the robot is forced back to the beginning and cant continue on the reexecuted path, in wich case it wouldnt matter as the path stays the same.
Comment by MarcoStb on 2020-08-20:
Sorry for the late response, I was busy with some other more important work. I think, I've found a solution which I posted as an answer below. Thank you for your input and suggestions!
Comment by HT38 on 2021-09-02:
Hey, I kind of faced the same issue. I have changed latch_xy_goal_tolerance to true and also prune_plan to true but my two-wheeled robot rotates in a circle for a U-shaped goal trajectory (it works fine for other trajectories paths). Any help Thank you