publishing nan velocity due to infinite inverse radius assignment

[talhaaliarslan]

Came across this corner case where angular field in the command velocity is set to nan which may cause erratic movements on a robot if not handled on the receiving side. The nan value is coming from feedforward_ang_ component of the controller, which uses inverse turning radius as:

path_tracking_pid/src/controller.cpp

Line 559 in 750f8c1

turning_radius_inv_vector_[find_result.last_visited_pose_index] * control_effort_long_;

The part where inverse turn radii are computed have a return value of infinity when delta is below an epsilon specified which includes a zero value too :

path_tracking_pid/src/calculations.cpp

Lines 45 to 60 in 750f8c1

	std::vector<double> inverse_turning_radiuses(const std::vector<tf2::Transform> & deltas)
	{
	auto result = std::vector<double>{};
	result.reserve(deltas.size() + 1);
	std::transform(
	deltas.cbegin(), deltas.cend(), std::back_inserter(result), [](const tf2::Transform & d) {
	const auto & origin = d.getOrigin();
	const auto dpX = origin.x();
	const auto dpY = origin.y();
	const auto dpXY2 = std::pow(dpX, 2) + std::pow(dpY, 2);
	if (dpXY2 < FLT_EPSILON) {
	return std::numeric_limits<double>::infinity();
	}
	return (2 * dpY) / dpXY2;
	});

This is causing an instance of nan command velocity (angular) in the controller which stays permanent even if inverse radii return back to finite values.

[Timple]

Ah, good catch!

Did you have fix for this already in mind? Otherwise we can look into it after next week.

[DemyKremers]

Hey @Timple , Talha and I fixed (and tested) this by replacing line 57 in calculations.cpp:
return std::numeric_limits<double>::infinity();

by:
return std::numeric_limits<double>::max();

Hope this helps.