investigate source of invalid relative location in Trapezoids

[rayvburn]

Recently, the following error was caught for the first time:

terminate called after throwing an instance of 'std::runtime_error'
  what():  wrong value of the relative location!

Leaving this for a further consideration as it may be related to the timing of inserting people in a simulation scenario (mid-run computation loop or something).

[rayvburn]

It happened again while mangling with the STG parameters (most likely caused by the negative Bn of the SFM). This is the call stack:

libc.so.6!__GI_raise(int sig) (/build/glibc-CVJwZb/glibc-2.27/sysdeps/unix/sysv/linux/raise.c:51)
libc.so.6!__GI_abort() (/build/glibc-CVJwZb/glibc-2.27/stdlib/abort.c:79)
libstdc++.so.6![Unknown/Just-In-Time compiled code] (Unknown Source:0)
libstdc++.so.6!std::terminate() (Unknown Source:0)
libstdc++.so.6!__cxa_throw (Unknown Source:0)
libfuzzy.so!hubero_local_planner::fuzz::TrapezoidLocDep::update(hubero_local_planner::RelativeLocation, hubero_local_planner::geometry::Angle const&, hubero_local_planner::geometry::Angle const&) (Unknown Source:0)
libfuzzy.so!hubero_local_planner::fuzz::Processor::updateRegions(double const&, double const&, double const&, double const&) (Unknown Source:0)
libfuzzy.so!hubero_local_planner::fuzz::Processor::process(double const&, std::vector<double, std::allocator<double> > const&, std::vector<double, std::allocator<double> > const&, std::vector<double, std::allocator<double> > const&) (Unknown Source:0)
libhubero_local_planner.so!hubero_local_planner::SocialTrajectoryGenerator::computeForces(hubero_local_planner::World const&, double const&, hubero_local_planner::SocialTrajectoryGenerator::SampleAmplifierSet const&, hubero_local_planner::geometry::Vector&, hubero_local_planner::geometry::Vector&, hubero_local_planner::geometry::Vector&, hubero_local_planner::geometry::Vector&, bool) (Unknown Source:0)
libhubero_local_planner.so!hubero_local_planner::SocialTrajectoryGenerator::generateTrajectory(hubero_local_planner::World const&, hubero_local_planner::SocialTrajectoryGenerator::SampleAmplifierSet const&, base_local_planner::Trajectory&) (Unknown Source:0)
libhubero_local_planner.so!hubero_local_planner::SocialTrajectoryGenerator::nextTrajectory(base_local_planner::Trajectory&) (Unknown Source:0)
libbase_local_planner.so!base_local_planner::SimpleScoredSamplingPlanner::findBestTrajectory(base_local_planner::Trajectory&, std::vector<base_local_planner::Trajectory, std::allocator<base_local_planner::Trajectory> >*) (Unknown Source:0)
libhubero_local_planner.so!hubero_local_planner::HuberoPlanner::planMovingRobot() (Unknown Source:0)
libhubero_local_planner.so!hubero_local_planner::HuberoPlanner::findBestTrajectory(hubero_local_planner::geometry::Vector const&, std::shared_ptr<std::vector<std::shared_ptr<hubero_local_planner::Obstacle>, std::allocator<std::shared_ptr<hubero_local_planner::Obstacle> > > const>, std::shared_ptr<std::vector<hubero_local_planner::Person, std::allocator<hubero_local_planner::Person> > const>, std::shared_ptr<std::vector<hubero_local_planner::Group, std::allocator<hubero_local_planner::Group> > const>, geometry_msgs::PoseStamped_<std::allocator<void> >&) (Unknown Source:0)
libhubero_local_planner.so!hubero_local_planner::HuberoPlannerROS::computeVelocityCommands(geometry_msgs::Twist_<std::allocator<void> >&) (Unknown Source:0)
libmove_base.so!move_base::MoveBase::executeCycle(geometry_msgs::PoseStamped_<std::allocator<void> >&, std::vector<geometry_msgs::PoseStamped_<std::allocator<void> >, std::allocator<geometry_msgs::PoseStamped_<std::allocator<void> > > >&) (Unknown Source:0)
libmove_base.so!move_base::MoveBase::executeCb(boost::shared_ptr<move_base_msgs::MoveBaseGoal_<std::allocator<void> > const> const&) (Unknown Source:0)
libmove_base.so!boost::_mfi::mf1<void, move_base::MoveBase, boost::shared_ptr<move_base_msgs::MoveBaseGoal_<std::allocator<void> > const> const&>::operator()(move_base::MoveBase*, boost::shared_ptr<move_base_msgs::MoveBaseGoal_<std::allocator<void> > const> const&) const (Unknown Source:0)
libmove_base.so!void boost::_bi::list2<boost::_bi::value<move_base::MoveBase*>, boost::arg<1> >::operator()<boost::_mfi::mf1<void, move_base::MoveBase, boost::shared_ptr<move_base_msgs::MoveBaseGoal_<std::allocator<void> > const> const&>, boost::_bi::rrlist1<boost::shared_ptr<move_base_msgs::MoveBaseGoal_<std::allocator<void> > const> const&> >(boost::_bi::type<void>, boost::_mfi::mf1<void, move_base::MoveBase, boost::shared_ptr<move_base_msgs::MoveBaseGoal_<std::allocator<void> > const> const&>&, boost::_bi::rrlist1<boost::shared_ptr<move_base_msgs::MoveBaseGoal_<std::allocator<void> > const> const&>&, int) (Unknown Source:0)
libmove_base.so!void boost::_bi::bind_t<void, boost::_mfi::mf1<void, move_base::MoveBase, boost::shared_ptr<move_base_msgs::MoveBaseGoal_<std::allocator<void> > const> const&>, boost::_bi::list2<boost::_bi::value<move_base::MoveBase*>, boost::arg<1> > >::operator()<boost::shared_ptr<move_base_msgs::MoveBaseGoal_<std::allocator<void> > const> const&>(boost::shared_ptr<move_base_msgs::MoveBaseGoal_<std::allocator<void> > const> const&) (Unknown Source:0)

[rayvburn]

To avoid that, such a strategy can be applied in decodeRelativeLocation:

if (rel_loc < 0.0) {
	// right side
	return RelativeLocation::LOCATION_RIGHT;
}
// left side is selected when rel_loc is non-negative or NAN
return RelativeLocation::LOCATION_LEFT;

However, wrong relative locations (with some parameter values) will still produce ill-formed trapezoids and runtime error will be thrown anyway.