Open rayvburn opened 11 months ago
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)
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.
Recently, the following error was caught for the first time:
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).