Segfault when creating multiple tasks

[JSchaaf2]

I was creating a minimal example for another issue and finally stumbled on a something that looks a lot like another issue I had but never found a minimal example for.

modefied doTask() from pick and place tutorial

```c++ void MTCTaskNode::doTask() { task_ = createTask(); task2 = createTask(); try { task_.init(); } catch (mtc::InitStageException& e) { RCLCPP_ERROR_STREAM(LOGGER, e); return; } if (!task_.plan(5)) { RCLCPP_ERROR_STREAM(LOGGER, "Task planning failed"); return; } try { task2.init(); } catch (mtc::InitStageException& e) { RCLCPP_ERROR_STREAM(LOGGER, e); return; } if (!task2.plan(5)) { RCLCPP_ERROR_STREAM(LOGGER, "Task planning failed"); return; } return; } ```

The above code causes a segfault in the collision checking of the IK wrapper for the second task and the following warning is very rarely given (I have only seen it once): [moveit_task_constructor_visualization.task_list_model]: unknown task: ... . This might not be the only situation this happens but I have no other examples that are simple or fail reliably. The other cases also didn't show the warning so I don't know if that is just something I did wrong. Changing the code such that the task2 = createTask(); line appears after the first one is initialised, or instead of being class members declaring the tasks like:

    mtc::Task task_ = createTask();
    mtc::Task task2 = createTask();

makes the segfault go away.

[JafarAbdi]

Do you mind sharing the stacktrace when it segfaults?

instead of being class members declaring the tasks like:

mtc::Task task_ = createTask(); mtc::Task task2 = createTask();

makes the segfault go away.

I wonder if it's related to the class implementation, is it possible to share the whole class code?

[JSchaaf2]

This is backtrace I get

```bash #0 0x00007ffff6382100 in ?? () from target:/lib/x86_64-linux-gnu/libfcl.so.0.7 #1 0x00007ffff63824a7 in ?? () from target:/lib/x86_64-linux-gnu/libfcl.so.0.7 #2 0x00007ffff6382545 in ?? () from target:/lib/x86_64-linux-gnu/libfcl.so.0.7 #3 0x00007ffff6382545 in ?? () from target:/lib/x86_64-linux-gnu/libfcl.so.0.7 #4 0x00007ffff637f32b in fcl::DynamicAABBTreeCollisionManager::registerObjects(std::vector*, std::allocator*> > const&) () from target:/lib/x86_64-linux-gnu/libfcl.so.0.7 #5 0x00007ffff70ad676 in collision_detection::FCLObject::registerTo(fcl::BroadPhaseCollisionManager*) () from target:/opt/ros/humble/lib/libmoveit_collision_detection_fcl.so.2.5.3 #6 0x00007ffff70b5f5f in collision_detection::CollisionEnvFCL::checkSelfCollisionHelper(collision_detection::CollisionRequest const&, collision_detection::CollisionResult&, moveit::core::RobotState const&, collision_detection::AllowedCollisionMatrix const*) const () from target:/opt/ros/humble/lib/libmoveit_collision_detection_fcl.so.2.5.3 #7 0x00007ffff7e40b50 in planning_scene::PlanningScene::checkCollision (acm=..., robot_state=..., res=..., req=..., this=0x5555567b4a10) at /opt/ros/humble/include/moveit/planning_scene/planning_scene.h:371 #8 moveit::task_constructor::stages::(anonymous namespace)::isTargetPoseCollidingInEEF (scene=..., robot_state=..., pose=..., link=, collision_result=0x7fffffff9640) at /home/jasper/MTC/src/moveit_task_constructor/core/src/stages/compute_ik.cpp:133 #9 0x00007ffff7e42553 in moveit::task_constructor::stages::ComputeIK::compute (this=0x555555e5ea30) at /home/jasper/MTC/src/moveit_task_constructor/core/src/stages/compute_ik.cpp:312 #10 0x00007ffff7ee9b1c in moveit::task_constructor::StagePrivate::runCompute (this=0x555555e5f740) at /home/jasper/MTC/src/moveit_task_constructor/core/include/moveit/task_constructor/stage_p.h:149 #11 0x00007ffff7ede00f in moveit::task_constructor::SerialContainer::compute (this=) at /home/jasper/MTC/src/moveit_task_constructor/core/src/container.cpp:684 #12 0x00007ffff7ee9b1c in moveit::task_constructor::StagePrivate::runCompute (this=0x555555e5d2a0) at /home/jasper/MTC/src/moveit_task_constructor/core/include/moveit/task_constructor/stage_p.h:149 #13 0x00007ffff7ede00f in moveit::task_constructor::SerialContainer::compute (this=) at /home/jasper/MTC/src/moveit_task_constructor/core/src/container.cpp:684 #14 0x00007ffff7ee9b1c in moveit::task_constructor::StagePrivate::runCompute (this=0x55555562cc30) at /home/jasper/MTC/src/moveit_task_constructor/core/include/moveit/task_constructor/stage_p.h:149 #15 0x00007ffff7f3a7b7 in moveit::task_constructor::Task::compute (this=0x55555562e600) at /home/jasper/MTC/src/moveit_task_constructor/core/include/moveit/task_constructor/container_p.h:190 #16 moveit::task_constructor::Task::plan (this=this@entry=0x55555562e600, max_solutions=max_solutions@entry=5) at /home/jasper/MTC/src/moveit_task_constructor/core/src/task.cpp:254 #17 0x0000555555561c1d in MTCTaskNode::doTask (this=0x55555562e5f0) at /home/jasper/MTC_minmal_example/src/mtc_tutorial.cpp:99 #18 0x000055555555d151 in main (argc=, argv=) at /usr/include/c++/11/bits/shared_ptr_base.h:1295 ```

It is the tutorial code from the pick and place example in the moveit humble docs.

My class looks like this

```c++ class MTCTaskNode { public: MTCTaskNode(const rclcpp::NodeOptions& options); rclcpp::node_interfaces::NodeBaseInterface::SharedPtr getNodeBaseInterface(); void doTask(); void setupPlanningScene(); private: // Compose an MTC task from a series of stages. mtc::Task createTask(); mtc::Task task_; mtc::Task task2; //I added this line rclcpp::Node::SharedPtr node_; }; ```

Here is the entire file to save having to look it up in the git or copy paste from the example

```c++ #include #include #include #include #include #include #if __has_include() #include #else #include #endif #if __has_include() #include #else #include #endif static const rclcpp::Logger LOGGER = rclcpp::get_logger("mtc_tutorial"); namespace mtc = moveit::task_constructor; class MTCTaskNode { public: MTCTaskNode(const rclcpp::NodeOptions& options); rclcpp::node_interfaces::NodeBaseInterface::SharedPtr getNodeBaseInterface(); void doTask(); void setupPlanningScene(); private: // Compose an MTC task from a series of stages. mtc::Task createTask(); mtc::Task task_; mtc::Task task2; rclcpp::Node::SharedPtr node_; }; rclcpp::node_interfaces::NodeBaseInterface::SharedPtr MTCTaskNode::getNodeBaseInterface() { return node_->get_node_base_interface(); } MTCTaskNode::MTCTaskNode(const rclcpp::NodeOptions& options) : node_{ std::make_shared("mtc_node", options) } { } void MTCTaskNode::setupPlanningScene() { moveit_msgs::msg::CollisionObject object; object.id = "object"; object.header.frame_id = "world"; object.primitives.resize(1); object.primitives[0].type = shape_msgs::msg::SolidPrimitive::CYLINDER; object.primitives[0].dimensions = { 0.1, 0.02 }; geometry_msgs::msg::Pose pose; pose.position.x = 0.5; pose.position.y = -0.25; object.pose = pose; moveit::planning_interface::PlanningSceneInterface psi; psi.applyCollisionObject(object); } void MTCTaskNode::doTask() { task_ = createTask(); task2 = createTask(); try { this->task_.init(); } catch (mtc::InitStageException& e) { RCLCPP_ERROR_STREAM(LOGGER, e); return; } if (!this->task_.plan(5)) { RCLCPP_ERROR_STREAM(LOGGER, "Task planning failed"); return; } try { this->task2.init(); } catch (mtc::InitStageException& e) { RCLCPP_ERROR_STREAM(LOGGER, e); return; } if (!this->task2.plan(5)) { RCLCPP_ERROR_STREAM(LOGGER, "Task planning failed"); return; } return; } mtc::Task MTCTaskNode::createTask() { moveit::task_constructor::Task task; task.stages()->setName("demo task"); task.loadRobotModel(node_); const auto& arm_group_name = "panda_arm"; const auto& hand_group_name = "hand"; const auto& hand_frame = "panda_hand"; // Set task properties task.setProperty("group", arm_group_name); task.setProperty("eef", hand_group_name); task.setProperty("ik_frame", hand_frame); mtc::Stage* current_state_ptr = nullptr; // Forward current_state on to grasp pose generator auto stage_state_current = std::make_unique("current"); current_state_ptr = stage_state_current.get(); task.add(std::move(stage_state_current)); auto sampling_planner = std::make_shared(node_); auto interpolation_planner = std::make_shared(); auto cartesian_planner = std::make_shared(); cartesian_planner->setMaxVelocityScaling(1.0); cartesian_planner->setMaxAccelerationScaling(1.0); cartesian_planner->setStepSize(.01); auto stage_open_hand = std::make_unique("open hand", interpolation_planner); stage_open_hand->setGroup(hand_group_name); stage_open_hand->setGoal("open"); task.add(std::move(stage_open_hand)); // Add the next lines of codes to define more stages here auto stage_move_to_pick = std::make_unique( "move to pick", mtc::stages::Connect::GroupPlannerVector{ { arm_group_name, sampling_planner } }); stage_move_to_pick->setTimeout(5.0); stage_move_to_pick->properties().configureInitFrom(mtc::Stage::PARENT); task.add(std::move(stage_move_to_pick)); mtc::Stage* attach_object_stage = nullptr; // Forward attach_object_stage to place pose generator { auto grasp = std::make_unique("pick object"); task.properties().exposeTo(grasp->properties(), { "eef", "group", "ik_frame" }); grasp->properties().configureInitFrom(mtc::Stage::PARENT, { "eef", "group", "ik_frame" }); { auto stage = std::make_unique("approach object", cartesian_planner); stage->properties().set("marker_ns", "approach_object"); stage->properties().set("link", hand_frame); stage->properties().configureInitFrom(mtc::Stage::PARENT, { "group" }); stage->setMinMaxDistance(0.1, 0.15); // Set hand forward direction geometry_msgs::msg::Vector3Stamped vec; vec.header.frame_id = hand_frame; vec.vector.z = 1.0; stage->setDirection(vec); grasp->insert(std::move(stage)); } { // Sample grasp pose auto stage = std::make_unique("generate grasp pose"); stage->properties().configureInitFrom(mtc::Stage::PARENT); stage->properties().set("marker_ns", "grasp_pose"); stage->setPreGraspPose("open"); stage->setObject("object"); stage->setAngleDelta(M_PI / 12); stage->setMonitoredStage(current_state_ptr); // Hook into current state Eigen::Isometry3d grasp_frame_transform; Eigen::Quaterniond q = Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitX()) * Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitZ()); grasp_frame_transform.linear() = q.matrix(); grasp_frame_transform.translation().z() = 0.1; // Compute IK auto wrapper = std::make_unique("grasp pose IK", std::move(stage)); wrapper->setMaxIKSolutions(8); wrapper->setMinSolutionDistance(1.0); wrapper->setIKFrame(grasp_frame_transform, hand_frame); wrapper->properties().configureInitFrom(mtc::Stage::PARENT, { "eef", "group" }); wrapper->properties().configureInitFrom(mtc::Stage::INTERFACE, { "target_pose" }); grasp->insert(std::move(wrapper)); } { auto stage = std::make_unique("allow collision (hand,object)"); stage->allowCollisions("object", task.getRobotModel() ->getJointModelGroup(hand_group_name) ->getLinkModelNamesWithCollisionGeometry(), true); grasp->insert(std::move(stage)); } { auto stage = std::make_unique("close hand", interpolation_planner); stage->setGroup(hand_group_name); stage->setGoal("close"); grasp->insert(std::move(stage)); } { auto stage = std::make_unique("attach object"); stage->attachObject("object", hand_frame); attach_object_stage = stage.get(); grasp->insert(std::move(stage)); } { auto stage = std::make_unique("lift object", cartesian_planner); stage->properties().configureInitFrom(mtc::Stage::PARENT, { "group" }); stage->setMinMaxDistance(0.1, 0.3); stage->setIKFrame(hand_frame); stage->properties().set("marker_ns", "lift_object"); // Set upward direction geometry_msgs::msg::Vector3Stamped vec; vec.header.frame_id = "world"; vec.vector.z = 1.0; stage->setDirection(vec); grasp->insert(std::move(stage)); } task.add(std::move(grasp)); } { auto stage_move_to_place = std::make_unique( "move to place", mtc::stages::Connect::GroupPlannerVector{ { arm_group_name, sampling_planner }, { hand_group_name, sampling_planner } }); stage_move_to_place->setTimeout(5.0); stage_move_to_place->properties().configureInitFrom(mtc::Stage::PARENT); task.add(std::move(stage_move_to_place)); } { auto place = std::make_unique("place object"); task.properties().exposeTo(place->properties(), { "eef", "group", "ik_frame" }); place->properties().configureInitFrom(mtc::Stage::PARENT, { "eef", "group", "ik_frame" }); { // Sample place pose auto stage = std::make_unique("generate place pose"); stage->properties().configureInitFrom(mtc::Stage::PARENT); stage->properties().set("marker_ns", "place_pose"); stage->setObject("object"); geometry_msgs::msg::PoseStamped target_pose_msg; target_pose_msg.header.frame_id = "object"; target_pose_msg.pose.position.y = 0.5; stage->setPose(target_pose_msg); stage->setMonitoredStage(attach_object_stage); // Hook into attach_object_stage // Compute IK auto wrapper = std::make_unique("place pose IK", std::move(stage)); wrapper->setMaxIKSolutions(2); wrapper->setMinSolutionDistance(1.0); wrapper->setIKFrame(hand_frame); wrapper->properties().configureInitFrom(mtc::Stage::PARENT, { "eef", "group" }); wrapper->properties().configureInitFrom(mtc::Stage::INTERFACE, { "target_pose" }); place->insert(std::move(wrapper)); } { auto stage = std::make_unique("open hand", interpolation_planner); stage->setGroup(hand_group_name); stage->setGoal("open"); place->insert(std::move(stage)); } { auto stage = std::make_unique("forbid collision (hand,object)"); stage->allowCollisions("object", task.getRobotModel() ->getJointModelGroup(hand_group_name) ->getLinkModelNamesWithCollisionGeometry(), false); place->insert(std::move(stage)); } { auto stage = std::make_unique("detach object"); stage->detachObject("object", hand_frame); place->insert(std::move(stage)); } { auto stage = std::make_unique("retreat", cartesian_planner); stage->properties().configureInitFrom(mtc::Stage::PARENT, { "group" }); stage->setMinMaxDistance(0.1, 0.3); stage->setIKFrame(hand_frame); stage->properties().set("marker_ns", "retreat"); // Set retreat direction geometry_msgs::msg::Vector3Stamped vec; vec.header.frame_id = "world"; vec.vector.x = -0.5; stage->setDirection(vec); place->insert(std::move(stage)); } task.add(std::move(place)); } { auto stage = std::make_unique("return home", interpolation_planner); stage->properties().configureInitFrom(mtc::Stage::PARENT, { "group" }); stage->setGoal("ready"); task.add(std::move(stage)); } return task; } int main(int argc, char** argv) { rclcpp::init(argc, argv); rclcpp::NodeOptions options; options.automatically_declare_parameters_from_overrides(true); auto mtc_task_node = std::make_shared(options); rclcpp::executors::MultiThreadedExecutor executor; auto spin_thread = std::make_unique([&executor, &mtc_task_node]() { executor.add_node(mtc_task_node->getNodeBaseInterface()); executor.spin(); executor.remove_node(mtc_task_node->getNodeBaseInterface()); }); mtc_task_node->setupPlanningScene(); mtc_task_node->doTask(); spin_thread->join(); rclcpp::shutdown(); return 0; } ```

[JafarAbdi]

I thought we fixed this with https://github.com/ros-planning/moveit_task_constructor/commit/e923fbc0c6bd477d15532b335581734857d0eafc, I'll take a look later this week!

[JSchaaf2]

I thought we fixed this with e923fbc, I'll take a look later this week!

Actually it look like the issue might be fixed, but I don't think it was e923fbc. I didn't properly clean everything and was still using 1e65027 (the commit after e923fbc), Using the latest commit in the ros2 branch that I can build without issues f28b6c4, the segfault from my example doesn't happen anymore. I don't see anything related to this that changed in the commits in between though.

[rhaschke]

Between 1e65027 and f28b6c4 there was no actual code change. Hence, it is surprising to me that the former exhibits the segfault while the latter does not. You mention build issues with the latest ros2 branch. Could you please report them in a new issue?

[JSchaaf2]

Could you please report them in a new issue?

I had some other issues with the moveit update, but I wanted to update before making the issue. I have updated to the latest ros2 and moveit version but I still have the same issues so I made #406.