Constrained smoother failing with duplicate path pose for Smac Lattice in-place rotations #4755
Description
Bug report
Required Info:
- Operating System:
- Debian 12
- ROS2 Version:
- humble
- Version or commit hash:
- humble branch commit 0f8fda3
Steps to reproduce issue
Any goal with a heading that triggers this issue:
#4596
and then using the constrained smoother on the path
Expected behavior
A normal smoothed path
Actual behavior
[smoother_server-8] [INFO] [1731936082.959181435] [smoother_server]: Received a path to smooth.
[smoother_server-8] WARNING: Logging before InitGoogleLogging() is written to STDERR
[smoother_server-8] W20241118 14:21:22.984320 497746 residual_block.cc:129]
[smoother_server-8]
[smoother_server-8] Error in evaluating the ResidualBlock.
[smoother_server-8]
[smoother_server-8] There are two possible reasons. Either the CostFunction did not evaluate and fill all
[smoother_server-8] residual and jacobians that were requested or there was a non-finite value (nan/infinite)
[smoother_server-8] generated during the or jacobian computation.
[smoother_server-8]
[smoother_server-8] Residual Block size: 3 parameter blocks x 4 residuals
[smoother_server-8]
[smoother_server-8] For each parameter block, the value of the parameters are printed in the first column
[smoother_server-8] and the value of the jacobian under the corresponding residual. If a ParameterBlock was
[smoother_server-8] held constant then the corresponding jacobian is printed as 'Not Computed'. If an entry
[smoother_server-8] of the Jacobian/residual array was requested but was not written to by user code, it is
[smoother_server-8] indicated by 'Uninitialized'. This is an error. Residuals or Jacobian values evaluating
[smoother_server-8] to Inf or NaN is also an error.
[smoother_server-8]
[smoother_server-8] Residuals: -nan 0 0 667.778
[smoother_server-8]
[smoother_server-8] Parameter Block 0, size: 2
[smoother_server-8]
[smoother_server-8] 0.0925396 | Not Computed Not Computed Not Computed Not Computed
[smoother_server-8] -208.843 | Not Computed Not Computed Not Computed Not Computed
[smoother_server-8]
[smoother_server-8] Parameter Block 1, size: 2
[smoother_server-8]
[smoother_server-8] 0.0925396 | Not Computed Not Computed Not Computed Not Computed
[smoother_server-8] -208.843 | Not Computed Not Computed Not Computed Not Computed
[smoother_server-8]
[smoother_server-8] Parameter Block 2, size: 2
[smoother_server-8]
[smoother_server-8] -1.30546 | -nan 0 0 0
[smoother_server-8] -207.879 | -nan 0 0 0
[smoother_server-8]
[smoother_server-8]
[smoother_server-8] E20241118 14:21:22.984406 497746 trust_region_minimizer.cc:71] Terminating: Residual and Jacobian evaluation failed.
[smoother_server-8] [WARN] [1731936082.984575316] [smoother_server]: constrained_smoother: failed to smooth plan, Ceres could not find a usable solution to optimize.
[bt_navigator-14] [ERROR] [1731936110.176972226] [bt_navigator_navigate_through_poses_rclcpp_node]: Failed to get result for smooth_path in node halt!
Additional information
I already debugged this and the issue is that the smoother crashes on duplicate poses because the distance between two poses is used in the smoothing cost as a division. Here the current segment length is used in a division while it can be 0 for duplicate poses. This happens in all branches, not just humble as far as I am aware. The duplicate poses should not be there to begin with so I looked at the smac_hybrid code and found the indexing to be the issue. Here the analytic expansion gets the index for the node that the analytic expansion goes through. This index in some cases fails to retrieve the goal pose because of the angle inaccuracy. When that happens the index next to it is wrongly set and this causes the last node of the expansion to be set as the parent of the goal pose even though their poses are identical. This seems to be fixed when backporting #4636 to the humble branch but I am a bit sceptical if this is a full fix or just covers it better than before.
Feature request
Feature description
This should be fixed in both the smoother and the smac planner. The smoother should check for duplicate poses and throw an error as the error message for this case is currently very cryptic compared to how simple the error is. On the other hand the problem of having the last pose duplicated by the smac planner should be fixed and probably will be when this change is backported to the humble branch.
Implementation considerations
The smoother just needs a check for the distance between poses and throw a meaningful error message. The smac planner needs the linked PR backported to the humble branch. I am not sure if this fully fixes the issue though as I am not 100% sure what is exactly happening and if this might have just been an accidental fix that doesnt fully cover the problem. I think another cause for this might be that there are a lot of implicit conversions. Most worrying for me are the ones between unsigned int and float/double. Fixing those is a fairly big task as compiling with -Wconversion -Werror throws a lot of errors but this might be part of what causes this error.