search

flexible-collision-library / fcl

Flexible Collision Library
https://flexible-collision-library.github.io/
Other
1.38k stars 416 forks source link

crash in HierarchyTree<fcl::AABB>::bottomup #521

Open christian-rauch opened 3 years ago

christian-rauch commented 3 years ago

I am using fcl 0.5 through RViz and recently got a reproducible crash:

#0  0x00007fff4b9a6d1b in fcl::HierarchyTree<fcl::AABB>::bottomup(__gnu_cxx::__normal_iterator<fcl::NodeBase<fcl::AABB>**, std::vector<fcl::NodeBase<fcl::AABB>*, std::allocator<fcl::NodeBase<fcl::AABB>*> > >, __gnu_cxx::__normal_iterator<fcl::NodeBase<fcl::AABB>**, std::vector<fcl::NodeBase<fcl::AABB>*, std::allocator<fcl::NodeBase<fcl::AABB>*> > >) () at /lib/x86_64-linux-gnu/libfcl.so.0.5
#1  0x00007fff4b9a7c25 in fcl::HierarchyTree<fcl::AABB>::topdown_0(__gnu_cxx::__normal_iterator<fcl::NodeBase<fcl::AABB>**, std::vector<fcl::NodeBase<fcl::AABB>*, std::allocator<fcl::NodeBase<fcl::AABB>*> > >, __gnu_cxx::__normal_iterator<fcl::NodeBase<fcl::AABB>**, std::vector<fcl::NodeBase<fcl::AABB>*, std::allocator<fcl::NodeBase<fcl::AABB>*> > >) () at /lib/x86_64-linux-gnu/libfcl.so.0.5
#2  0x00007fff4b9a7c06 in fcl::HierarchyTree<fcl::AABB>::topdown_0(__gnu_cxx::__normal_iterator<fcl::NodeBase<fcl::AABB>**, std::vector<fcl::NodeBase<fcl::AABB>*, std::allocator<fcl::NodeBase<fcl::AABB>*> > >, __gnu_cxx::__normal_iterator<fcl::NodeBase<fcl::AABB>**, std::vector<fcl::NodeBase<fcl::AABB>*, std::allocator<fcl::NodeBase<fcl::AABB>*> > >) () at /lib/x86_64-linux-gnu/libfcl.so.0.5
#3  0x00007fff4b9a7bf4 in fcl::HierarchyTree<fcl::AABB>::topdown_0(__gnu_cxx::__normal_iterator<fcl::NodeBase<fcl::AABB>**, std::vector<fcl::NodeBase<fcl::AABB>*, std::allocator<fcl::NodeBase<fcl::AABB>*> > >, __gnu_cxx::__normal_iterator<fcl::NodeBase<fcl::AABB>**, std::vector<fcl::NodeBase<fcl::AABB>*, std::allocator<fcl::NodeBase<fcl::AABB>*> > >) () at /lib/x86_64-linux-gnu/libfcl.so.0.5
#4  0x00007fff4b9a7c06 in fcl::HierarchyTree<fcl::AABB>::topdown_0(__gnu_cxx::__normal_iterator<fcl::NodeBase<fcl::AABB>**, std::vector<fcl::NodeBase<fcl::AABB>*, std::allocator<fcl::NodeBase<fcl::AABB>*> > >, __gnu_cxx::__normal_iterator<fcl::NodeBase<fcl::AABB>**, std::vector<fcl::NodeBase<fcl::AABB>*, std::allocator<fcl::NodeBase<fcl::AABB>*> > >) () at /lib/x86_64-linux-gnu/libfcl.so.0.5
#5  0x00007fff4b9a7bf4 in fcl::HierarchyTree<fcl::AABB>::topdown_0(__gnu_cxx::__normal_iterator<fcl::NodeBase<fcl::AABB>**, std::vector<fcl::NodeBase<fcl::AABB>*, std::allocator<fcl::NodeBase<fcl::AABB>*> > >, __gnu_cxx::__normal_iterator<fcl::NodeBase<fcl::AABB>**, std::vector<fcl::NodeBase<fcl::AABB>*, std::allocator<fcl::NodeBase<fcl::AABB>*> > >) () at /lib/x86_64-linux-gnu/libfcl.so.0.5
#6  0x00007fff4b9a7bf4 in fcl::HierarchyTree<fcl::AABB>::topdown_0(__gnu_cxx::__normal_iterator<fcl::NodeBase<fcl::AABB>**, std::vector<fcl::NodeBase<fcl::AABB>*, std::allocator<fcl::NodeBase<fcl::AABB>*> > >, __gnu_cxx::__normal_iterator<fcl::NodeBase<fcl::AABB>**, std::vector<fcl::NodeBase<fcl::AABB>*, std::allocator<fcl::NodeBase<fcl::AABB>*> > >) () at /lib/x86_64-linux-gnu/libfcl.so.0.5
#7  0x00007fff4b9a7e65 in fcl::HierarchyTree<fcl::AABB>::init_0(std::vector<fcl::NodeBase<fcl::AABB>*, std::allocator<fcl::NodeBase<fcl::AABB>*> >&) ()
    at /lib/x86_64-linux-gnu/libfcl.so.0.5
#8  0x00007fff4b9a5a2a in fcl::DynamicAABBTreeCollisionManager::registerObjects(std::vector<fcl::CollisionObject*, std::allocator<fcl::CollisionObject*> > const&) () at /lib/x86_64-linux-gnu/libfcl.so.0.5
#9  0x00007fff5cd58d68 in collision_detection::FCLObject::registerTo(fcl::BroadPhaseCollisionManager*) ()
    at /opt/ros/noetic/lib/libmoveit_collision_detection_fcl.so.1.1.0
#10 0x00007fff5cd6ff19 in collision_detection::CollisionEnvFCL::checkSelfCollisionHelper(collision_detection::CollisionRequest const&, collision_detection::CollisionResult&, moveit::core::RobotState const&, collision_detection::AllowedCollisionMatrix const*) const ()
    at /opt/ros/noetic/lib/libmoveit_collision_detection_fcl.so.1.1.0
#11 0x00007fff5db6f544 in planning_scene::PlanningScene::isStateColliding(moveit::core::RobotState const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool) const ()
    at /opt/ros/noetic/lib/libmoveit_planning_scene.so.1.1.0
#12 0x00007fff801956d5 in moveit_rviz_plugin::MotionPlanningDisplay::isIKSolutionCollisionFree(moveit::core::RobotState*, moveit::core::JointModelGroup const*, double const*) const ()
    at /opt/ros/noetic/lib/libmoveit_motion_planning_rviz_plugin_core.so.1.1.0
#13 0x00007fff5dc87943 in  ()
    at /opt/ros/noetic/lib/libmoveit_robot_state.so.1.1.0
#14 0x00007fff3abc7565 in kdl_kinematics_plugin::KDLKinematicsPlugin::searchPositionIK(geometry_msgs::Pose_<std::allocator<void> > const&, std::vector<double, std::allocator<double> > const&, double, std::vector<double, std::allocator<double> > const&, std::vector<double, std::allocator<double> >&, boost::function<void (geometry_msgs::Pose_<std::allocator<void> > const&, std::vector<double, std::allocator<double> > const&, moveit_msgs::MoveItErrorCodes_<std::allocator<void> >&)> const&, moveit_msgs::MoveItErrorCodes_<std::allocator<void> >&, kinematics::KinematicsQueryOptions const&) const ()
    at /opt/ros/noetic/lib//libmoveit_kdl_kinematics_plugin.so
#15 0x00007fff5c8d4d22 in kinematics::KinematicsBase::searchPositionIK(std::vector<geometry_msgs::Pose_<std::allocator<void> >, std::allocator<geometry_msgs::Pose_<std::allocator<void> > > > const&, std::vector<double, std::allocator<double> > const&, double, std::vector<double, std::allocator<double> > const&, std::vector<double, std::allocator<double> >&, boost::function<void (geometry_msgs::Pose_<std::allocator<void> > const&, std::vector<double, std::allocator<double> > const&, moveit_msgs::MoveItErrorCodes_<std::allocator<void> >&)> const&, moveit_msgs::MoveItErrorCodes_<std::allocator<void> >&, kinematics::KinematicsQueryOptions const&, moveit::core::RobotState const*) const ()
    at /opt/ros/noetic/lib/libmoveit_kinematics_base.so.1.1.0
#16 0x00007fff5dc917bd in moveit::core::RobotState::setFromIK(moveit::core::JointModelGroup const*, std::vector<Eigen::Transform<double, 3, 1, 0>, Eigen::aligned_allocator<Eigen::Transform<double, 3, 1, 0> > > const&, std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > const&, std::vector<std::vector<double, std::allocator<double> >, std::allocator<std::vector<double, std::allocator<double> > > > const&, double, boost::function<bool (moveit::core::RobotState*, moveit::core::JointModelGroup const*, double const*)> const&, kinematics::KinematicsQueryOptions const&)
    () at /opt/ros/noetic/lib/libmoveit_robot_state.so.1.1.0
#17 0x00007fff5dc91e03 in moveit::core::RobotState::setFromIK(moveit::core::JointModelGroup const*, Eigen::Transform<double, 3, 1, 0> const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::vector<double, std::allocator<double> > const&, double, boost::function<bool (moveit::core::RobotState*, moveit::core::JointModelGroup const*, double const*)> const&, kinematics::KinematicsQueryOptions const&) ()
    at /opt/ros/noetic/lib/libmoveit_robot_state.so.1.1.0
#18 0x00007fff5dc9212c in moveit::core::RobotState::setFromIK(moveit::core::JointModelGroup const*, geometry_msgs::Pose_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, double, boost::function<bool (moveit::core::RobotState*, moveit::core::JointModelGroup const*, double const*)> const&, kinematics::KinematicsQueryOptions const&) () at /opt/ros/noetic/lib/libmoveit_robot_state.so.1.1.0
#19 0x00007fff5de2fab7 in robot_interaction::KinematicOptions::setStateFromIK(moveit::core::RobotState&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, geometry_msgs::Pose_<std::allocator<void> > const&) const ()
    at /opt/ros/noetic/lib/libmoveit_robot_interaction.so.1.1.0
#20 0x00007fff5de3f160 in robot_interaction::InteractionHandler::updateStateEndEffector(moveit::core::RobotState*, robot_interaction::EndEffectorInteraction const*, geometry_msgs::Pose_<std::allocator<void> > const*, boost::function<void (robot_interaction::InteractionHandler*)>*) ()
    at /opt/ros/noetic/lib/libmoveit_robot_interaction.so.1.1.0
#21 0x00007fff5de3b121 in robot_interaction::LockedRobotState::modifyState(boost::function<void (moveit::core::RobotState*)> const&) ()
    at /opt/ros/noetic/lib/libmoveit_robot_interaction.so.1.1.0
#22 0x00007fff5de415f2 in robot_interaction::InteractionHandler::handleEndEffector(robot_interaction::EndEffectorInteraction const&, boost::shared_ptr<visualization_msgs::InteractiveMarkerFeedback_<std::allocator<void> > const> const&)
    () at /opt/ros/noetic/lib/libmoveit_robot_interaction.so.1.1.0
#23 0x00007fff5de49875 in robot_interaction::RobotInteraction::processingThread() () at /opt/ros/noetic/lib/libmoveit_robot_interaction.so.1.1.0
#24 0x00007ffff6c6e43b in  ()
    at /lib/x86_64-linux-gnu/libboost_thread.so.1.71.0
#25 0x00007ffff689b609 in start_thread (arg=<optimised out>)
    at pthread_create.c:477
#26 0x00007ffff6e5a293 in clone ()
    at ../sysdeps/unix/sysv/linux/x86_64/clone.S:95
JStech commented 3 years ago

I recommend you double-check that the RobotState isn't corrupted. I had a segfault with a nearly-identical stack trace and it was because there were NaN values in the collision body transforms.

christian-rauch commented 3 years ago

Thanks for the hint. Back when this happened it was reproducible, but I actually cannot remember what I did back then, so it is technically not reproducible anymore :-)

Anyway, in such situations, it would be useful to check the input and print an error or throw an exception (that is caught by .g. RViz) instead of tearing down the process.