Fix calcuation of angle difference

[kyoichi-sugahara]

In the function of angle difference calculation, NaN value is returned when the absolute value of inner product is larger than 1.

autoware.universe/map/lanelet2_extension/lib/query.cpp

Lines 38 to 45 in 045009c

	double getAngleDifference(const double angle1, const double angle2)
	{
	Eigen::Vector2d vec1, vec2;
	vec1 << std::cos(angle1), std::sin(angle1);
	vec2 << std::cos(angle2), std::sin(angle2);
	const double diff_angle = std::acos(vec1.dot(vec2));
	return std::fabs(diff_angle);
	}

This is because arg of atd::acos shouldn't be outside the range [-1.0, 1.0] expalined here.
Numerical error happens in some cases so gurad needs to be added.

[kenji-miyake]

@kyoichi-sugahara Could you explain specifically in what situations the inner product will be larger than 1, please?

[kyoichi-sugahara]

@kenji-miyake
Sorry for not describing the detail.
I had same a same problem with the link

When angle1 and angle2 are the same, vec1.dot(vec2) expected to be 1. but in some cases, the value which is larger than 1 is calculated because of numerical error.