Original report by Peter Schüller (Bitbucket: peterschueller, GitHub: peterschueller).
I am doing many RRTstar motion plans and sometimes I get segfaults within OMPL.
I updated to the latest OMPL, built it, and rebuilt moveit and I can verify that moveit links to my build of libompl.so
(This time the problem is not like in #39, I am sure that I really use RRTstar all the time.)
The segfault seems to be timing-related as I cannot reproduce it with valgrind or gdb, I can only analyze post-mortem core-dumps, they look as follows:
(gdb) info threads
Id Target Id Frame
9 Thread 0x2b3425237700 (LWP 20707) 0x00002b341fe64afe in fcl::obbDisjoint(fcl::Matrix3fX<fcl::details::Matrix3Data<double> > const&, fcl::Vec3fX<fcl::details::Vec3Data<double> > const&, fcl::Vec3fX<fcl::details::Vec3Data<double> > const&, fcl::Vec3fX<fcl::details::Vec3Data<double> > const&) () from /opt/ros/groovy/lib/libfcl.so
8 Thread 0x2b3425033700 (LWP 20022) pthread_cond_wait@@GLIBC_2.3.2 () at ../nptl/sysdeps/unix/sysv/linux/x86_64/pthread_cond_wait.S:162
7 Thread 0x2b34245e4700 (LWP 19971) pthread_cond_wait@@GLIBC_2.3.2 () at ../nptl/sysdeps/unix/sysv/linux/x86_64/pthread_cond_wait.S:162
6 Thread 0x2b34241e2700 (LWP 19969) 0x00002b341497be63 in __GI___poll (fds=<optimized out>, nfds=<optimized out>, timeout=<optimized out>) at ../sysdeps/unix/sysv/linux/poll.c:87
5 Thread 0x2b341847ce60 (LWP 19958) sem_wait () at ../nptl/sysdeps/unix/sysv/linux/x86_64/sem_wait.S:86
4 Thread 0x2b34247e5700 (LWP 19991) pthread_cond_timedwait@@GLIBC_2.3.2 () at ../nptl/sysdeps/unix/sysv/linux/x86_64/pthread_cond_timedwait.S:216
3 Thread 0x2b34243e3700 (LWP 19970) 0x00002b3414981003 in select () at ../sysdeps/unix/syscall-template.S:82
2 Thread 0x2b3425639700 (LWP 20709) __memcmp_sse4_1 () at ../sysdeps/x86_64/multiarch/memcmp-sse4.S:45
* 1 Thread 0x2b3425438700 (LWP 20708) 0x00002b341e1de0cd in ompl::geometric::RRTstar::updateChildCosts(ompl::geometric::RRTstar::Motion*, double) () from /usr/local/lib/libompl.so.6
Thread 1 (here the segfault happens, it looks like an infinite recursion):
(gdb) bt
#0 0x00002b341e1de0cd in ompl::geometric::RRTstar::updateChildCosts(ompl::geometric::RRTstar::Motion*, double) () from /usr/local/lib/libompl.so.6
#1 0x00002b341e1de0d7 in ompl::geometric::RRTstar::updateChildCosts(ompl::geometric::RRTstar::Motion*, double) () from /usr/local/lib/libompl.so.6
#2 0x00002b341e1de0d7 in ompl::geometric::RRTstar::updateChildCosts(ompl::geometric::RRTstar::Motion*, double) () from /usr/local/lib/libompl.so.6
#3 0x00002b341e1de0d7 in ompl::geometric::RRTstar::updateChildCosts(ompl::geometric::RRTstar::Motion*, double) () from /usr/local/lib/libompl.so.6
#4 0x00002b341e1de0d7 in ompl::geometric::RRTstar::updateChildCosts(ompl::geometric::RRTstar::Motion*, double) () from /usr/local/lib/libompl.so.6
#5 0x00002b341e1de0d7 in ompl::geometric::RRTstar::updateChildCosts(ompl::geometric::RRTstar::Motion*, double) () from /usr/local/lib/libompl.so.6
#6 0x00002b341e1de0d7 in ompl::geometric::RRTstar::updateChildCosts(ompl::geometric::RRTstar::Motion*, double) () from /usr/local/lib/libompl.so.6
#7 0x00002b341e1de0d7 in ompl::geometric::RRTstar::updateChildCosts(ompl::geometric::RRTstar::Motion*, double) () from /usr/local/lib/libompl.so.6
#8 0x00002b341e1de0d7 in ompl::geometric::RRTstar::updateChildCosts(ompl::geometric::RRTstar::Motion*, double) () from /usr/local/lib/libompl.so.6
#9 0x00002b341e1de0d7 in ompl::geometric::RRTstar::updateChildCosts(ompl::geometric::RRTstar::Motion*, double) () from /usr/local/lib/libompl.so.6
#10 0x00002b341e1de0d7 in ompl::geometric::RRTstar::updateChildCosts(ompl::geometric::RRTstar::Motion*, double) () from /usr/local/lib/libompl.so.6
...
...
#43577 0x00002b341e1de0d7 in ompl::geometric::RRTstar::updateChildCosts(ompl::geometric::RRTstar::Motion*, double) () from /usr/local/lib/libompl.so.6
#43578 0x00002b341e1de0d7 in ompl::geometric::RRTstar::updateChildCosts(ompl::geometric::RRTstar::Motion*, double) () from /usr/local/lib/libompl.so.6
#43579 0x00002b341e1de0d7 in ompl::geometric::RRTstar::updateChildCosts(ompl::geometric::RRTstar::Motion*, double) () from /usr/local/lib/libompl.so.6
#43580 0x00002b341e1e0b55 in ompl::geometric::RRTstar::solve(ompl::base::PlannerTerminationCondition const&) () from /usr/local/lib/libompl.so.6
#43581 0x00002b341e27a847 in ompl::tools::ParallelPlan::solveMore(ompl::base::Planner*, unsigned long, unsigned long, ompl::base::PlannerTerminationCondition const*) () from /usr/local/lib/libompl.so.6
#43582 0x00002b3413ce1ba9 in thread_proxy () from /usr//lib/libboost_thread.so.1.46.1
#43583 0x00002b341468defc in start_thread (arg=0x2b3425438700) at pthread_create.c:304
#43584 0x00002b3414987f8d in clone () at ../sysdeps/unix/sysv/linux/x86_64/clone.S:112
#43585 0x0000000000000000 in ?? ()
Thread 9:
(gdb) bt
#0 0x00002b341fe64afe in fcl::obbDisjoint(fcl::Matrix3fX<fcl::details::Matrix3Data<double> > const&, fcl::Vec3fX<fcl::details::Vec3Data<double> > const&, fcl::Vec3fX<fcl::details::Vec3Data<double> > const&, fcl::Vec3fX<fcl::details::Vec3Data<double> > const&) () from /opt/ros/groovy/lib/libfcl.so
#1 0x00002b341fe65458 in fcl::overlap(fcl::Matrix3fX<fcl::details::Matrix3Data<double> > const&, fcl::Vec3fX<fcl::details::Vec3Data<double> > const&, fcl::OBB const&, fcl::OBB const&) ()
from /opt/ros/groovy/lib/libfcl.so
#2 0x00002b341ffdcc89 in fcl::MeshCollisionTraversalNodeOBBRSS::BVTesting(int, int) const () from /opt/ros/groovy/lib/libfcl.so
#3 0x00002b341ffdb14d in fcl::collisionRecurse(fcl::CollisionTraversalNodeBase*, int, int, std::list<fcl::BVHFrontNode, std::allocator<fcl::BVHFrontNode> >*) () from /opt/ros/groovy/lib/libfcl.so
#4 0x00002b341fef193b in unsigned long fcl::details::orientedMeshCollide<fcl::MeshCollisionTraversalNodeOBBRSS, fcl::OBBRSS>(fcl::CollisionGeometry const*, fcl::Transform3f const&, fcl::CollisionGeometry const*, fcl::Transform3f const&, fcl::CollisionRequest const&, fcl::CollisionResult&) () from /opt/ros/groovy/lib/libfcl.so
#5 0x00002b341fe6e6fe in unsigned long fcl::collide<fcl::GJKSolver_libccd>(fcl::CollisionGeometry const*, fcl::Transform3f const&, fcl::CollisionGeometry const*, fcl::Transform3f const&, fcl::GJKSolver_libccd const*, fcl::CollisionRequest const&, fcl::CollisionResult&) () from /opt/ros/groovy/lib/libfcl.so
#6 0x00002b341fe6e8a4 in unsigned long fcl::collide<fcl::GJKSolver_libccd>(fcl::CollisionObject const*, fcl::CollisionObject const*, fcl::GJKSolver_libccd const*, fcl::CollisionRequest const&, fcl::CollisionResult&) () from /opt/ros/groovy/lib/libfcl.so
#7 0x00002b341fe6e525 in fcl::collide(fcl::CollisionObject const*, fcl::CollisionObject const*, fcl::CollisionRequest const&, fcl::CollisionResult&) () from /opt/ros/groovy/lib/libfcl.so
#8 0x00002b341cf05633 in collision_detection::collisionCallback(fcl::CollisionObject*, fcl::CollisionObject*, void*) () from /home/ps/_science/ros/moveit_src/devel/lib/libmoveit_collision_detection_fcl.so
#9 0x00002b342007e1ff in fcl::details::dynamic_AABB_tree::collisionRecurse(fcl::NodeBase<fcl::AABB>*, fcl::CollisionObject*, void*, bool (*)(fcl::CollisionObject*, fcl::CollisionObject*, void*)) ()
from /opt/ros/groovy/lib/libfcl.so
#10 0x00002b342007e1ff in fcl::details::dynamic_AABB_tree::collisionRecurse(fcl::NodeBase<fcl::AABB>*, fcl::CollisionObject*, void*, bool (*)(fcl::CollisionObject*, fcl::CollisionObject*, void*)) ()
from /opt/ros/groovy/lib/libfcl.so
#11 0x00002b342007e1ff in fcl::details::dynamic_AABB_tree::collisionRecurse(fcl::NodeBase<fcl::AABB>*, fcl::CollisionObject*, void*, bool (*)(fcl::CollisionObject*, fcl::CollisionObject*, void*)) ()
from /opt/ros/groovy/lib/libfcl.so
#12 0x00002b3420081869 in fcl::DynamicAABBTreeCollisionManager::collide(fcl::CollisionObject*, void*, bool (*)(fcl::CollisionObject*, fcl::CollisionObject*, void*)) const () from /opt/ros/groovy/lib/libfcl.so
#13 0x00002b341cf15390 in collision_detection::CollisionWorldFCL::checkRobotCollisionHelper(collision_detection::CollisionRequest const&, collision_detection::CollisionResult&, collision_detection::CollisionRobot const&, robot_state::RobotState const&, collision_detection::AllowedCollisionMatrix const*) const () from /home/ps/_science/ros/moveit_src/devel/lib/libmoveit_collision_detection_fcl.so
#14 0x00002b34198f363b in planning_scene::PlanningScene::checkCollision(collision_detection::CollisionRequest const&, collision_detection::CollisionResult&, robot_state::RobotState const&) const ()
from /home/ps/_science/ros/moveit_src/devel/lib/libmoveit_planning_scene.so
#15 0x00002b341a67362f in ompl_interface::StateValidityChecker::isValidWithCache(ompl::base::State const*, bool) const () from /home/ps/_science/ros/moveit_src/devel/lib/libmoveit_ompl_interface.so
#16 0x00002b341a677124 in ompl_interface::ConstrainedGoalSampler::sampleUsingConstraintSampler(ompl::base::GoalLazySamples const*, ompl::base::State*) ()
from /home/ps/_science/ros/moveit_src/devel/lib/libmoveit_ompl_interface.so
#17 0x00002b341e0d6e3b in ompl::base::GoalLazySamples::goalSamplingThread() () from /usr/local/lib/libompl.so.6
#18 0x00002b3413ce1ba9 in thread_proxy () from /usr//lib/libboost_thread.so.1.46.1
#19 0x00002b341468defc in start_thread (arg=0x2b3425237700) at pthread_create.c:304
#20 0x00002b3414987f8d in clone () at ../sysdeps/unix/sysv/linux/x86_64/clone.S:112
#21 0x0000000000000000 in ?? ()
Thread 8:
(gdb) bt
#0 pthread_cond_wait@@GLIBC_2.3.2 () at ../nptl/sysdeps/unix/sysv/linux/x86_64/pthread_cond_wait.S:162
#1 0x00002b3413ce2e9b in boost::thread::join() () from /usr//lib/libboost_thread.so.1.46.1
#2 0x00002b341e279bbc in ompl::tools::ParallelPlan::solve(ompl::base::PlannerTerminationCondition const&, unsigned long, unsigned long, bool) () from /usr/local/lib/libompl.so.6
#3 0x00002b341a65c433 in ompl_interface::ModelBasedPlanningContext::solve(double, unsigned int) () from /home/ps/_science/ros/moveit_src/devel/lib/libmoveit_ompl_interface.so
#4 0x00002b341a62c415 in ompl_interface::OMPLInterface::solve(boost::shared_ptr<planning_scene::PlanningScene const> const&, moveit_msgs::MotionPlanRequest_<std::allocator<void> > const&, planning_interface::MotionPlanResponse&) const () from /home/ps/_science/ros/moveit_src/devel/lib/libmoveit_ompl_interface.so
#5 0x00002b3418bbff88 in pm::PlanMotion::Impl::pathExists(int, int, int, int, double*, pm::PlanMotion::PlannerConfiguration const&, std::vector<pm::PlanMotion::Obstacle, std::allocator<pm::PlanMotion::Obstacle> > const&) const () from /home/ps/_science/planning_levels_of_integration/experiments/ros_catkin_ws/devel/lib/libhousekeep-util.so
#6 0x00002b3418bbcf30 in pm::PlanMotion::pathExists(int, int, int, int, double*, pm::PlanMotion::PlannerConfiguration const&, std::vector<pm::PlanMotion::Obstacle, std::allocator<pm::PlanMotion::Obstacle> > const&) const () from /home/ps/_science/planning_levels_of_integration/experiments/ros_catkin_ws/devel/lib/libhousekeep-util.so
#7 0x00002b341882861b in dlvhex::housekeeping::CumulativePathExistsObstaclesAtom::retrieve(dlvhex::PluginAtom::Query const&, dlvhex::PluginAtom::Answer&, boost::shared_ptr<dlvhex::NogoodContainer>) ()
from ./lib/libdlvhexplugin-housekeeping.so
#8 0x00002b341323561c in dlvhex::BaseModelGenerator::evaluateExternalAtomQuery(dlvhex::PluginAtom::Query&, dlvhex::BaseModelGenerator::ExternalAnswerTupleCallback&, boost::shared_ptr<dlvhex::NogoodContainer>) const () from /home/ps/_science/hex/core/build_benchmark_boost146/install/lib/libdlvhex2-base.so.7
#9 0x00002b34132364a3 in dlvhex::BaseModelGenerator::evaluateExternalAtom(dlvhex::ProgramCtx&, dlvhex::ExternalAtom const&, boost::shared_ptr<dlvhex::Interpretation const>, dlvhex::BaseModelGenerator::ExternalAnswerTupleCallback&, boost::shared_ptr<dlvhex::NogoodContainer>) const () from /home/ps/_science/hex/core/build_benchmark_boost146/install/lib/libdlvhex2-base.so.7
#10 0x00002b34132e55ff in dlvhex::GenuineGuessAndCheckModelGenerator::verifyExternalAtom(int, boost::shared_ptr<dlvhex::Interpretation const>, boost::shared_ptr<dlvhex::Interpretation const>, boost::shared_ptr<dlvhex::Interpretation const>) () from /home/ps/_science/hex/core/build_benchmark_boost146/install/lib/libdlvhex2-base.so.7
#11 0x00002b34132e653a in dlvhex::GenuineGuessAndCheckModelGenerator::verifyExternalAtoms(boost::shared_ptr<dlvhex::Interpretation const>, boost::shared_ptr<dlvhex::Interpretation const>, boost::shared_ptr<dlvhex::Interpretation const>) () from /home/ps/_science/hex/core/build_benchmark_boost146/install/lib/libdlvhex2-base.so.7
#12 0x00002b34132e6a12 in dlvhex::GenuineGuessAndCheckModelGenerator::propagate(boost::shared_ptr<dlvhex::Interpretation const>, boost::shared_ptr<dlvhex::Interpretation const>, boost::shared_ptr<dlvhex::Interpretation const>) () from /home/ps/_science/hex/core/build_benchmark_boost146/install/lib/libdlvhex2-base.so.7
#13 0x00002b3413259740 in dlvhex::ClaspSolver::ExternalPropagator::prop(Clasp::Solver&) () from /home/ps/_science/hex/core/build_benchmark_boost146/install/lib/libdlvhex2-base.so.7
#14 0x00002b34132625f5 in dlvhex::ClaspSolver::ExternalPropagator::propagate(Clasp::Solver&) () from /home/ps/_science/hex/core/build_benchmark_boost146/install/lib/libdlvhex2-base.so.7
#15 0x00002b34134842ae in Clasp::PostPropagator::propagateFixpoint(Clasp::Solver&) () from /home/ps/_science/hex/core/build_benchmark_boost146/install/lib/libdlvhex2-base.so.7
#16 0x00002b3413484173 in Clasp::Solver::PPList::propagate(Clasp::Solver&, Clasp::PostPropagator*) const () from /home/ps/_science/hex/core/build_benchmark_boost146/install/lib/libdlvhex2-base.so.7
#17 0x00002b3413486cab in Clasp::Solver::propagate() () from /home/ps/_science/hex/core/build_benchmark_boost146/install/lib/libdlvhex2-base.so.7
#18 0x00002b341348bb52 in Clasp::Solver::search(Clasp::SearchLimits&, double) () from /home/ps/_science/hex/core/build_benchmark_boost146/install/lib/libdlvhex2-base.so.7
#19 0x00002b3413481c73 in Clasp::SolveAlgorithm::solvePath(Clasp::Solver&, Clasp::SolveParams const&, Clasp::SolveLimits&) ()
from /home/ps/_science/hex/core/build_benchmark_boost146/install/lib/libdlvhex2-base.so.7
#20 0x00002b3413482a49 in Clasp::SimpleSolve::doSolve(Clasp::Solver&, Clasp::SolveParams const&) () from /home/ps/_science/hex/core/build_benchmark_boost146/install/lib/libdlvhex2-base.so.7
#21 0x00002b3413480f60 in Clasp::SolveAlgorithm::solve(Clasp::SharedContext&, Clasp::SolveParams const&, bk_lib::pod_vector<Clasp::Literal, std::allocator<Clasp::Literal> > const&) ()
from /home/ps/_science/hex/core/build_benchmark_boost146/install/lib/libdlvhex2-base.so.7
#22 0x00002b3413480d8b in Clasp::solve(Clasp::SharedContext&, Clasp::SolveParams const&, bk_lib::pod_vector<Clasp::Literal, std::allocator<Clasp::Literal> > const&, Clasp::SolveLimits const&) ()
from /home/ps/_science/hex/core/build_benchmark_boost146/install/lib/libdlvhex2-base.so.7
#23 0x00002b3413258cf5 in dlvhex::ClaspSolver::runClasp() () from /home/ps/_science/hex/core/build_benchmark_boost146/install/lib/libdlvhex2-base.so.7
#24 0x00002b3413ce1ba9 in thread_proxy () from /usr//lib/libboost_thread.so.1.46.1
#25 0x00002b341468defc in start_thread (arg=0x2b3425033700) at pthread_create.c:304
#26 0x00002b3414987f8d in clone () at ../sysdeps/unix/sysv/linux/x86_64/clone.S:112
#27 0x0000000000000000 in ?? ()
Thread 2:
(gdb) bt
#0 __memcmp_sse4_1 () at ../sysdeps/x86_64/multiarch/memcmp-sse4.S:45
#1 0x00002b341420ac73 in std::string::compare(std::string const&) const () from /usr/lib/x86_64-linux-gnu/libstdc++.so.6
#2 0x00002b341340e989 in bool std::operator< <char, std::char_traits<char>, std::allocator<char> >(std::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::basic_string<char, std::char_traits<char>, std::allocator<char> > const&) () from /home/ps/_science/hex/core/build_benchmark_boost146/install/lib/libdlvhex2-base.so.7
#3 0x00002b3419b4df42 in collision_detection::AllowedCollisionMatrix::getEntry(std::string const&, std::string const&, collision_detection::AllowedCollision::Type&) const ()
from /home/ps/_science/ros/moveit_src/devel/lib/libmoveit_collision_detection.so
#4 0x00002b3419b4e0b1 in collision_detection::AllowedCollisionMatrix::getAllowedCollision(std::string const&, std::string const&, collision_detection::AllowedCollision::Type&) const ()
from /home/ps/_science/ros/moveit_src/devel/lib/libmoveit_collision_detection.so
#5 0x00002b341cf042fc in collision_detection::collisionCallback(fcl::CollisionObject*, fcl::CollisionObject*, void*) () from /home/ps/_science/ros/moveit_src/devel/lib/libmoveit_collision_detection_fcl.so
#6 0x00002b342007e1ff in fcl::details::dynamic_AABB_tree::collisionRecurse(fcl::NodeBase<fcl::AABB>*, fcl::CollisionObject*, void*, bool (*)(fcl::CollisionObject*, fcl::CollisionObject*, void*)) ()
from /opt/ros/groovy/lib/libfcl.so
#7 0x00002b342007e1ff in fcl::details::dynamic_AABB_tree::collisionRecurse(fcl::NodeBase<fcl::AABB>*, fcl::CollisionObject*, void*, bool (*)(fcl::CollisionObject*, fcl::CollisionObject*, void*)) ()
from /opt/ros/groovy/lib/libfcl.so
#8 0x00002b3420081869 in fcl::DynamicAABBTreeCollisionManager::collide(fcl::CollisionObject*, void*, bool (*)(fcl::CollisionObject*, fcl::CollisionObject*, void*)) const () from /opt/ros/groovy/lib/libfcl.so
#9 0x00002b341cf15390 in collision_detection::CollisionWorldFCL::checkRobotCollisionHelper(collision_detection::CollisionRequest const&, collision_detection::CollisionResult&, collision_detection::CollisionRobot const&, robot_state::RobotState const&, collision_detection::AllowedCollisionMatrix const*) const () from /home/ps/_science/ros/moveit_src/devel/lib/libmoveit_collision_detection_fcl.so
#10 0x00002b34198f363b in planning_scene::PlanningScene::checkCollision(collision_detection::CollisionRequest const&, collision_detection::CollisionResult&, robot_state::RobotState const&) const ()
from /home/ps/_science/ros/moveit_src/devel/lib/libmoveit_planning_scene.so
#11 0x00002b341a67362f in ompl_interface::StateValidityChecker::isValidWithCache(ompl::base::State const*, bool) const () from /home/ps/_science/ros/moveit_src/devel/lib/libmoveit_ompl_interface.so
#12 0x00002b341e132f27 in ompl::base::DiscreteMotionValidator::checkMotion(ompl::base::State const*, ompl::base::State const*) const () from /usr/local/lib/libompl.so.6
#13 0x00002b341e1e0964 in ompl::geometric::RRTstar::solve(ompl::base::PlannerTerminationCondition const&) () from /usr/local/lib/libompl.so.6
#14 0x00002b341e27a847 in ompl::tools::ParallelPlan::solveMore(ompl::base::Planner*, unsigned long, unsigned long, ompl::base::PlannerTerminationCondition const*) () from /usr/local/lib/libompl.so.6
#15 0x00002b3413ce1ba9 in thread_proxy () from /usr//lib/libboost_thread.so.1.46.1
#16 0x00002b341468defc in start_thread (arg=0x2b3425639700) at pthread_create.c:304
#17 0x00002b3414987f8d in clone () at ../sysdeps/unix/sysv/linux/x86_64/clone.S:112
#18 0x0000000000000000 in ?? ()
(The other threads are roscpp, ros-xmlrpc threads and a logic programming solver.)
Do you have any idea how I could reproduce the problem more deterministically? Can I set a random seed and a fixed number of computations after which RRTstar has to give up, instead of a timeout?