Required Info:
- Operating System:
- Computer:
- ROS2 Version:
- Version or commit hash:
I'm working on a high-precision autonomous robot, so it is important that the planned path ends exactly at the requested goal pose whenever possible.
However, when using the SMAC 2D planner, I noticed that the final pose in the returned path does not match the requested goal pose, even though the planner reports success.
Here's my ComputePathToPose request
ros2 action send_goal /compute_path_to_pose nav2_msgs/action/ComputePathToPose "{
goal: {
header: {
frame_id: 'map'
},
pose: {
position: {
x: 0.0,
y: 0.0,
z: 0.0
},
orientation: {
x: 0.0,
y: 0.0,
z: 0.0,
w: 1.0
}
}
},
planner_id: '',
use_start: false
}"
The planner succeeds, but the last pose (-0.01, -0.04) in the returned path is not the same as the requested goal (0.0, 0.0).
.....
- header:
stamp:
sec: 1784109083
nanosec: 316063669
frame_id: map
pose:
position:
x: -0.009999722242355347
y: -0.039999984204769135
z: 0.0
orientation:
x: 0.0
y: 0.0
z: 0.0
w: 1.0
planning_time:
sec: 0
nanosec: 4490850
error_code: 0
error_msg: ''
Goal finished with status: SUCCEEDED
Here's the image of the returned path:
I also tried reducing the planner tolerance to a very small value (1e-6), but the behavior did not change. Here's my configuration (my map resolution is 0.05):
planner_server:
ros__parameters:
planner_plugins: ["GridBased"]
GridBased:
plugin: "nav2_smac_planner::SmacPlanner2D" # In Iron and older versions, "/" was used instead of "::"
tolerance: 0.000001 # tolerance for planning if unable to reach exact pose, in meters
downsample_costmap: false # whether or not to downsample the map
downsampling_factor: 1 # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm)
allow_unknown: true # allow traveling in unknown space
max_iterations: -1 # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable
max_on_approach_iterations: 10000 # maximum number of iterations to attempt to reach goal once in tolerance
max_planning_time: 2.0 # max time in s for planner to plan, smooth
cost_travel_multiplier: 2.0 # Cost multiplier to apply to search to steer away from high cost areas. Larger values will place in the center of aisles more exactly (if non-`FREE` cost potential field exists) but take slightly longer to compute. To optimize for speed, a value of 1.0 is reasonable. A reasonable tradeoff value is 2.0. A value of 0.0 effective disables steering away from obstacles and acts like a naive binary search A*.
use_final_approach_orientation: false # Whether to set the final path pose at the goal's orientation to the requested orientation (false) or in line with the approach angle so the robot doesn't rotate to heading (true)
smoother:
max_iterations: 1000
w_smooth: 0.3
w_data: 0.2
tolerance: 1.0e-10
However, when using the NavFn planner with the same goal, the final pose in the returned path matches the requested goal exactly.
...
- header:
stamp:
sec: 1784109676
nanosec: 462453852
frame_id: map
pose:
position:
x: 0.0
y: 0.0
z: 0.0
orientation:
x: 0.0
y: 0.0
z: 0.0
w: 1.0
planning_time:
sec: 0
nanosec: 3007432
error_code: 0
error_msg: ''
Goal finished with status: SUCCEEDED
Here's the image of the NavFn result:
While looking into the implementation, I noticed that in SMAC 2D, after createPath() is called, the last pose of the generated path is not replaced with the requested goal pose:
|
if (!_a_star->createPath( |
|
path, num_iterations, |
|
_tolerance / static_cast<float>(costmap->getResolution()), cancel_checker)) |
In contrast, NavFn explicitly initializes the path with the exact goal pose before tracing the path back through the potential field (when potential < POT_HIGH), which appears to guarantee that the final pose matches the requested goal.
|
planner_->setStart(map_goal); |
I'm wondering whether this is the expected behavior of SMAC 2D, where the final pose of the generated path may not exactly match the requested goal pose even when planning succeeds. If this is expected, would it make sense for SMAC 2D to adopt a similar approach to NavFn by replacing the last pose in the path with the requested goal pose after path generation? Or is this behavior considered a bug?
Required Info:
I'm working on a high-precision autonomous robot, so it is important that the planned path ends exactly at the requested goal pose whenever possible.
However, when using the SMAC 2D planner, I noticed that the final pose in the returned path does not match the requested goal pose, even though the planner reports success.
Here's my ComputePathToPose request
The planner succeeds, but the last pose (-0.01, -0.04) in the returned path is not the same as the requested goal (0.0, 0.0).
Here's the image of the returned path:
I also tried reducing the planner tolerance to a very small value (1e-6), but the behavior did not change. Here's my configuration (my map resolution is 0.05):
However, when using the NavFn planner with the same goal, the final pose in the returned path matches the requested goal exactly.
Here's the image of the NavFn result:
While looking into the implementation, I noticed that in SMAC 2D, after createPath() is called, the last pose of the generated path is not replaced with the requested goal pose:
navigation2/nav2_smac_planner/src/smac_planner_2d.cpp
Lines 288 to 290 in 71a1d84
In contrast, NavFn explicitly initializes the path with the exact goal pose before tracing the path back through the potential field (when potential < POT_HIGH), which appears to guarantee that the final pose matches the requested goal.
navigation2/nav2_navfn_planner/src/navfn_planner.cpp
Line 400 in 71a1d84
I'm wondering whether this is the expected behavior of SMAC 2D, where the final pose of the generated path may not exactly match the requested goal pose even when planning succeeds. If this is expected, would it make sense for SMAC 2D to adopt a similar approach to NavFn by replacing the last pose in the path with the requested goal pose after path generation? Or is this behavior considered a bug?