Segmentation fault during execution, using MTC

[Marcel3245]

Hello MoveIt Task Constructor developers,

I am currently working on a pick and place project using MTC. The idea is first to detect (-> localised) the object in the gazebo simulation, send the coordinates, which then will be used as a target position of the workpiece. I included two buttons in my project, 1st - is to spawn the workpieces (simple cylinders), and the 2nd is to start the MTC search. Everything works well, it finds 5 possible trajectories, but when it should execute the trajectory, the program fails, and throws an error: "Segmentation fault (Address not mapped to object [0xe8])", which I assume is my spawned workpiece. What is more, it was working a few weeks ago, with spawning and moving the workpiece, but I had to update plugins, and as I remember, that's the last moment when it was doing what I asked for. Interestingly, using the standard "Plan & Execute" functionality from the Motion Planning for the same robot and target pose (without MTC) works as expected, even with the spawned objects present in the scene.

Some details:

• ROS 2 Distro: Jazzy
• Ubuntu WSL Distro: Ubuntu 24.04 (Noble Numbat)
• MoveIt 2 (build binary) version: 2.12.13
• MTC: the newest in the main branch
• Gazebo version: v8.9.0 - Harmonic

I have run through the entire internet, but I couldn't find the solution to my problem, so thank you in advance!

The command line error:

user:~/ros2_ws$ ros2 launch mycobot_mtc run.launch.py
[INFO] [launch]: All log files can be found below /home/user/.ros/log/2025-06-02-11-09-02-412410-DESKTOP-0CDUC1V-55401
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [static_transform_publisher-1]: process started with pid [55410]
[INFO] [robot_state_publisher-2]: process started with pid [55411]
[INFO] [static_transform_publisher-3]: process started with pid [55412]
[INFO] [mtc_node-4]: process started with pid [55413]
[INFO] [img_pubsub-5]: process started with pid [55414]
[INFO] [gazebo-6]: process started with pid [55415]
[INFO] [create-7]: process started with pid [55416]
[INFO] [parameter_bridge-8]: process started with pid [55419]
[INFO] [move_group-9]: process started with pid [55420]
[INFO] [spawner-10]: process started with pid [55421]
[static_transform_publisher-3] [INFO] [1748855344.488727316] [static_tf_pub_camera_model]: Spinning until stopped - publishing transform
[mtc_node-4] [WARN] [1748855344.617600155] [rcl.logging_rosout]: Publisher already registered for node name: 'mtc_node'. If this is due to multiple nodes with the same name then all logs for the logger named 'mtc_node' will go out over the existing publisher. As soon as any node with that name is destructed it will unregister the publisher, preventing any further logs for that name from being published on the rosout topic.
[mtc_node-4] [INFO] [1748855344.696608495] [mycobot_mtc]: MTCTaskNode initialized. Waiting for button presses and camera data...
[mtc_node-4] [INFO] [1748855344.696839794] [mycobot_mtc]: MTC node is spinning and waiting for button presses...
[move_group-9] [INFO] [1748855344.721250945] [move_group.moveit.moveit.ros.rdf_loader]: Loaded robot model in 0 seconds
[move_group-9] [INFO] [1748855344.721373345] [move_group.moveit.moveit.core.robot_model]: Loading robot model 'ur'...
[move_group-9] [INFO] [1748855344.721395544] [move_group.moveit.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint
[move_group-9] [INFO] [1748855344.847537589] [move_group.moveit.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'arm_controller': 1 1 1 1 1 1
[move_group-9] [INFO] [1748855345.311221976] [move_group.moveit.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene'
[move_group-9] [INFO] [1748855345.311518177] [move_group.moveit.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states
[move_group-9] [INFO] [1748855345.312535481] [move_group.moveit.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states'
[move_group-9] [INFO] [1748855345.313625186] [move_group.moveit.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects
[move_group-9] [INFO] [1748855345.313686286] [move_group.moveit.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher.
[move_group-9] [INFO] [1748855345.314246189] [move_group.moveit.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene.
[move_group-9] [INFO] [1748855345.316651299] [move_group.moveit.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene'
[move_group-9] [INFO] [1748855345.316842000] [move_group.moveit.moveit.ros.planning_scene_monitor]: Starting planning scene monitor
[move_group-9] [INFO] [1748855345.318351806] [move_group.moveit.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene'
[move_group-9] [INFO] [1748855345.318417407] [move_group.moveit.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates.
[move_group-9] [INFO] [1748855345.319166210] [move_group.moveit.moveit.ros.planning_scene_monitor]: Listening to 'collision_object'
[move_group-9] [INFO] [1748855345.322298223] [move_group.moveit.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry
[move_group-9] [WARN] [1748855345.324473133] [move_group.moveit.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead
[spawner-10] [INFO] [1748855345.443412846] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available...
[move_group-9] [INFO] [1748855345.798222778] [move_group]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds'
[move_group-9] [INFO] [1748855345.798305078] [move_group]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds'
[move_group-9] [INFO] [1748855345.798370778] [move_group]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds'
[move_group-9] [INFO] [1748855345.798396178] [move_group]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision'
[move_group-9] [INFO] [1748855345.798426578] [move_group]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision'
[move_group-9] [INFO] [1748855345.802349495] [move_group]: Try loading adapter 'default_planning_response_adapters/ValidateSolution'
[move_group-9] [INFO] [1748855345.812264438] [move_group]: Loaded adapter 'default_planning_response_adapters/ValidateSolution'
[move_group-9] [INFO] [1748855345.812332638] [move_group]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath'
[move_group-9] [INFO] [1748855345.821645879] [move_group]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath'
[create-7] [INFO] [1748855345.862078853] [ros_gz_sim]: Entity creation successful.
[move_group-9] [INFO] [1748855345.868821082] [move_group.moveit.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL'
[move_group-9] [INFO] [1748855345.875701312] [move_group]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames'
[move_group-9] [INFO] [1748855345.876168914] [move_group]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames'
[move_group-9] [INFO] [1748855345.876211114] [move_group]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds'
[move_group-9] [INFO] [1748855345.876790717] [move_group]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds'
[move_group-9] [INFO] [1748855345.876825117] [move_group]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds'
[move_group-9] [INFO] [1748855345.879105627] [move_group]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds'
[move_group-9] [INFO] [1748855345.879186927] [move_group]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision'
[move_group-9] [INFO] [1748855345.879254527] [move_group]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision'
[move_group-9] [INFO] [1748855345.885041752] [move_group]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization'
[move_group-9] [INFO] [1748855345.887489163] [move_group]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization'
[move_group-9] [INFO] [1748855345.887578863] [move_group]: Try loading adapter 'default_planning_response_adapters/ValidateSolution'
[move_group-9] [INFO] [1748855345.888547067] [move_group]: Loaded adapter 'default_planning_response_adapters/ValidateSolution'
[move_group-9] [INFO] [1748855345.888634668] [move_group]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath'
[move_group-9] [INFO] [1748855345.890325675] [move_group]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath'
[move_group-9] [INFO] [1748855346.049656560] [move_group.moveit.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for arm_controller
[move_group-9] [INFO] [1748855346.049806460] [move_group.moveit.moveit.plugins.simple_controller_manager]: gripper_action_controller will command a max effort of: 0
[move_group-9] [INFO] [1748855346.052640572] [move_group.moveit.moveit.plugins.simple_controller_manager]: Added GripperCommand controller for gripper_action_controller
[move_group-9] [INFO] [1748855346.053046474] [move_group.moveit.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list
[move_group-9] [INFO] [1748855346.053130174] [move_group.moveit.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list
[move_group-9] [INFO] [1748855346.053672777] [move_group.moveit.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers
[move_group-9] [INFO] [1748855346.053715377] [move_group]: MoveGroup debug mode is ON
[INFO] [create-7]: process has finished cleanly [pid 55416]
[move_group-9] [INFO] [1748855346.130398902] [move_group.moveit.moveit.planners.pilz.move_group_sequence_action]: initialize move group sequence action
[move_group-9] [INFO] [1748855346.139927343] [move_group.moveit.moveit.planners.pilz.joint_limits_aggregator]: Reading limits from namespace robot_description_planning
[move_group-9] [INFO] [1748855346.140320945] [move_group.moveit.moveit.planners.pilz.joint_limits_aggregator]: Reading limits from namespace robot_description_planning
[move_group-9] [INFO] [1748855346.145073465] [move_group.moveit.moveit.ros.move_group.executable]:
[move_group-9]
[move_group-9] ********************************************************
[move_group-9] * MoveGroup using:
[move_group-9] *     - apply_planning_scene_service
[move_group-9] *     - clear_octomap_service
[move_group-9] *     - ExecuteTaskSolution
[move_group-9] *     - get_group_urdf
[move_group-9] *     - load_geometry_from_file
[move_group-9] *     - CartesianPathService
[move_group-9] *     - execute_trajectory_action
[move_group-9] *     - get_planning_scene_service
[move_group-9] *     - kinematics_service
[move_group-9] *     - move_action
[move_group-9] *     - motion_plan_service
[move_group-9] *     - query_planners_service
[move_group-9] *     - state_validation_service
[move_group-9] *     - save_geometry_to_file
[move_group-9] *     - SequenceAction
[move_group-9] *     - SequenceService
[move_group-9] ********************************************************
[move_group-9]
[move_group-9] [INFO] [1748855346.145191165] [move_group.moveit.moveit.ros.move_group.context]: MoveGroup context using pipeline pilz
[move_group-9] [INFO] [1748855346.145227965] [move_group.moveit.moveit.ros.move_group.context]: MoveGroup context initialization complete
[move_group-9] Loading 'move_group/ApplyPlanningSceneService'...
[move_group-9] Loading 'move_group/ClearOctomapService'...
[move_group-9] Loading 'move_group/ExecuteTaskSolutionCapability'...
[move_group-9] Loading 'move_group/GetUrdfService'...
[move_group-9] Loading 'move_group/LoadGeometryFromFileService'...
[move_group-9] Loading 'move_group/MoveGroupCartesianPathService'...
[move_group-9] Loading 'move_group/MoveGroupExecuteTrajectoryAction'...
[move_group-9] Loading 'move_group/MoveGroupGetPlanningSceneService'...
[move_group-9] Loading 'move_group/MoveGroupKinematicsService'...
[move_group-9] Loading 'move_group/MoveGroupMoveAction'...
[move_group-9] Loading 'move_group/MoveGroupPlanService'...
[move_group-9] Loading 'move_group/MoveGroupQueryPlannersService'...
[move_group-9] Loading 'move_group/MoveGroupStateValidationService'...
[move_group-9] Loading 'move_group/SaveGeometryToFileService'...
[move_group-9] Loading 'pilz_industrial_motion_planner/MoveGroupSequenceAction'...
[move_group-9] Loading 'pilz_industrial_motion_planner/MoveGroupSequenceService'...
[move_group-9]
[move_group-9] You can start planning now!
[move_group-9]
[gazebo-6] Warning [Utils.cc:132] [/sdf/model[@name="ur"]/joint[@name="wrist_3_joint"]/physics/ode/provide_feedback:<urdf-string>:L0]: XML Element[provide_feedback], child of element[ode], not defined in SDF. Copying[provide_feedback] as children of [ode].
[gazebo-6] [INFO] [1748855347.154343140] [gz_ros_control]: [gz_ros2_control] Fixed joint [base_joint] (Entity=76)] is skipped
[gazebo-6] [INFO] [1748855347.154559041] [gz_ros_control]: [gz_ros2_control] Fixed joint [world_to_start-storage_joint] (Entity=91)] is skipped
[gazebo-6] [INFO] [1748855347.154599741] [gz_ros_control]: [gz_ros2_control] Fixed joint [world_to_table_joint] (Entity=92)] is skipped
[gazebo-6] [INFO] [1748855347.157523953] [gz_ros_control]: Loading controller_manager
[gazebo-6] [INFO] [1748855347.190931393] [controller_manager]: Using ROS clock for triggering controller manager cycles.
[gazebo-6] [INFO] [1748855347.195778213] [controller_manager]: Subscribing to '/robot_description' topic for robot description.
[gazebo-6] [WARN] [1748855347.200594633] [gz_ros_control]: Waiting RM to load and initialize hardware...
[gazebo-6] [INFO] [1748855347.200945335] [controller_manager]: Received robot description from topic.
[gazebo-6] [INFO] [1748855347.211852380] [gz_ros_control]: The position_proportional_gain has been set to: 0.1
[gazebo-6] [INFO] [1748855347.212010781] [gz_ros_control]: Loading joint: shoulder_pan_joint
[gazebo-6] [INFO] [1748855347.212030481] [gz_ros_control]:      State:
[gazebo-6] [INFO] [1748855347.212043681] [gz_ros_control]:               position
[gazebo-6] [INFO] [1748855347.212091281] [gz_ros_control]:                       found initial value: 0.000000
[gazebo-6] [INFO] [1748855347.212111281] [gz_ros_control]:               velocity
[gazebo-6] [INFO] [1748855347.212128181] [gz_ros_control]:               effort
[gazebo-6] [INFO] [1748855347.212140881] [gz_ros_control]:      Command:
[gazebo-6] [INFO] [1748855347.212151481] [gz_ros_control]:               position
[gazebo-6] [INFO] [1748855347.212174882] [gz_ros_control]:               velocity
[gazebo-6] [INFO] [1748855347.212197882] [gz_ros_control]: Loading joint: shoulder_lift_joint
[gazebo-6] [INFO] [1748855347.212209882] [gz_ros_control]:      State:
[gazebo-6] [INFO] [1748855347.212220182] [gz_ros_control]:               position
[gazebo-6] [INFO] [1748855347.212234382] [gz_ros_control]:                       found initial value: -1.570000
[gazebo-6] [INFO] [1748855347.212245782] [gz_ros_control]:               velocity
[gazebo-6] [INFO] [1748855347.212258082] [gz_ros_control]:               effort
[gazebo-6] [INFO] [1748855347.212269582] [gz_ros_control]:      Command:
[gazebo-6] [INFO] [1748855347.212280582] [gz_ros_control]:               position
[gazebo-6] [INFO] [1748855347.212297782] [gz_ros_control]:               velocity
[gazebo-6] [INFO] [1748855347.212318882] [gz_ros_control]: Loading joint: elbow_joint
[gazebo-6] [INFO] [1748855347.212330482] [gz_ros_control]:      State:
[gazebo-6] [INFO] [1748855347.212341182] [gz_ros_control]:               position
[gazebo-6] [INFO] [1748855347.212353182] [gz_ros_control]:                       found initial value: 0.000000
[gazebo-6] [INFO] [1748855347.212383282] [gz_ros_control]:               velocity
[gazebo-6] [INFO] [1748855347.212394683] [gz_ros_control]:               effort
[gazebo-6] [INFO] [1748855347.212408283] [gz_ros_control]:      Command:
[gazebo-6] [INFO] [1748855347.212419383] [gz_ros_control]:               position
[gazebo-6] [INFO] [1748855347.212436583] [gz_ros_control]:               velocity
[gazebo-6] [INFO] [1748855347.212473483] [gz_ros_control]: Loading joint: wrist_1_joint
[gazebo-6] [INFO] [1748855347.212503483] [gz_ros_control]:      State:
[gazebo-6] [INFO] [1748855347.212529783] [gz_ros_control]:               position
[gazebo-6] [INFO] [1748855347.212560483] [gz_ros_control]:                       found initial value: -1.570000
[gazebo-6] [INFO] [1748855347.212572583] [gz_ros_control]:               velocity
[gazebo-6] [INFO] [1748855347.212583283] [gz_ros_control]:               effort
[gazebo-6] [INFO] [1748855347.212593483] [gz_ros_control]:      Command:
[gazebo-6] [INFO] [1748855347.212603083] [gz_ros_control]:               position
[gazebo-6] [INFO] [1748855347.212634284] [gz_ros_control]:               velocity
[gazebo-6] [INFO] [1748855347.212697884] [gz_ros_control]: Loading joint: wrist_2_joint
[gazebo-6] [INFO] [1748855347.212713084] [gz_ros_control]:      State:
[gazebo-6] [INFO] [1748855347.212723784] [gz_ros_control]:               position
[gazebo-6] [INFO] [1748855347.212736384] [gz_ros_control]:                       found initial value: 0.000000
[gazebo-6] [INFO] [1748855347.212748584] [gz_ros_control]:               velocity
[gazebo-6] [INFO] [1748855347.212759184] [gz_ros_control]:               effort
[gazebo-6] [INFO] [1748855347.212769384] [gz_ros_control]:      Command:
[gazebo-6] [INFO] [1748855347.212779484] [gz_ros_control]:               position
[gazebo-6] [INFO] [1748855347.212797784] [gz_ros_control]:               velocity
[gazebo-6] [INFO] [1748855347.212816784] [gz_ros_control]: Loading joint: wrist_3_joint
[gazebo-6] [INFO] [1748855347.212828084] [gz_ros_control]:      State:
[gazebo-6] [INFO] [1748855347.212837984] [gz_ros_control]:               position
[gazebo-6] [INFO] [1748855347.212865484] [gz_ros_control]:                       found initial value: 0.000000
[gazebo-6] [INFO] [1748855347.212932285] [gz_ros_control]:               velocity
[gazebo-6] [INFO] [1748855347.212947585] [gz_ros_control]:               effort
[gazebo-6] [INFO] [1748855347.212973785] [gz_ros_control]:      Command:
[gazebo-6] [INFO] [1748855347.212983585] [gz_ros_control]:               position
[gazebo-6] [INFO] [1748855347.213017885] [gz_ros_control]:               velocity
[gazebo-6] [INFO] [1748855347.213038885] [gz_ros_control]: Loading joint: robotiq_85_left_knuckle_joint
[gazebo-6] [INFO] [1748855347.213049885] [gz_ros_control]:      State:
[gazebo-6] [INFO] [1748855347.213060585] [gz_ros_control]:               position
[gazebo-6] [INFO] [1748855347.213070885] [gz_ros_control]:               velocity
[gazebo-6] [INFO] [1748855347.213081785] [gz_ros_control]:      Command:
[gazebo-6] [INFO] [1748855347.213094285] [gz_ros_control]:               position
[gazebo-6] [INFO] [1748855347.213135286] [gz_ros_control]: Loading joint: robotiq_85_right_knuckle_joint
[gazebo-6] [INFO] [1748855347.213149986] [gz_ros_control]: Joint 'robotiq_85_right_knuckle_joint'is mimicking joint 'robotiq_85_left_knuckle_joint' with multiplier: 1 and offset: 0
[gazebo-6] [INFO] [1748855347.213162386] [gz_ros_control]:      State:
[gazebo-6] [INFO] [1748855347.213175486] [gz_ros_control]:               position
[gazebo-6] [INFO] [1748855347.213193786] [gz_ros_control]:               velocity
[gazebo-6] [INFO] [1748855347.213222986] [gz_ros_control]:      Command:
[gazebo-6] [INFO] [1748855347.213243686] [gz_ros_control]: Loading joint: robotiq_85_left_inner_knuckle_joint
[gazebo-6] [INFO] [1748855347.213257186] [gz_ros_control]: Joint 'robotiq_85_left_inner_knuckle_joint'is mimicking joint 'robotiq_85_left_knuckle_joint' with multiplier: 1 and offset: 0
[gazebo-6] [INFO] [1748855347.213269186] [gz_ros_control]:      State:
[gazebo-6] [INFO] [1748855347.213279886] [gz_ros_control]:               position
[gazebo-6] [INFO] [1748855347.213305486] [gz_ros_control]:               velocity
[gazebo-6] [INFO] [1748855347.213333886] [gz_ros_control]:      Command:
[gazebo-6] [INFO] [1748855347.213351887] [gz_ros_control]: Loading joint: robotiq_85_right_inner_knuckle_joint
[gazebo-6] [INFO] [1748855347.213364787] [gz_ros_control]: Joint 'robotiq_85_right_inner_knuckle_joint'is mimicking joint 'robotiq_85_left_knuckle_joint' with multiplier: 1 and offset: 0
[gazebo-6] [INFO] [1748855347.213376487] [gz_ros_control]:      State:
[gazebo-6] [INFO] [1748855347.213386387] [gz_ros_control]:               position
[gazebo-6] [INFO] [1748855347.213396387] [gz_ros_control]:               velocity
[gazebo-6] [INFO] [1748855347.213421587] [gz_ros_control]:      Command:
[gazebo-6] [INFO] [1748855347.213457487] [gz_ros_control]: Loading joint: robotiq_85_left_finger_joint
[gazebo-6] [INFO] [1748855347.213468287] [gz_ros_control]:      State:
[gazebo-6] [INFO] [1748855347.213478187] [gz_ros_control]:               position
[gazebo-6] [INFO] [1748855347.213488887] [gz_ros_control]:               velocity
[gazebo-6] [INFO] [1748855347.213498987] [gz_ros_control]:      Command:
[gazebo-6] [INFO] [1748855347.213516787] [gz_ros_control]: Loading joint: robotiq_85_right_finger_joint
[gazebo-6] [INFO] [1748855347.213527587] [gz_ros_control]:      State:
[gazebo-6] [INFO] [1748855347.213537587] [gz_ros_control]:               position
[gazebo-6] [INFO] [1748855347.213547887] [gz_ros_control]:               velocity
[gazebo-6] [INFO] [1748855347.213557987] [gz_ros_control]:      Command:
[gazebo-6] [INFO] [1748855347.213576087] [gz_ros_control]: Loading joint: robotiq_85_left_finger_tip_joint
[gazebo-6] [INFO] [1748855347.213588987] [gz_ros_control]: Joint 'robotiq_85_left_finger_tip_joint'is mimicking joint 'robotiq_85_left_knuckle_joint' with multiplier: -1 and offset: 0
[gazebo-6] [INFO] [1748855347.213601288] [gz_ros_control]:      State:
[gazebo-6] [INFO] [1748855347.213612188] [gz_ros_control]:               position
[gazebo-6] [INFO] [1748855347.213623188] [gz_ros_control]:               velocity
[gazebo-6] [INFO] [1748855347.213633788] [gz_ros_control]:      Command:
[gazebo-6] [INFO] [1748855347.213680688] [gz_ros_control]: Loading joint: robotiq_85_right_finger_tip_joint
[gazebo-6] [INFO] [1748855347.213698788] [gz_ros_control]: Joint 'robotiq_85_right_finger_tip_joint'is mimicking joint 'robotiq_85_left_knuckle_joint' with multiplier: -1 and offset: 0
[gazebo-6] [INFO] [1748855347.213712688] [gz_ros_control]:      State:
[gazebo-6] [INFO] [1748855347.213723488] [gz_ros_control]:               position
[gazebo-6] [INFO] [1748855347.213739788] [gz_ros_control]:               velocity
[gazebo-6] [INFO] [1748855347.213751088] [gz_ros_control]:      Command:
[gazebo-6] [INFO] [1748855347.213929189] [gz_ros_control.resource_manager]: Initialize hardware 'arm_controller'
[gazebo-6] [INFO] [1748855347.214191690] [gz_ros_control.resource_manager]: Successful initialization of hardware 'arm_controller'
[gazebo-6] [INFO] [1748855347.214613492] [resource_manager]: 'configure' hardware 'arm_controller'
[gazebo-6] [INFO] [1748855347.214666992] [gz_ros_control]: System Successfully configured!
[gazebo-6] [INFO] [1748855347.214692692] [resource_manager]: Successful 'configure' of hardware 'arm_controller'
[gazebo-6] [INFO] [1748855347.214716992] [resource_manager]: 'activate' hardware 'arm_controller'
[gazebo-6] [INFO] [1748855347.214736792] [resource_manager]: Successful 'activate' of hardware 'arm_controller'
[gazebo-6] [INFO] [1748855347.214816093] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services...
[gazebo-6] [INFO] [1748855347.448132867] [controller_manager]: Loading controller : 'joint_state_broadcaster' of type 'joint_state_broadcaster/JointStateBroadcaster'
[gazebo-6] [INFO] [1748855347.448372468] [controller_manager]: Loading controller 'joint_state_broadcaster'
[gazebo-6] [INFO] [1748855347.462361527] [controller_manager]: Controller 'joint_state_broadcaster' node arguments: --ros-args --params-file /home/user/ros2_ws/install/mycobot_moveit/share/mycobot_moveit/config/ros2_controllers.yaml --ros-args -p use_sim_time:=true --param use_sim_time:=true
[spawner-10] [INFO] [1748855347.487199530] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster
[gazebo-6] [INFO] [1748855347.488693437] [controller_manager]: Configuring controller: 'joint_state_broadcaster'
[gazebo-6] [INFO] [1748855347.489513340] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published
[gazebo-6] [INFO] [1748855347.503615699] [controller_manager]: Activating controllers: [ joint_state_broadcaster ]
[gazebo-6] [Err] [Physics.cc:1785] Attempting to create a mimic constraint for joint [robotiq_85_left_inner_knuckle_joint] but the chosen physics engine does not support mimic constraints, so no constraint will be created.
[gazebo-6] [WARN] [1748855349.532907834] [gz_ros_control]:  Desired controller update period (0.01 s) is slower than the gazebo simulation period (0.001 s).
[gazebo-6] NVD3D10: CPU cyclestats are disabled on client virtualization
[gazebo-6] NVD3D10: CPU cyclestats are disabled on client virtualization
[gazebo-6] NVD3D10: CPU cyclestats are disabled on client virtualization
[gazebo-6] Warning [Utils.cc:132] [/sdf/model[@name="ur"]/joint[@name="wrist_3_joint"]/physics/ode/provide_feedback:<data-string>:L331]: XML Element[provide_feedback], child of element[ode], not defined in SDF. Copying[provide_feedback] as children of [ode].
[gazebo-6] Warning [Utils.cc:132] [/sdf/model[@name="ur"]/joint[@name="wrist_3_joint"]/physics/ode/provide_feedback:<data-string>:L331]: XML Element[provide_feedback], child of element[ode], not defined in SDF. Copying[provide_feedback] as children of [ode].
[gazebo-6] [INFO] [1748855351.984210034] [controller_manager]: Successfully switched controllers!
[spawner-10] [INFO] [1748855352.002073797] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster
[gazebo-6] NVD3D10: CPU cyclestats are disabled on client virtualization
[gazebo-6] NVD3D10: CPU cyclestats are disabled on client virtualization
[gazebo-6] NVD3D10: CPU cyclestats are disabled on client virtualization
[INFO] [spawner-10]: process has finished cleanly [pid 55421]
[INFO] [spawner-11]: process started with pid [55729]
[INFO] [rviz2-12]: process started with pid [55730]
[gazebo-6] [INFO] [1748855354.583917355] [controller_manager]: Loading controller : 'arm_controller' of type 'joint_trajectory_controller/JointTrajectoryController'
[gazebo-6] [INFO] [1748855354.584054055] [controller_manager]: Loading controller 'arm_controller'
[gazebo-6] [INFO] [1748855354.599413150] [controller_manager]: Controller 'arm_controller' node arguments: --ros-args --params-file /home/user/ros2_ws/install/mycobot_moveit/share/mycobot_moveit/config/ros2_controllers.yaml --ros-args -p use_sim_time:=true --param use_sim_time:=true
[spawner-11] [INFO] [1748855354.723775213] [spawner_arm_controller]: Loaded arm_controller
[gazebo-6] [INFO] [1748855354.737630808] [controller_manager]: Configuring controller: 'arm_controller'
[gazebo-6] [INFO] [1748855354.737986408] [arm_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter.
[gazebo-6] [INFO] [1748855354.738102608] [arm_controller]: Command interfaces are [position velocity] and state interfaces are [position velocity].
[gazebo-6] [INFO] [1748855354.738183908] [arm_controller]: Using 'splines' interpolation method.
[gazebo-6] [INFO] [1748855354.744447106] [arm_controller]: Action status changes will be monitored at 20.00 Hz.
[gazebo-6] [INFO] [1748855354.797396390] [controller_manager]: Activating controllers: [ arm_controller ]
[gazebo-6] [INFO] [1748855354.805873888] [controller_manager]: Successfully switched controllers!
[spawner-11] [INFO] [1748855354.833197179] [spawner_arm_controller]: Configured and activated arm_controller
[INFO] [spawner-11]: process has finished cleanly [pid 55729]
[INFO] [spawner-13]: process started with pid [55767]
[img_pubsub-5] NVD3D10: CPU cyclestats are disabled on client virtualization
[img_pubsub-5] NVD3D10: CPU cyclestats are disabled on client virtualization
[gazebo-6] [INFO] [1748855356.330377810] [controller_manager]: Loading controller : 'gripper_action_controller' of type 'position_controllers/GripperActionController'
[gazebo-6] [INFO] [1748855356.330475610] [controller_manager]: Loading controller 'gripper_action_controller'
[gazebo-6] [INFO] [1748855356.340981505] [controller_manager]: Controller 'gripper_action_controller' node arguments: --ros-args --params-file /home/user/ros2_ws/install/mycobot_moveit/share/mycobot_moveit/config/ros2_controllers.yaml --ros-args -p use_sim_time:=true --param use_sim_time:=true
[gazebo-6] [WARN] [1748855356.371282892] [gripper_action_controller]: [Deprecated]: the `position_controllers/GripperActionController` and `effort_controllers::GripperActionController` controllers are replaced by 'parallel_gripper_controllers/GripperActionController' controller
[spawner-13] [INFO] [1748855356.437866363] [spawner_gripper_action_controller]: Loaded gripper_action_controller
[gazebo-6] [INFO] [1748855356.442131461] [controller_manager]: Configuring controller: 'gripper_action_controller'
[gazebo-6] [INFO] [1748855356.442300061] [gripper_action_controller]: Action status changes will be monitored at 20.000000 Hz.
[gazebo-6] [INFO] [1748855356.469997048] [controller_manager]: Activating controllers: [ gripper_action_controller ]
[gazebo-6] [INFO] [1748855356.497819936] [controller_manager]: Successfully switched controllers!
[rviz2-12] [INFO] [1748855356.504710133] [rviz2]: Stereo is NOT SUPPORTED
[rviz2-12] [INFO] [1748855356.504996133] [rviz2]: OpenGl version: 4.2 (GLSL 4.2)
[spawner-13] [INFO] [1748855356.523249525] [spawner_gripper_action_controller]: Configured and activated gripper_action_controller
[rviz2-12] [INFO] [1748855356.642680572] [rviz2]: Stereo is NOT SUPPORTED
[INFO] [spawner-13]: process has finished cleanly [pid 55767]
[rviz2-12] NVD3D10: CPU cyclestats are disabled on client virtualization
[rviz2-12] NVD3D10: CPU cyclestats are disabled on client virtualization
[rviz2-12] NVD3D10: CPU cyclestats are disabled on client virtualization
[rviz2-12] Warning: class_loader.impl: SEVERE WARNING!!! A namespace collision has occurred with plugin factory for class rviz_default_plugins::displays::InteractiveMarkerDisplay. New factory will OVERWRITE existing one. This situation occurs when libraries containing plugins are directly linked against an executable (the one running right now generating this message). Please separate plugins out into their own library or just don't link against the library and use either class_loader::ClassLoader/MultiLibraryClassLoader to open.
[rviz2-12]          at line 321 in /opt/ros/jazzy/include/class_loader/class_loader/class_loader_core.hpp
[rviz2-12] [WARN] [1748855356.953483435] [rcl.logging_rosout]: Publisher already registered for node name: 'rviz2'. If this is due to multiple nodes with the same name then all logs for the logger named 'rviz2' will go out over the existing publisher. As soon as any node with that name is destructed it will unregister the publisher, preventing any further logs for that name from being published on the rosout topic.
[rviz2-12] [INFO] [1748855356.998503115] [rviz2.moveit.ros.rdf_loader]: Loaded robot model in 0.013491 seconds
[rviz2-12] [INFO] [1748855356.998754215] [rviz2.moveit.core.robot_model]: Loading robot model 'ur'...
[rviz2-12] [INFO] [1748855356.998817115] [rviz2.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint
[rviz2-12] [ERROR] [1748855361.872683136] [rviz2.moveit.ros.motion_planning_frame]: Action server: /recognize_objects not available
[rviz2-12] [INFO] [1748855361.939605724] [rviz2.moveit.ros.motion_planning_frame]: MoveGroup namespace changed: / -> . Reloading params.
[rviz2-12] [WARN] [1748855362.024796261] [rcl.logging_rosout]: Publisher already registered for node name: 'rviz2'. If this is due to multiple nodes with the same name then all logs for the logger named 'rviz2' will go out over the existing publisher. As soon as any node with that name is destructed it will unregister the publisher, preventing any further logs for that name from being published on the rosout topic.
[rviz2-12] [INFO] [1748855362.085571828] [rviz2.moveit.ros.rdf_loader]: Loaded robot model in 0.00486671 seconds
[rviz2-12] [INFO] [1748855362.085807428] [rviz2.moveit.core.robot_model]: Loading robot model 'ur'...
[rviz2-12] [INFO] [1748855362.085866328] [rviz2.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint
[rviz2-12] [INFO] [1748855362.342027129] [rviz2.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'arm_controller': 1 1 1 1 1 1
[rviz2-12] [INFO] [1748855362.878976099] [rviz2.moveit.ros.planning_scene_monitor]: Starting planning scene monitor
[rviz2-12] [INFO] [1748855362.882154908] [rviz2.moveit.ros.planning_scene_monitor]: Listening to '/monitored_planning_scene'
[rviz2-12] [INFO] [1748855362.914869397] [interactive_marker_display_101162029286400]: Connected on namespace: /rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic
[rviz2-12] [INFO] [1748855362.997848324] [rviz2.moveit.ros.motion_planning_frame]: group arm_controller
[rviz2-12] [INFO] [1748855362.997956224] [rviz2.moveit.ros.motion_planning_frame]: Constructing new MoveGroup connection for group 'arm_controller' in namespace ''
[rviz2-12] [INFO] [1748855363.030569912] [rviz2.moveit.ros.move_group_interface]: Ready to take commands for planning group arm_controller.
[rviz2-12] [INFO] [1748855363.033620920] [rviz2.moveit.ros.motion_planning_frame]: group arm_controller
[rviz2-12] [INFO] [1748855363.034010321] [rviz2.moveit.ros.motion_planning_frame]: group arm_controller
[rviz2-12] [INFO] [1748855363.120174951] [interactive_marker_display_101162029286400]: Sending request for interactive markers
[rviz2-12] [INFO] [1748855363.181088114] [interactive_marker_display_101162029286400]: Service response received for initialization
[gazebo-6] NodeShared::Publish() Error: Interrupted system call
[rviz2-12] [INFO] [1748855401.620782815] [rviz_push_button_node]: Workpieces spawned!
[mtc_node-4] [INFO] [1748855401.621227215] [mycobot_mtc]: Received button1 press (true). Attempting to setup planning scene...
[mtc_node-4] [INFO] [1748855401.621290914] [mycobot_mtc]: Setting up planning scene...
[mtc_node-4] [WARN] [1748855401.671087149] [rcl.logging_rosout]: Publisher already registered for node name: 'mtc_node'. If this is due to multiple nodes with the same name then all logs for the logger named 'mtc_node' will go out over the existing publisher. As soon as any node with that name is destructed it will unregister the publisher, preventing any further logs for that name from being published on the rosout topic.
[mtc_node-4] [INFO] [1748855401.731708269] [mycobot_mtc]: Detected TF Yaw close to 0 or 180 degrees (0.00 rad) for smaller objects layout.
[mtc_node-4] [INFO] [1748855401.731793369] [mycobot_mtc]: Adding object 0 at (-0.010, -0.417, 0.947) based on TF.
[mtc_node-4] [INFO] [1748855401.731818569] [mycobot_mtc]: Adding object 1 at (-0.070, -0.417, 0.947) based on TF.
[mtc_node-4] [INFO] [1748855401.731836369] [mycobot_mtc]: Adding object 2 at (-0.130, -0.417, 0.947) based on TF.
[mtc_node-4] [INFO] [1748855401.731859369] [mycobot_mtc]: Adding object 3 at (-0.010, -0.353, 0.947) based on TF.
[mtc_node-4] [INFO] [1748855401.731879069] [mycobot_mtc]: Adding object 4 at (-0.070, -0.353, 0.947) based on TF.
[mtc_node-4] [INFO] [1748855401.731898069] [mycobot_mtc]: Adding object 5 at (-0.130, -0.353, 0.947) based on TF.
[mtc_node-4] [INFO] [1748855401.735099065] [mycobot_mtc]: Applied 6 smaller collision objects based on TF.
[rviz2-12] [INFO] [1748855403.866860792] [rviz_push_button_node]: Run MTC
[mtc_node-4] [INFO] [1748855403.867266691] [mycobot_mtc]: Received button2 press (true). Running MTC task...
[mtc_node-4] [INFO] [1748855403.867320891] [mycobot_mtc]: Setting target place pose for THIS task run to (-0.189, 0.381, 0.947)
[mtc_node-4] [WARN] [1748855403.868378990] [rcl.logging_rosout]: Publisher already registered for node name: 'mtc_node'. If this is due to multiple nodes with the same name then all logs for the logger named 'mtc_node' will go out over the existing publisher. As soon as any node with that name is destructed it will unregister the publisher, preventing any further logs for that name from being published on the rosout topic.
[gazebo-6] [INFO] [1748855403.929063902] [controller_manager]: Received robot description from topic.
[gazebo-6] [WARN] [1748855403.929208402] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description.
[mtc_node-4] [INFO] [1748855403.930198201] [planning_scene_interface_140666822783792.mtc_node.moveit.ros.rdf_loader]: Loaded robot model in 0.006 seconds
[mtc_node-4] [INFO] [1748855403.930435100] [planning_scene_interface_140666822783792.mtc_node.moveit.core.robot_model]: Loading robot model 'ur'...
[mtc_node-4] [INFO] [1748855403.930486700] [planning_scene_interface_140666822783792.mtc_node.moveit.core.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint
[mtc_node-4] [INFO] [1748855404.122623616] [planning_scene_interface_140666822783792.mtc_node.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'arm_controller': 1 1 1 1 1 1
[mtc_node-4] [INFO] [1748855404.218151873] [planning_scene_interface_140666822783792.mtc_node.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL'
[mtc_node-4] [INFO] [1748855404.225965861] [mtc_node]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames'
[mtc_node-4] [INFO] [1748855404.231345453] [mtc_node]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames'
[mtc_node-4] [INFO] [1748855404.231392553] [mtc_node]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds'
[mtc_node-4] [INFO] [1748855404.232062352] [mtc_node]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds'
[mtc_node-4] [INFO] [1748855404.232111452] [mtc_node]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds'
[mtc_node-4] [INFO] [1748855404.232196152] [mtc_node]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds'
[mtc_node-4] [INFO] [1748855404.232209552] [mtc_node]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision'
[mtc_node-4] [INFO] [1748855404.232242251] [mtc_node]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision'
[mtc_node-4] [INFO] [1748855404.245956231] [mtc_node]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization'
[mtc_node-4] [INFO] [1748855404.254515818] [mtc_node]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization'
[mtc_node-4] [INFO] [1748855404.254555718] [mtc_node]: Try loading adapter 'default_planning_response_adapters/ValidateSolution'
[mtc_node-4] [INFO] [1748855404.257088514] [mtc_node]: Loaded adapter 'default_planning_response_adapters/ValidateSolution'
[mtc_node-4] [INFO] [1748855404.257143414] [mtc_node]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath'
[mtc_node-4] [INFO] [1748855404.263203905] [mtc_node]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath'
[mtc_node-4] [INFO] [1748855404.265929601] [mycobot_mtc]: Task initialized. Planning...
[mtc_node-4] [INFO] [1748855407.192788383] [mtc_node]: Calling PlanningRequestAdapter 'ResolveConstraintFrames'
[mtc_node-4] [INFO] [1748855407.193134983] [mtc_node]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds'
[mtc_node-4] [WARN] [1748855407.193154983] [planning_scene_interface_140666822783792.mtc_node.moveit.ros.validate_workspace_bounds]: It looks like the planning volume was not specified. Using default values.
[mtc_node-4] [INFO] [1748855407.193348282] [mtc_node]: Calling PlanningRequestAdapter 'CheckStartStateBounds'
[mtc_node-4] [INFO] [1748855407.193618382] [mtc_node]: Calling PlanningRequestAdapter 'CheckStartStateCollision'
[mtc_node-4] [INFO] [1748855407.196764877] [planning_scene_interface_140666822783792.mtc_node.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'arm_controller' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[mtc_node-4] [INFO] [1748855407.197645875] [mtc_node]: Calling Planner 'OMPL'
[mtc_node-4] [INFO] [1748855407.307220590] [mtc_node]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization'
[mtc_node-4] [INFO] [1748855407.308646888] [mtc_node]: Calling PlanningResponseAdapter 'ValidateSolution'
[mtc_node-4] [INFO] [1748855407.311730783] [mtc_node]: Calling PlanningResponseAdapter 'DisplayMotionPath'
[mtc_node-4] [WARN] [1748855407.312849081] [planning_scene_interface_140666822783792.mtc_node.moveit.ros.planning_pipeline]: The planner plugin did not fill out the 'planner_id' field of the MotionPlanResponse. Setting it to the planner ID name of the MotionPlanRequest assuming that the planner plugin does warn you if it does not use the requested planner.
[mtc_node-4] [INFO] [1748855407.351405916] [mtc_node]: Calling PlanningRequestAdapter 'ResolveConstraintFrames'
[mtc_node-4] [INFO] [1748855407.351850515] [mtc_node]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds'
[mtc_node-4] [WARN] [1748855407.351881615] [planning_scene_interface_140666822783792.mtc_node.moveit.ros.validate_workspace_bounds]: It looks like the planning volume was not specified. Using default values.
[mtc_node-4] [INFO] [1748855407.352192914] [mtc_node]: Calling PlanningRequestAdapter 'CheckStartStateBounds'
[mtc_node-4] [INFO] [1748855407.352473714] [mtc_node]: Calling PlanningRequestAdapter 'CheckStartStateCollision'
[mtc_node-4] [INFO] [1748855407.353909011] [planning_scene_interface_140666822783792.mtc_node.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'arm_controller' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[mtc_node-4] [INFO] [1748855407.354344411] [mtc_node]: Calling Planner 'OMPL'
[mtc_node-4] [INFO] [1748855407.430938381] [mtc_node]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization'
[mtc_node-4] [INFO] [1748855407.433302977] [mtc_node]: Calling PlanningResponseAdapter 'ValidateSolution'
[mtc_node-4] [INFO] [1748855407.436792471] [mtc_node]: Calling PlanningResponseAdapter 'DisplayMotionPath'
[mtc_node-4] [WARN] [1748855407.437324271] [planning_scene_interface_140666822783792.mtc_node.moveit.ros.planning_pipeline]: The planner plugin did not fill out the 'planner_id' field of the MotionPlanResponse. Setting it to the planner ID name of the MotionPlanRequest assuming that the planner plugin does warn you if it does not use the requested planner.
[mtc_node-4] [INFO] [1748855407.504074158] [mtc_node]: Calling PlanningRequestAdapter 'ResolveConstraintFrames'
[mtc_node-4] [INFO] [1748855407.504509857] [mtc_node]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds'
[mtc_node-4] [WARN] [1748855407.504538957] [planning_scene_interface_140666822783792.mtc_node.moveit.ros.validate_workspace_bounds]: It looks like the planning volume was not specified. Using default values.
[mtc_node-4] [INFO] [1748855407.504766557] [mtc_node]: Calling PlanningRequestAdapter 'CheckStartStateBounds'
[mtc_node-4] [INFO] [1748855407.504911956] [mtc_node]: Calling PlanningRequestAdapter 'CheckStartStateCollision'
[mtc_node-4] [INFO] [1748855407.506194154] [planning_scene_interface_140666822783792.mtc_node.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'arm_controller' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[mtc_node-4] [INFO] [1748855407.506387054] [mtc_node]: Calling Planner 'OMPL'
[mtc_node-4] [INFO] [1748855407.625578253] [mtc_node]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization'
[mtc_node-4] [INFO] [1748855407.626934350] [mtc_node]: Calling PlanningResponseAdapter 'ValidateSolution'
[mtc_node-4] [INFO] [1748855407.630832644] [mtc_node]: Calling PlanningResponseAdapter 'DisplayMotionPath'
[mtc_node-4] [WARN] [1748855407.631540943] [planning_scene_interface_140666822783792.mtc_node.moveit.ros.planning_pipeline]: The planner plugin did not fill out the 'planner_id' field of the MotionPlanResponse. Setting it to the planner ID name of the MotionPlanRequest assuming that the planner plugin does warn you if it does not use the requested planner.
[mtc_node-4] [INFO] [1748855408.728815346] [mtc_node]: Calling PlanningRequestAdapter 'ResolveConstraintFrames'
[mtc_node-4] [INFO] [1748855408.729317645] [mtc_node]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds'
[mtc_node-4] [WARN] [1748855408.729347045] [planning_scene_interface_140666822783792.mtc_node.moveit.ros.validate_workspace_bounds]: It looks like the planning volume was not specified. Using default values.
[mtc_node-4] [INFO] [1748855408.729506744] [mtc_node]: Calling PlanningRequestAdapter 'CheckStartStateBounds'
[mtc_node-4] [INFO] [1748855408.729679344] [mtc_node]: Calling PlanningRequestAdapter 'CheckStartStateCollision'
[mtc_node-4] [INFO] [1748855408.731837840] [planning_scene_interface_140666822783792.mtc_node.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'arm_controller' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[mtc_node-4] [INFO] [1748855408.732585339] [mtc_node]: Calling Planner 'OMPL'
[mtc_node-4] [INFO] [1748855408.917971315] [mtc_node]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization'
[mtc_node-4] [INFO] [1748855408.921839808] [mtc_node]: Calling PlanningResponseAdapter 'ValidateSolution'
[mtc_node-4] [INFO] [1748855408.927851197] [mtc_node]: Calling PlanningResponseAdapter 'DisplayMotionPath'
[mtc_node-4] [WARN] [1748855408.928518096] [planning_scene_interface_140666822783792.mtc_node.moveit.ros.planning_pipeline]: The planner plugin did not fill out the 'planner_id' field of the MotionPlanResponse. Setting it to the planner ID name of the MotionPlanRequest assuming that the planner plugin does warn you if it does not use the requested planner.
[mtc_node-4] [INFO] [1748855408.979622807] [mtc_node]: Calling PlanningRequestAdapter 'ResolveConstraintFrames'
[mtc_node-4] [INFO] [1748855408.979882006] [mtc_node]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds'
[mtc_node-4] [WARN] [1748855408.979899606] [planning_scene_interface_140666822783792.mtc_node.moveit.ros.validate_workspace_bounds]: It looks like the planning volume was not specified. Using default values.
[mtc_node-4] [INFO] [1748855408.980076806] [mtc_node]: Calling PlanningRequestAdapter 'CheckStartStateBounds'
[mtc_node-4] [INFO] [1748855408.980331205] [mtc_node]: Calling PlanningRequestAdapter 'CheckStartStateCollision'
[mtc_node-4] [INFO] [1748855408.981480903] [planning_scene_interface_140666822783792.mtc_node.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'arm_controller' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[mtc_node-4] [INFO] [1748855408.981686303] [mtc_node]: Calling Planner 'OMPL'
[mtc_node-4] [INFO] [1748855409.147079005] [mtc_node]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization'
[mtc_node-4] [INFO] [1748855409.151222597] [mtc_node]: Calling PlanningResponseAdapter 'ValidateSolution'
[mtc_node-4] [INFO] [1748855409.156395188] [mtc_node]: Calling PlanningResponseAdapter 'DisplayMotionPath'
[mtc_node-4] [WARN] [1748855409.157214787] [planning_scene_interface_140666822783792.mtc_node.moveit.ros.planning_pipeline]: The planner plugin did not fill out the 'planner_id' field of the MotionPlanResponse. Setting it to the planner ID name of the MotionPlanRequest assuming that the planner plugin does warn you if it does not use the requested planner.
[mtc_node-4] [INFO] [1748855409.203350003] [mtc_node]: Calling PlanningRequestAdapter 'ResolveConstraintFrames'
[mtc_node-4] [INFO] [1748855409.203613302] [mtc_node]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds'
[mtc_node-4] [WARN] [1748855409.203634402] [planning_scene_interface_140666822783792.mtc_node.moveit.ros.validate_workspace_bounds]: It looks like the planning volume was not specified. Using default values.
[mtc_node-4] [INFO] [1748855409.203854702] [mtc_node]: Calling PlanningRequestAdapter 'CheckStartStateBounds'
[mtc_node-4] [INFO] [1748855409.204108002] [mtc_node]: Calling PlanningRequestAdapter 'CheckStartStateCollision'
[mtc_node-4] [INFO] [1748855409.204955200] [planning_scene_interface_140666822783792.mtc_node.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'arm_controller' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[mtc_node-4] [INFO] [1748855409.205163100] [mtc_node]: Calling Planner 'OMPL'
[mtc_node-4] [INFO] [1748855409.300730927] [mtc_node]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization'
[mtc_node-4] [INFO] [1748855409.303731921] [mtc_node]: Calling PlanningResponseAdapter 'ValidateSolution'
[mtc_node-4] [INFO] [1748855409.307953213] [mtc_node]: Calling PlanningResponseAdapter 'DisplayMotionPath'
[mtc_node-4] [WARN] [1748855409.308562412] [planning_scene_interface_140666822783792.mtc_node.moveit.ros.planning_pipeline]: The planner plugin did not fill out the 'planner_id' field of the MotionPlanResponse. Setting it to the planner ID name of the MotionPlanRequest assuming that the planner plugin does warn you if it does not use the requested planner.
[mtc_node-4] [INFO] [1748855409.311473407] [mtc_node]: Calling PlanningRequestAdapter 'ResolveConstraintFrames'
[mtc_node-4] [INFO] [1748855409.312121006] [mtc_node]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds'
[mtc_node-4] [WARN] [1748855409.312159306] [planning_scene_interface_140666822783792.mtc_node.moveit.ros.validate_workspace_bounds]: It looks like the planning volume was not specified. Using default values.
[mtc_node-4] [INFO] [1748855409.313138404] [mtc_node]: Calling PlanningRequestAdapter 'CheckStartStateBounds'
[mtc_node-4] [INFO] [1748855409.313653203] [mtc_node]: Calling PlanningRequestAdapter 'CheckStartStateCollision'
[mtc_node-4] [INFO] [1748855409.316081999] [planning_scene_interface_140666822783792.mtc_node.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'arm_controller' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[mtc_node-4] [INFO] [1748855409.316398698] [mtc_node]: Calling Planner 'OMPL'
[mtc_node-4] [INFO] [1748855409.500350042] [mtc_node]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization'
[mtc_node-4] [INFO] [1748855409.503375896] [mtc_node]: Calling PlanningResponseAdapter 'ValidateSolution'
[mtc_node-4] [INFO] [1748855409.510493822] [mtc_node]: Calling PlanningResponseAdapter 'DisplayMotionPath'
[mtc_node-4] [WARN] [1748855409.513143768] [planning_scene_interface_140666822783792.mtc_node.moveit.ros.planning_pipeline]: The planner plugin did not fill out the 'planner_id' field of the MotionPlanResponse. Setting it to the planner ID name of the MotionPlanRequest assuming that the planner plugin does warn you if it does not use the requested planner.
[mtc_node-4] [INFO] [1748855409.618924538] [mtc_node]: Calling PlanningRequestAdapter 'ResolveConstraintFrames'
[mtc_node-4] [INFO] [1748855409.619248643] [mtc_node]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds'
[mtc_node-4] [WARN] [1748855409.619267144] [planning_scene_interface_140666822783792.mtc_node.moveit.ros.validate_workspace_bounds]: It looks like the planning volume was not specified. Using default values.
[mtc_node-4] [INFO] [1748855409.619578149] [mtc_node]: Calling PlanningRequestAdapter 'CheckStartStateBounds'
[mtc_node-4] [INFO] [1748855409.619748452] [mtc_node]: Calling PlanningRequestAdapter 'CheckStartStateCollision'
[mtc_node-4] [INFO] [1748855409.621090176] [planning_scene_interface_140666822783792.mtc_node.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'arm_controller' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[mtc_node-4] [INFO] [1748855409.622348498] [mtc_node]: Calling Planner 'OMPL'
[mtc_node-4] [INFO] [1748855409.769633401] [mtc_node]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization'
[mtc_node-4] [INFO] [1748855409.773822775] [mtc_node]: Calling PlanningResponseAdapter 'ValidateSolution'
[mtc_node-4] [INFO] [1748855409.779567776] [mtc_node]: Calling PlanningResponseAdapter 'DisplayMotionPath'
[mtc_node-4] [WARN] [1748855409.780449192] [planning_scene_interface_140666822783792.mtc_node.moveit.ros.planning_pipeline]: The planner plugin did not fill out the 'planner_id' field of the MotionPlanResponse. Setting it to the planner ID name of the MotionPlanRequest assuming that the planner plugin does warn you if it does not use the requested planner.
[mtc_node-4] [INFO] [1748855409.998481045] [mycobot_mtc]: Task planning succeeded. Publishing solution...
[mtc_node-4] [INFO] [1748855410.008756226] [mycobot_mtc]: Executing task...
[move_group-9] [WARN] [1748855410.082569626] [moveit_task_constructor_visualization.execute_task_solution]: The trajectory of stage '3' from task '' does not have any controllers specified for trajectory execution. This might lead to unexpected controller selection.
[move_group-9] [WARN] [1748855410.082910932] [moveit_task_constructor_visualization.execute_task_solution]: The trajectory of stage '4' from task '' does not have any controllers specified for trajectory execution. This might lead to unexpected controller selection.
[move_group-9] [WARN] [1748855410.083657245] [moveit_task_constructor_visualization.execute_task_solution]: The trajectory of stage '6' from task '' does not have any controllers specified for trajectory execution. This might lead to unexpected controller selection.
[move_group-9] [WARN] [1748855410.084145554] [moveit_task_constructor_visualization.execute_task_solution]: The trajectory of stage '10' from task '' does not have any controllers specified for trajectory execution. This might lead to unexpected controller selection.
[move_group-9] [WARN] [1748855410.085053570] [moveit_task_constructor_visualization.execute_task_solution]: The trajectory of stage '12' from task '' does not have any controllers specified for trajectory execution. This might lead to unexpected controller selection.
[move_group-9] [WARN] [1748855410.085595579] [moveit_task_constructor_visualization.execute_task_solution]: The trajectory of stage '14' from task '' does not have any controllers specified for trajectory execution. This might lead to unexpected controller selection.
[move_group-9] [WARN] [1748855410.085968786] [moveit_task_constructor_visualization.execute_task_solution]: The trajectory of stage '18' from task '' does not have any controllers specified for trajectory execution. This might lead to unexpected controller selection.
[move_group-9] [WARN] [1748855410.086325092] [moveit_task_constructor_visualization.execute_task_solution]: The trajectory of stage '20' from task '' does not have any controllers specified for trajectory execution. This might lead to unexpected controller selection.
[move_group-9] [INFO] [1748855410.086358693] [moveit_task_constructor_visualization.execute_task_solution]: Executing TaskSolution
[move_group-9] [INFO] [1748855410.086485195] [move_group.moveit.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list
[move_group-9] [INFO] [1748855410.086590497] [move_group.moveit.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list
[move_group-9] [INFO] [1748855410.086680498] [move_group.moveit.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list
[move_group-9] [INFO] [1748855410.086767300] [move_group.moveit.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list
[move_group-9] [INFO] [1748855410.086891002] [move_group.moveit.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list
[move_group-9] [INFO] [1748855410.086948003] [move_group.moveit.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list
[move_group-9] [INFO] [1748855410.087032405] [move_group.moveit.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list
[move_group-9] [INFO] [1748855410.087064605] [move_group.moveit.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list
[move_group-9] [INFO] [1748855410.087191207] [move_group.moveit.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list
[move_group-9] [INFO] [1748855410.087234108] [move_group.moveit.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list
[move_group-9] [INFO] [1748855410.087356610] [move_group.moveit.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list
[move_group-9] [INFO] [1748855410.087397611] [move_group.moveit.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list
[move_group-9] [INFO] [1748855410.087452712] [move_group.moveit.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list
[move_group-9] [INFO] [1748855410.087544414] [move_group.moveit.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list
[move_group-9] [INFO] [1748855410.087638215] [move_group.moveit.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list
[move_group-9] [INFO] [1748855410.087677816] [move_group.moveit.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list
[move_group-9] [INFO] [1748855410.087887220] [move_group.moveit.moveit.ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01
[move_group-9] [INFO] [1748855410.101703163] [move_group.moveit.moveit.ros.trajectory_execution_manager]: Starting trajectory execution ...
[move_group-9] Stack trace (most recent call last) in thread 55898:
[move_group-9] [INFO] [1748855410.104321009] [move_group.moveit.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list
[move_group-9] [INFO] [1748855410.104548213] [move_group.moveit.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list
[move_group-9] [INFO] [1748855410.106331044] [move_group.moveit.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list
[move_group-9] [INFO] [1748855410.106493747] [move_group.moveit.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list
[gazebo-6] [INFO] [1748855410.107822571] [gripper_action_controller]: Received & accepted new action goal
[move_group-9] #10   Object "", at 0xffffffffffffffff, in
[move_group-9] #9    Source "../sysdeps/unix/sysv/linux/x86_64/clone3.S", line 78, in clone3 [0x7c44c2129c3b]
[move_group-9] #8    Source "./nptl/pthread_create.c", line 447, in start_thread [0x7c44c209caa3]
[move_group-9] #7    Source "../../../../../src/libstdc++-v3/src/c++11/thread.cc", line 104, in execute_native_thread_routine [0x7c44c24ecdb3]
[move_group-9] #6    Object "/home/user/ros2_ws/install/moveit_task_constructor_capabilities/lib/libmoveit_task_constructor_capabilities.so", at 0x7c4475e9d157, in std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<void (move_group::ExecuteTaskSolutionCapability::*)(std::shared_ptr<rclcpp_action::ServerGoalHandle<moveit_task_constructor_msgs::action::ExecuteTaskSolution> > const&), move_group::ExecuteTaskSolutionCapability*, std::shared_ptr<rclcpp_action::ServerGoalHandle<moveit_task_constructor_msgs::action::ExecuteTaskSolution> > > >, void>::_M_run()
[move_group-9] #5    Source "./nptl/pthread_once.c", line 116, in __pthread_once_slow [0x7c44c20a1ed2]
[move_group-9] #4  | Source "/usr/include/c++/13/future", line 589, in operator()
[move_group-9]     |   587:       _M_do_set(function<_Ptr_type()>* __f, bool* __did_set)
[move_group-9]     |   588:       {
[move_group-9]     | > 589:         _Ptr_type __res = (*__f)();
[move_group-9]     |   590:         // Notify the caller that we did try to set; if we do not throw an
[move_group-9]     |   591:         // exception, the caller will be aware that it did set (e.g., see
[move_group-9]       Source "/usr/include/c++/13/bits/std_function.h", line 591, in _M_do_set [0x7c44c2bc97f1]
[move_group-9]         588:       {
[move_group-9]         589:     if (_M_empty())
[move_group-9]         590:       __throw_bad_function_call();
[move_group-9]       > 591:     return _M_invoker(_M_functor, std::forward<_ArgTypes>(__args)...);
[move_group-9]         592:       }
[move_group-9]         593:
[move_group-9]         594: #if __cpp_rtti
[move_group-9] #3    Object "/home/user/ros2_ws/install/moveit_task_constructor_capabilities/lib/libmoveit_task_constructor_capabilities.so", at 0x7c4475e9bd1f, in std::_Function_handler<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> (), std::__future_base::_Task_setter<std::unique_ptr<std::__future_base::_Result<void>, std::__future_base::_Result_base::_Deleter>, std::thread::_Invoker<std::tuple<void (move_group::ExecuteTaskSolutionCapability::*)(std::shared_ptr<rclcpp_action::ServerGoalHandle<moveit_task_constructor_msgs::action::ExecuteTaskSolution> > const&), move_group::ExecuteTaskSolutionCapability*, std::shared_ptr<rclcpp_action::ServerGoalHandle<moveit_task_constructor_msgs::action::ExecuteTaskSolution> > > >, void> >::_M_invoke(std::_Any_data const&)
[move_group-9] #2    Object "/home/user/ros2_ws/install/moveit_task_constructor_capabilities/lib/libmoveit_task_constructor_capabilities.so", at 0x7c4475e9b29e, in move_group::ExecuteTaskSolutionCapability::execCallback(std::shared_ptr<rclcpp_action::ServerGoalHandle<moveit_task_constructor_msgs::action::ExecuteTaskSolution> > const&)
[move_group-9] #1    Source "/usr/src/ros-jazzy-moveit-ros-planning-2.12.3-1noble.20250520.213359/plan_execution/src/plan_execution.cpp", line 475, in executeAndMonitor [0x7c44c26e603c]
[move_group-9] #0  | Source "/usr/src/ros-jazzy-moveit-core-2.12.3-1noble.20250520.202002/robot_state/src/robot_state.cpp", line 1226, in begin
[move_group-9]     | Source "/usr/include/c++/13/bits/stl_map.h", line 378, in begin
[move_group-9]     |   376:       const_iterator
[move_group-9]     |   377:       begin() const _GLIBCXX_NOEXCEPT
[move_group-9]     | > 378:       { return _M_t.begin(); }
[move_group-9]     |   379:
[move_group-9]     |   380:       /**
[move_group-9]       Source "/usr/include/c++/13/bits/stl_tree.h", line 1002, in getAttachedBodies [0x7c44c23c474b]
[move_group-9]        1000:       const_iterator
[move_group-9]        1001:       begin() const _GLIBCXX_NOEXCEPT
[move_group-9]       >1002:       { return const_iterator(this->_M_impl._M_header._M_left); }
[move_group-9]        1003:
[move_group-9]        1004:       iterator
[move_group-9]        1005:       end() _GLIBCXX_NOEXCEPT
[move_group-9] Segmentation fault (Address not mapped to object [0xe8])
[ERROR] [move_group-9]: process has died [pid 55420, exit code -11, cmd '/opt/ros/jazzy/lib/moveit_ros_move_group/move_group --ros-args --params-file /tmp/launch_params_rhteziag --params-file /tmp/launch_params_ci1w8w2d --params-file /tmp/launch_params_qbdbnm6o --params-file /tmp/launch_params_sa30psmx --params-file /tmp/launch_params_yto5f730 --params-file /tmp/launch_params_bnsf3whj --params-file /tmp/launch_params_yiu6uurw --params-file /tmp/launch_params_bmphz8zv --params-file /tmp/launch_params_03dgi3qw --params-file /tmp/launch_params_7rc9lsxc --params-file /tmp/launch_params_hcn77_53'].

[Marcel3245]

And here is my mtc_node code, because of "character limit":

#include <rclcpp/rclcpp.hpp>
#include <moveit/planning_scene/planning_scene.hpp>
#include <moveit/planning_scene_interface/planning_scene_interface.h> // Use planning_scene_interface.h for add/remove collision objects
#include <geometric_shapes/shape_operations.h>
#include <moveit/task_constructor/task.h>
#include <moveit/task_constructor/solvers.h>
#include <moveit/task_constructor/stages.h>
#if __has_include(<tf2_geometry_msgs/tf2_geometry_msgs.hpp>)
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#else
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#endif
#if __has_include(<tf2_eigen/tf2_eigen.hpp>)
#include <tf2_eigen/tf2_eigen.hpp>
#else
#include <tf2_eigen/tf2_eigen.h>
#endif

#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <tf2/time.h>
#include <tf2/utils.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>

#include "std_msgs/msg/float64_multi_array.hpp"
#include "std_msgs/msg/bool.hpp" // Include for Bool message
#include <visualization_msgs/msg/marker.hpp>
#include <visualization_msgs/msg/marker_array.hpp>

#include <chrono>
#include <memory>
#include <mutex> 
#include <array>
using namespace std::chrono_literals;

static const rclcpp::Logger LOGGER = rclcpp::get_logger("mycobot_mtc");
namespace mtc = moveit::task_constructor;

// Global array to store circle data (assuming this is still needed for other parts)
float data_array[6][4] = {0}; // Likely stores circles data
std::mutex data_mutex_;

class MTCTaskNode
{
public:
  MTCTaskNode(const rclcpp::NodeOptions& options);
  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr getNodeBaseInterface();

  void doTask();
  void setupPlanningScene();

private:
  mtc::Task createTask();

  // Subscription callbacks
  // Renamed to circles_data_callback for clarity, handling the original "camera/circles_to_world"
  void circles_data_callback(const std_msgs::msg::Float64MultiArray::SharedPtr msg);
  // New callback specifically for the storage data
  void storage_data_callback(const std_msgs::msg::Float64MultiArray::SharedPtr msg);

  void spawn_workpieces_callback(const std_msgs::msg::Bool::SharedPtr msg);
  void run_mtc_callback(const std_msgs::msg::Bool::SharedPtr msg);


  mtc::Task task_;
  rclcpp::Node::SharedPtr node_;

  // Subscriptions
  rclcpp::Subscription<std_msgs::msg::Float64MultiArray>::SharedPtr circles_data_sub_; // Renamed
  rclcpp::Subscription<std_msgs::msg::Float64MultiArray>::SharedPtr camera_data_storage_sub_; // Stays the same topic name

  rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr spawn_workpieces_sub_;
  rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr run_mtc_sub_;

  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr marker_pub_;

  // State variables
  bool workpieces_spawned_ = false;
  bool task_planned_executed_ = false;
  bool circles_data_received_ = false; // Flag for circles data
  bool storage_pose_data_received_ = false; // New flag for storage pose data
  bool target_storage_exists_ = false;

  // Class members to store storage pose data derived from camera
  float x_mid_storage = 0.0;
  float y_mid_storage = 0.0;
  float z_storage = 0.0; // Initialize to 0.0, will be set by callback
  float alfa = 0.0;      // Yaw angle for storage orientation

  geometry_msgs::msg::PoseStamped target_place_pose_; // Used for object placement, not storage spawn

  // ... other members (TF, start_storage_pose, etc.) ...
  std::optional<geometry_msgs::msg::Pose> get_start_storage_pose();
  std::optional<geometry_msgs::msg::Pose> start_storage_pose_; 

  std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
  std::unique_ptr<tf2_ros::TransformListener> tf_listener_;
};

MTCTaskNode::MTCTaskNode(const rclcpp::NodeOptions& options)
  : node_{ std::make_shared<rclcpp::Node>("mtc_node", options) }
{
  // Camera data subscription (circles) - Renamed callback
  circles_data_sub_ = node_->create_subscription<std_msgs::msg::Float64MultiArray>(
    "camera/circles_to_world", 10, std::bind(&MTCTaskNode::circles_data_callback, this, std::placeholders::_1));

  // Camera data subscription (storage) - New callback
  camera_data_storage_sub_ = node_->create_subscription<std_msgs::msg::Float64MultiArray>(
    "camera/storage_to_world", 10, std::bind(&MTCTaskNode::storage_data_callback, this, std::placeholders::_1)); // Use new callback

  // Button subscriptions
  spawn_workpieces_sub_ = node_->create_subscription<std_msgs::msg::Bool>(
    "button1", 10, std::bind(&MTCTaskNode::spawn_workpieces_callback, this, std::placeholders::_1));

  run_mtc_sub_ = node_->create_subscription<std_msgs::msg::Bool>(
    "button2", 10, std::bind(&MTCTaskNode::run_mtc_callback, this, std::placeholders::_1));

  // Publisher for visualization markers
  marker_pub_ = node_->create_publisher<visualization_msgs::msg::MarkerArray>(
    "mtc_node/visualization_marker_array", 10);

  target_place_pose_.header.frame_id = "world";
  target_place_pose_.pose.orientation.w = 1.0;

  // Initialize TF components
  tf_buffer_ = std::make_unique<tf2_ros::Buffer>(node_->get_clock());
  tf_listener_ = std::make_unique<tf2_ros::TransformListener>(*tf_buffer_);

  // TF lookup for start_storage pose (used for adding small objects relative to TF,
  // distinct from the main storage mesh added from camera data)
  start_storage_pose_ = get_start_storage_pose();


  RCLCPP_INFO(LOGGER, "MTCTaskNode initialized. Waiting for button presses and camera data...");
}

rclcpp::node_interfaces::NodeBaseInterface::SharedPtr MTCTaskNode::getNodeBaseInterface()
{
  return node_->get_node_base_interface();
}

// Renamed from camera_data_callback
void MTCTaskNode::circles_data_callback(const std_msgs::msg::Float64MultiArray::SharedPtr msg)
{
    // Existing logic to handle circle data and visualize markers
    const size_t expected_elements = 6 * 4;
    if (msg->data.size() < expected_elements) {
        RCLCPP_WARN(LOGGER, "Received circles data array is smaller than expected (got %zu, expected %zu elements)", msg->data.size(), expected_elements);
        return;
    }

    {
        std::lock_guard<std::mutex> lock(data_mutex_);
        for (size_t i = 0; i < 6; ++i) {
            for (size_t j = 0; j < 4; ++j) {
                size_t index = i * 4 + j;
                if (index < msg->data.size()) {
                    data_array[i][j] = msg->data[index];
                } else {
                     RCLCPP_ERROR(LOGGER, "Unexpected index out of bounds during circle data copy.");
                     break;
                }
            }
        }
        circles_data_received_ = true; // Indicate circles data received
    } // End mutex scope

    // Marker visualization remains the same (for circles)
    visualization_msgs::msg::MarkerArray marker_array;
    marker_array.markers.resize(6);

    for (size_t i = 0; i < 6; ++i) {
        marker_array.markers[i].header.frame_id = "world";
        marker_array.markers[i].header.stamp = node_->now();
        marker_array.markers[i].ns = "circles_" + std::to_string(i);
        marker_array.markers[i].id = i;
        marker_array.markers[i].type = visualization_msgs::msg::Marker::CYLINDER;
        marker_array.markers[i].action = visualization_msgs::msg::Marker::ADD;
        marker_array.markers[i].pose.position.x = data_array[i][0];
        marker_array.markers[i].pose.position.y = data_array[i][1];
        marker_array.markers[i].pose.position.z = data_array[i][2];
        marker_array.markers[i].pose.orientation.x = 0.0;
        marker_array.markers[i].pose.orientation.y = 0.0;
        marker_array.markers[i].pose.orientation.z = 0.0;
        marker_array.markers[i].pose.orientation.w = 1.0;
        marker_array.markers[i].scale.x = 0.043; // Diameter
        marker_array.markers[i].scale.y = 0.043; // Diameter
        marker_array.markers[i].scale.z = 0.001; // Height
        if (data_array[i][3] == 0){
            marker_array.markers[i].color.r = 0.0f;
            marker_array.markers[i].color.g = 1.0f;
            marker_array.markers[i].color.b = 0.0f;
        } else {
            marker_array.markers[i].color.r = 1.0f;
            marker_array.markers[i].color.g = 0.0f;
            marker_array.markers[i].color.b = 0.0f;
        }
        marker_array.markers[i].color.a = 1.0f;
        marker_array.markers[i].lifetime = rclcpp::Duration::from_seconds(2.0);
    }

    marker_pub_->publish(marker_array);
}

void MTCTaskNode::storage_data_callback(const std_msgs::msg::Float64MultiArray::SharedPtr msg)
{
    // Expected data format: [x, y, z, yaw]
    if (msg->data.size() < 4) {
        storage_pose_data_received_ = false; // Data not valid
        return;
    }

    {
        std::lock_guard<std::mutex> lock(data_mutex_);
        // Store the received pose data in class members
        x_mid_storage = msg->data[0];
        y_mid_storage = msg->data[1];
        z_storage = msg->data[2];
        alfa = msg->data[3]; // Assuming the 4th element is the yaw angle
        storage_pose_data_received_ = true; // Indicate valid storage data received
    } // End mutex scope

    // --- Visualization Part ---
    visualization_msgs::msg::Marker storage_marker;
    storage_marker.header.frame_id = "world";
    storage_marker.header.stamp = node_->now();
    storage_marker.ns = "storage_object";
    storage_marker.id = 0; // Unique ID for this marker namespace
    storage_marker.action = visualization_msgs::msg::Marker::ADD;
    storage_marker.type = visualization_msgs::msg::Marker::MESH_RESOURCE; // Use MESH_RESOURCE
    storage_marker.mesh_resource = "package://mycobot_gazebo/models/storage/collision/storage.stl"; // Path to the mesh

    // Set the pose using the received camera data
    {
         std::lock_guard<std::mutex> lock(data_mutex_); // Lock again to access stored data
         storage_marker.pose.position.x = x_mid_storage;
         storage_marker.pose.position.y = y_mid_storage;
         storage_marker.pose.position.z = z_storage; // Using the Z from camera data

         // Convert yaw (alfa) to quaternion
         tf2::Quaternion quaternion;
         quaternion.setRPY(0, 0, alfa); // Set roll=0, pitch=0, yaw=alfa
         storage_marker.pose.orientation.x = quaternion.x();
         storage_marker.pose.orientation.y = quaternion.y();
         storage_marker.pose.orientation.z = quaternion.z();
         storage_marker.pose.orientation.w = quaternion.w();
    }


    // Set scale (often 1,1,1 if the mesh has correct units)
    storage_marker.scale.x = 1.0;
    storage_marker.scale.y = 1.0;
    storage_marker.scale.z = 1.0;

    // Set color (e.g., a light grey)
    storage_marker.color.r = 0.8f;
    storage_marker.color.g = 0.8f;
    storage_marker.color.b = 0.8f;
    storage_marker.color.a = 0.7f; // Slightly transparent

    // Set lifetime so it doesn't disappear immediately
    storage_marker.lifetime = rclcpp::Duration::from_seconds(2.0); // Stays for 2 seconds, then refreshed

    // Publish the marker (wrap in MarkerArray)
    visualization_msgs::msg::MarkerArray marker_array_storage;
    marker_array_storage.markers.push_back(storage_marker);
    marker_pub_->publish(marker_array_storage);

}

// Callback for button1 (Spawn Workpieces/Setup Scene)
void MTCTaskNode::spawn_workpieces_callback(const std_msgs::msg::Bool::SharedPtr msg)
{
    if (msg->data) {
        if (!workpieces_spawned_) {
            // Require storage pose data before setting up the scene
            if (!storage_pose_data_received_) {
                 RCLCPP_WARN(LOGGER, "Received button1 press, but storage pose data has not been received yet. Cannot setup planning scene.");
                 return;
            }
            RCLCPP_INFO(LOGGER, "Received button1 press (true). Attempting to setup planning scene...");
            setupPlanningScene();
            workpieces_spawned_ = true;
            task_planned_executed_ = false;
        } else {
            RCLCPP_INFO(LOGGER, "Received button1 press (true), but workpieces already spawned.");
            task_planned_executed_ = false;
        }
    }
}

// Callback for button2 (Run MTC)
void MTCTaskNode::run_mtc_callback(const std_msgs::msg::Bool::SharedPtr msg)
{
     if (msg->data) {
        if (!workpieces_spawned_) {
            RCLCPP_WARN(LOGGER, "Received button2 press (true), but workpieces are not yet spawned.");
            return;
        }
         if (task_planned_executed_) {
             RCLCPP_INFO(LOGGER, "Received button2 press (true), but MTC task already completed this cycle.");
             return;
         }

        {
             std::lock_guard<std::mutex> lock(data_mutex_);
             if (!circles_data_received_) {
                 RCLCPP_WARN(LOGGER, "Received button2 press (true), but no circles data has been received yet. Cannot set target place pose or pick objects.");
                 return;
             }
        }

        RCLCPP_INFO(LOGGER, "Received button2 press (true). Running MTC task...");
        doTask();
     }
}

std::optional<geometry_msgs::msg::Pose> MTCTaskNode::get_start_storage_pose()
{
    const std::string target_frame = "world";
    const std::string source_frame = "start_storage";

    geometry_msgs::msg::TransformStamped transform_stamped;

    try {
        transform_stamped = tf_buffer_->lookupTransform(
            target_frame, source_frame,
            tf2::TimePointZero, 5s);

        geometry_msgs::msg::Pose pose;

        pose.position.x = transform_stamped.transform.translation.x;
        pose.position.y = transform_stamped.transform.translation.y;
        pose.position.z = transform_stamped.transform.translation.z;

        pose.orientation = transform_stamped.transform.rotation;

        RCLCPP_INFO(LOGGER, "TF Lookup successful for %s to %s. Position: (%.3f, %.3f, %.3f), Orientation: (%.3f, %.3f, %.3f, %.3f)",
                    source_frame.c_str(), target_frame.c_str(),
                    pose.position.x, pose.position.y, pose.position.z,
                    pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w);

        return pose;

    } catch (tf2::TransformException &ex) {
        RCLCPP_ERROR(LOGGER, "Could not transform %s to %s: %s",
                    source_frame.c_str(), target_frame.c_str(), ex.what());
        return std::nullopt;
    }
}


void MTCTaskNode::setupPlanningScene()
{
    RCLCPP_INFO(LOGGER, "Setting up planning scene...");
    moveit::planning_interface::PlanningSceneInterface psi;

    // // Clear any old objects
    // std::vector<std::string> object_ids_to_remove;
    // object_ids_to_remove.push_back("target_storage");
    // for(size_t i=0; i<6; ++i) object_ids_to_remove.push_back("object_" + std::to_string(i));
    // psi.removeCollisionObjects(object_ids_to_remove);
    // RCLCPP_INFO(LOGGER, "Removed previous collision objects.");


    // moveit_msgs::msg::CollisionObject storage_object;
    // storage_object.header.frame_id = "world";
    // storage_object.id = "target_storage";

    // shapes::Mesh* raw_mesh = shapes::createMeshFromResource("package://mycobot_gazebo/models/storage/collision/storage.stl");

    // if (!raw_mesh) {
    //     RCLCPP_ERROR(LOGGER, "Failed to load storage mesh from resource. Cannot add storage collision object.");
    //     target_storage_exists_ = false;
    //     // Continue to add smaller objects if TF is available, or return.
    //     // Let's add smaller objects if TF is available, even if storage mesh failed.
    // } else {
    //     shape_msgs::msg::Mesh converted_mesh;
    //     shapes::ShapeMsg mesh_msg;
    //     shapes::constructMsgFromShape(raw_mesh, mesh_msg);
    //     converted_mesh = boost::get<shape_msgs::msg::Mesh>(mesh_msg);

    //     geometry_msgs::msg::Pose mesh_pose;
    //     {
    //         std::lock_guard<std::mutex> lock(data_mutex_);
    //         // Use the pose data received from the camera callback
    //         mesh_pose.position.x = x_mid_storage;
    //         mesh_pose.position.y = y_mid_storage;
    //         // Use the Z from camera data, or a fixed Z if the camera data Z is unreliable for the base
    //         mesh_pose.position.z = z_storage; // Using Z from camera data
    //          // If the camera detects the top surface Z, you might need to subtract half the storage height
    //          // For example, if storage height is 0.05m: mesh_pose.position.z = z_storage - 0.025;
    //          // The provided Z_storage=0.9001 suggests a fixed world Z, maybe ignore camera Z here?
    //          // Let's stick to using the camera Z as requested, but be aware of potential issues.
    //         // mesh_pose.position.z = 0.9001; // Alternative: use fixed Z

    //         // Set orientation based on the yaw angle from camera data
    //         tf2::Quaternion quaternion;
    //         quaternion.setRPY(0, 0, alfa); // Set roll=0, pitch=0, yaw=alfa
    //         mesh_pose.orientation.x = quaternion.x();
    //         mesh_pose.orientation.y = quaternion.y();
    //         mesh_pose.orientation.z = quaternion.z();
    //         mesh_pose.orientation.w = quaternion.w();

    //         RCLCPP_INFO(LOGGER, "Adding target_storage mesh at (%.3f, %.3f, %.3f) with yaw %.3f rad.",
    //                     mesh_pose.position.x, mesh_pose.position.y, mesh_pose.position.z, alfa);
    //     }


    //     storage_object.meshes.push_back(converted_mesh);
    //     storage_object.mesh_poses.push_back(mesh_pose);
    //     storage_object.operation = storage_object.ADD;

    //     std::vector<moveit_msgs::msg::CollisionObject> collision_objects = { storage_object };
    //     psi.addCollisionObjects(collision_objects);
    //     target_storage_exists_ = true;

    //     delete raw_mesh;
    // }


    // The following block adds smaller cylinder objects,
    // which seems intended for the pick locations *within* the storage.
    // This part uses the TF lookup `start_storage_pose_`, which might be different
    // from the pose derived from camera data for the main storage mesh.
    // This section is kept from the original code structure but note the potential
    // inconsistency if the TF and camera data for storage base are not aligned.
    if (start_storage_pose_) {
        const geometry_msgs::msg::Pose& storage_pose = start_storage_pose_.value();

        tf2::Quaternion q(
            storage_pose.orientation.x,
            storage_pose.orientation.y,
            storage_pose.orientation.z,
            storage_pose.orientation.w);

        double yaw = tf2::getYaw(q);

        std::vector<float> x_diff;
        std::vector<float> y_diff;
        float z_offset;

        const double PI = M_PI;
        bool is_yaw_0_or_180 = std::abs(yaw) < 0.1 || std::abs(yaw - PI) < 0.1 || std::abs(yaw + PI) < 0.1;
        bool is_yaw_plus_minus_90 = std::abs(yaw - PI/2.0) < 0.1 || std::abs(yaw + PI/2.0) < 0.1;


        if (is_yaw_0_or_180) {
            RCLCPP_INFO(LOGGER, "Detected TF Yaw close to 0 or 180 degrees (%.2f rad) for smaller objects layout.", yaw);
            x_diff = {-0.0325, -0.0325, -0.0325, 0.0325, 0.0325, 0.0325};
            y_diff = {-0.06, 0, 0.06, -0.06, 0, 0.06};
            z_offset = 0.925; // This Z offset seems related to the TF's height
        } else if (is_yaw_plus_minus_90) {
             RCLCPP_INFO(LOGGER, "Detected TF Yaw close to 90 or -90 degrees (%.2f rad) for smaller objects layout.", yaw);
             x_diff = {-0.06, 0, 0.06, -0.06, 0, 0.06};
             y_diff = {-0.0325, -0.0325, -0.0325, 0.0325, 0.0325, 0.0325};
             z_offset = 0.925; // This Z offset seems related to the TF's height
        }
         else {
             RCLCPP_WARN(LOGGER, "Detected TF Yaw is %.2f rad, which is not close to 0, 90, 180, or -90 degrees. Cannot determine smaller object layout based on TF.", yaw);
             // Do not add smaller objects if TF orientation is unexpected
             return;
        }

        if (x_diff.size() != 6 || y_diff.size() != 6) {
             RCLCPP_ERROR(LOGGER, "Internal error: Diff vector sizes are incorrect (%zu, %zu). Cannot add smaller collision objects.", x_diff.size(), y_diff.size());
             return;
        }

        std::vector<moveit_msgs::msg::CollisionObject> objects_to_add;
        objects_to_add.reserve(6); // Reserve space for efficiency

        for (size_t i = 0; i < 6; ++i) {
            moveit_msgs::msg::CollisionObject object;
            geometry_msgs::msg::Pose object_pose;

            object.id = "object_" + std::to_string(i);
            object.header.frame_id = "world";

            shape_msgs::msg::SolidPrimitive cylinder;
            cylinder.type = shape_msgs::msg::SolidPrimitive::CYLINDER;
            cylinder.dimensions.resize(2);
            cylinder.dimensions[0] = 0.043; // Height
            cylinder.dimensions[1] = 0.008; // Radius
            object.primitives.push_back(cylinder);

            // Calculate position relative to the TF frame pose
            if (is_yaw_plus_minus_90) {
                object_pose.position.x = storage_pose.position.x + x_diff[i];
                object_pose.position.y = storage_pose.position.y + y_diff[i];
            } else { // Assuming is_yaw_0_or_180
                 object_pose.position.x = storage_pose.position.x - y_diff[i];
                 object_pose.position.y = storage_pose.position.y + x_diff[i];
            }

            object_pose.position.z = cylinder.dimensions[0]/2 + z_offset; // Z relative to TF Z base + cylinder half height

            object_pose.orientation = storage_pose.orientation; // Inherit orientation from TF

            object.pose = object_pose;

            object.operation = moveit_msgs::msg::CollisionObject::ADD;

            objects_to_add.push_back(object);

            RCLCPP_INFO(LOGGER, "Adding object %zu at (%.3f, %.3f, %.3f) based on TF.",
                        i, object_pose.position.x, object_pose.position.y, object_pose.position.z);
        }

        if (!objects_to_add.empty()) {
             psi.applyCollisionObjects(objects_to_add);
             RCLCPP_INFO(LOGGER, "Applied %zu smaller collision objects based on TF.", objects_to_add.size());
        } else {
             RCLCPP_WARN(LOGGER, "No smaller collision objects were generated to apply based on TF.");
        }

    } else {
        RCLCPP_WARN(LOGGER, "Start storage TF pose not available. Cannot add smaller collision objects.");
    }
}
void MTCTaskNode::doTask()
{
  { 
      std::lock_guard<std::mutex> lock(data_mutex_); 

      if (!circles_data_received_) { 
          RCLCPP_ERROR(LOGGER, "No camera data received yet. Cannot set target place pose for MTC."); 
          return;
      }
      if (std::abs(data_array[5][0]) < 1e-6 && std::abs(data_array[5][1]) < 1e-6 && std::abs(data_array[5][2]) < 1e-6) { 
           RCLCPP_ERROR(LOGGER, "Camera data for object 5 appears invalid (all zeros). Cannot set target place pose for MTC.");
           return; 
      }

      target_place_pose_.header.frame_id = "world"; 
      target_place_pose_.header.stamp = node_->now(); 
      target_place_pose_.pose.position.x = data_array[5][0];
      target_place_pose_.pose.position.y = data_array[5][1];
      target_place_pose_.pose.position.z = data_array[5][2] + 0.043/2 + 0.001; // Adjusted height
      target_place_pose_.pose.orientation.w = 1.0; 

      RCLCPP_INFO(LOGGER, "Setting target place pose for THIS task run to (%.3f, %.3f, %.3f)",
                  target_place_pose_.pose.position.x, 
                  target_place_pose_.pose.position.y, 
                  target_place_pose_.pose.position.z);
  }

  task_ = createTask();

  try
  {
    task_.init();
  }
  catch (mtc::InitStageException& e)
  {
    RCLCPP_ERROR_STREAM(LOGGER, "Task initialization failed: " << e.what()); 
    return; 
  }

  RCLCPP_INFO(LOGGER, "Task initialized. Planning..."); 

  if (!task_.plan(5)) 
  {
    RCLCPP_ERROR_STREAM(LOGGER, "Task planning failed");
    return;
  }

  RCLCPP_INFO(LOGGER, "Task planning succeeded. Publishing solution..."); 
  task_.introspection().publishSolution(*task_.solutions().front()); 

  RCLCPP_INFO(LOGGER, "Executing task..."); 
  auto result = task_.execute(*task_.solutions().front());
  if (result.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS) { 
    RCLCPP_ERROR_STREAM(LOGGER, "Task execution failed with code: " << result.val); 
    return; 
  }

  RCLCPP_INFO(LOGGER, "Task executed successfully."); 
  task_planned_executed_ = true; 
}

mtc::Task MTCTaskNode::createTask()
{
// ======================================================================================================== //
// ========================================= PICK AND PLACE =============================================== //
// ======================================================================================================== //
  mtc::Task task;
  task.stages()->setName("Pick and Place Task");
  task.loadRobotModel(node_);

  const auto& arm_group_name = "arm_controller";
  const auto& hand_group_name = "gripper";
  const auto& hand_frame = "ee_link";
  const auto& eef = "eef_gripper";

  task.setProperty("group", arm_group_name);
  task.setProperty("eef", eef);
  task.setProperty("ik_frame", hand_frame);

  std::string object_name = "object_5";

  // Disable warnings for this line, as it's a variable that's set but not used in this example
  #pragma GCC diagnostic push
  #pragma GCC diagnostic ignored "-Wunused-but-set-variable"
  #pragma GCC diagnostic pop
  mtc::Stage* current_state_ptr = nullptr; 

  // ================================ GET STARTING POSITION (CURRENT) ================================ //
  auto stage_state_current = std::make_unique<mtc::stages::CurrentState>("current");
  current_state_ptr = stage_state_current.get();
  task.add(std::move(stage_state_current));

  auto sampling_planner = std::make_shared<mtc::solvers::PipelinePlanner>(node_);
  auto interpolation_planner = std::make_shared<mtc::solvers::JointInterpolationPlanner>();

  auto cartesian_planner = std::make_shared<mtc::solvers::CartesianPath>();
  cartesian_planner->setMaxVelocityScalingFactor(0.1);
  cartesian_planner->setMaxAccelerationScalingFactor(0.1);
  cartesian_planner->setStepSize(.01);

  // ======================================== OPEN GRIPPER ============================================== //
  auto stage_open_hand = std::make_unique<mtc::stages::MoveTo>("open hand", interpolation_planner);
  stage_open_hand->setGroup(hand_group_name);
  stage_open_hand->setGoal("open");
  task.add(std::move(stage_open_hand));


  // ======================================== MOVE TO PICK ============================================== //
  auto stage_move_to_pick = std::make_unique<mtc::stages::Connect>("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));

  // Forward attach_object_stage to place pose generator
  mtc::Stage* attach_object_stage =
      nullptr;
  // ========================================= PICK OBJECT =============================================== //
  {
    auto grasp = std::make_unique<mtc::SerialContainer>("pick object");
    task.properties().exposeTo(grasp->properties(), { "eef", "group", "ik_frame" });
    grasp->properties().configureInitFrom(mtc::Stage::PARENT,
                                        { "eef", "group", "ik_frame" });
  // ======================================== APPROACH OBJECT ============================================== //
    {
      auto stage =
        std::make_unique<mtc::stages::MoveRelative>("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));
    }
  // ========================================== GRASP OBJECT ================================================ //
  {
    auto stage = std::make_unique<mtc::stages::GenerateGraspPose>("generate grasp pose");
    stage->properties().configureInitFrom(mtc::Stage::PARENT);
    stage->properties().set("marker_ns", "grasp_pose");
    stage->setPreGraspPose("open");
    stage->setObject(object_name);
    stage->setAngleDelta(M_PI / 12);
    stage->setMonitoredStage(current_state_ptr);

    // This is the transform from the object frame to the end-effector frame
    Eigen::Isometry3d grasp_frame_transform;
    // Initial orientation for the grasp relative to the object
    Eigen::Quaterniond q_initial = Eigen::AngleAxisd(M_PI / 2.0, Eigen::Vector3d::UnitX()) *
                                   Eigen::AngleAxisd(M_PI / 2.0, Eigen::Vector3d::UnitY()) *
                                   Eigen::AngleAxisd(M_PI / 2.0, Eigen::Vector3d::UnitZ());
    grasp_frame_transform.linear() = q_initial.matrix();
    grasp_frame_transform.translation().z() = 0.095;


    // Compute IK
    auto wrapper = std::make_unique<mtc::stages::ComputeIK>("grasp pose IK", std::move(stage));
    wrapper->setMaxIKSolutions(8);
    wrapper->setMinSolutionDistance(1.0);
    // The ComputeIK stage will now use the modified grasp_frame_transform
    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));
  }
  // ======================================== ALLOW COLLISION ============================================== //
  // ================================= *to connect gripper with object ===================================== //
    {
      auto stage = std::make_unique<mtc::stages::ModifyPlanningScene>("allow collision (hand+object)");
      stage->allowCollisions(object_name,
                            task.getRobotModel()
                                ->getJointModelGroup(hand_group_name)
                                ->getLinkModelNamesWithCollisionGeometry(),
                            true);
      grasp->insert(std::move(stage));
    }
  // ========================================= CLOSE GRIPPER =============================================== //
    {
      auto stage = std::make_unique<mtc::stages::MoveTo>("close hand", interpolation_planner);
      stage->setGroup(hand_group_name);
      stage->setGoal("closed");
      grasp->insert(std::move(stage));
    }
  // ========================================= ATTACHE OBJECT =============================================== //
    {
      auto stage = std::make_unique<mtc::stages::ModifyPlanningScene>("attach object");
      stage->attachObject(object_name, hand_frame);
      attach_object_stage = stage.get();
      grasp->insert(std::move(stage));
    }
  // =========================================== LIFT OBJECT ================================================ //
    {
      auto stage = std::make_unique<mtc::stages::MoveRelative>("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));
    }
    
    {
      auto stage = std::make_unique<mtc::stages::ModifyPlanningScene>("forbid collision (object, support_surface)");
      stage->allowCollisions(object_name,
                            task.getRobotModel()
                                ->getJointModelGroup(hand_group_name)
                                ->getLinkModelNamesWithCollisionGeometry(),
                            false);
      grasp->insert(std::move(stage));
    }
    task.add(std::move(grasp));
  }
  // ======================================== MOVE TO PLACE ============================================== //
  {
    auto stage_move_to_place = std::make_unique<mtc::stages::Connect>(
        "move to place",
        mtc::stages::Connect::GroupPlannerVector{ { arm_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));
  }
  // ======================================== PLACE OBJECT ============================================== //
  {
      auto place = std::make_unique<mtc::SerialContainer>("place object");
      task.properties().exposeTo(place->properties(), { "eef", "group", "ik_frame" });
      place->properties().configureInitFrom(mtc::Stage::PARENT,
                                            { "eef", "group", "ik_frame" });

  // ======================================== LOWER OBJECT ============================================== //
      {
        auto stage = std::make_unique<mtc::stages::GeneratePlacePose>("generate place pose");
        stage->properties().configureInitFrom(mtc::Stage::PARENT);
        stage->properties().set("marker_ns", "place_pose");
        stage->setObject(object_name);

        stage->setPose(target_place_pose_);
        stage->setMonitoredStage(attach_object_stage);  

        auto wrapper =
            std::make_unique<mtc::stages::ComputeIK>("place pose IK", std::move(stage));
        wrapper->setMaxIKSolutions(2);
        wrapper->setMinSolutionDistance(1.0);
        wrapper->setIKFrame(object_name);
        wrapper->properties().configureInitFrom(mtc::Stage::PARENT, { "eef", "group" });
        wrapper->properties().configureInitFrom(mtc::Stage::INTERFACE, { "target_pose" });
        place->insert(std::move(wrapper));
      }
  // ======================================== OPEN GRIPPER ============================================== //
      {
        auto stage = std::make_unique<mtc::stages::MoveTo>("open hand", interpolation_planner);
        stage->setGroup(hand_group_name);
        stage->setGoal("open");
        place->insert(std::move(stage));
      }
  // ========================================= DETACH OBJECT ============================================= //
      {
        auto stage = std::make_unique<mtc::stages::ModifyPlanningScene>("detach object");
        stage->detachObject(object_name, hand_frame);
        // attach_object_stage = stage.get();
        place->insert(std::move(stage));
      }
    task.add(std::move(place));
  }
  // =========================================== GO HOME ================================================ //
  {
    auto stage = std::make_unique<mtc::stages::MoveTo>("return home", interpolation_planner);
    stage->properties().configureInitFrom(mtc::Stage::PARENT, { "group" });
    stage->setGoal("home");
    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<MTCTaskNode>(options);
  rclcpp::executors::MultiThreadedExecutor executor;

  auto spin_thread = std::make_unique<std::thread>([&executor, &mtc_task_node]() {
    executor.add_node(mtc_task_node->getNodeBaseInterface());
    executor.spin();
    executor.remove_node(mtc_task_node->getNodeBaseInterface());
  });

  RCLCPP_INFO(LOGGER, "MTC node is spinning and waiting for button presses...");

  spin_thread->join();

  rclcpp::shutdown();
  return 0;
}

[rhaschke]

This looks like an ABI incompatibility. Try to rebuild your workspace from scratch.

[Marcel3245]

Hello,
Thanks for the quick response. I have completely reset my programming environment, even unregistering and reinstalling Ubuntu-24.04 in WSL, essentially starting from scratch. Despite this, I'm still encountering the same error. I can provide more details if that would be helpfull.

error:
[move_group-9] Stack trace (most recent call last) in thread 18325:
[gazebo-6] [INFO] [1748868740.834113709] [gripper_action_controller]: Received & accepted new action goal
[move_group-9] #10 Object "", at 0xffffffffffffffff, in
[move_group-9] #9 Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7de60d929c3b, in
[move_group-9] #8 Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7de60d89caa3, in
[move_group-9] #7 Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.33", at 0x7de60dcecdb3, in
[move_group-9] #6 Object "/home/marcel3245/ros2_ws/build/moveit_task_constructor_capabilities/libmoveit_task_constructor_capabilities.so", at 0x7de5d4320157, in std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<void (move_group::ExecuteTaskSolutionCapability::)(std::shared_ptr<rclcpp_action::ServerGoalHandle<moveit_task_constructor_msgs::action::ExecuteTaskSolution> > const&), move_group::ExecuteTaskSolutionCapability, std::shared_ptr<rclcpp_action::ServerGoalHandle<moveit_task_constructor_msgs::action::ExecuteTaskSolution> > > >, void>::_M_run()
[move_group-9] #5 Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7de60d8a1ed2, in
[move_group-9] #4 | Source "/usr/include/c++/13/future", line 589, in operator()
[move_group-9] | 587: _M_do_set(function<_Ptr_type()>* __f, bool* __did_set)
[move_group-9] | 588: {
[move_group-9] | > 589: _Ptr_type __res = (__f)();
[move_group-9] | 590: // Notify the caller that we did try to set; if we do not throw an
[move_group-9] | 591: // exception, the caller will be aware that it did set (e.g., see
[move_group-9] Source "/usr/include/c++/13/bits/std_function.h", line 591, in _M_do_set [0x7de60e5367f1]
[move_group-9] 588: {
[move_group-9] 589: if (_M_empty())
[move_group-9] 590: __throw_bad_function_call();
[move_group-9] > 591: return _M_invoker(_M_functor, std::forward<_ArgTypes>(__args)...);
[move_group-9] 592: }
[move_group-9] 593:
[move_group-9] 594: #if __cpp_rtti
[move_group-9] #3 Object "/home/marcel3245/ros2_ws/build/moveit_task_constructor_capabilities/libmoveit_task_constructor_capabilities.so", at 0x7de5d431ed1f, in std::_Function_handler<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> (), std::__future_base::_Task_setter<std::unique_ptr<std::__future_base::_Result, std::__future_base::_Result_base::_Deleter>, std::thread::_Invoker<std::tuple<void (move_group::ExecuteTaskSolutionCapability::)(std::shared_ptr<rclcpp_action::ServerGoalHandle<moveit_task_constructor_msgs::action::ExecuteTaskSolution> > const&), move_group::ExecuteTaskSolutionCapability*, std::shared_ptr<rclcpp_action::ServerGoalHandle<moveit_task_constructor_msgs::action::ExecuteTaskSolution> > > >, void> >::_M_invoke(std::_Any_data const&)
[move_group-9] #2 Object "/home/marcel3245/ros2_ws/build/moveit_task_constructor_capabilities/libmoveit_task_constructor_capabilities.so", at 0x7de5d431e29e, in move_group::ExecuteTaskSolutionCapability::execCallback(std::shared_ptr<rclcpp_action::ServerGoalHandle<moveit_task_constructor_msgs::action::ExecuteTaskSolution> > const&)
[move_group-9] #1 Source "/usr/src/ros-jazzy-moveit-ros-planning-2.12.3-1noble.20250520.213359/plan_execution/src/plan_execution.cpp", line 475, in executeAndMonitor [0x7de60e2b003c]
[move_group-9] #0 | Source "/usr/src/ros-jazzy-moveit-core-2.12.3-1noble.20250520.202002/robot_state/src/robot_state.cpp", line 1226, in begin
[move_group-9] | Source "/usr/include/c++/13/bits/stl_map.h", line 378, in begin
[move_group-9] | 376: const_iterator
[move_group-9] | 377: begin() const _GLIBCXX_NOEXCEPT
[move_group-9] | > 378: { return _M_t.begin(); }
[move_group-9] | 379:
[move_group-9] | 380: /**
[move_group-9] Source "/usr/include/c++/13/bits/stl_tree.h", line 1002, in getAttachedBodies [0x7de60dfc474b]
[move_group-9] 1000: const_iterator
[move_group-9] 1001: begin() const _GLIBCXX_NOEXCEPT
[move_group-9] >1002: { return const_iterator(this->_M_impl._M_header._M_left); }
[move_group-9] 1003:
[move_group-9] 1004: iterator
[move_group-9] 1005: end() _GLIBCXX_NOEXCEPT
[move_group-9] Segmentation fault (Address not mapped to object [0xe8])
[ERROR] [move_group-9]: process has died [pid 17634, exit code -11, cmd '/opt/ros/jazzy/lib/moveit_ros_move_group/move_group --ros-args --params-file /tmp/launch_params_itgyca_q --params-file /tmp/launch_params_0k_4f923 --params-file /tmp/launch_params_k6n64669 --params-file /tmp/launch_params_qju79jc5 --params-file /tmp/launch_params_d50bzkzm --params-file /tmp/launch_params__mvmu2vl --params-file /tmp/launch_params_zhgu7e82 --params-file /tmp/launch_params_2e4j1sgs --params-file /tmp/launch_params_16il1rcf --params-file /tmp/launch_params_20oou7ni --params-file /tmp/launch_params_rcl482rn'].

[rhaschke]

Does the standard pick+place example of MTC work as expected?

[rhaschke]

Looks like a bug recently introduced in MoveIt (see here).

[Marcel3245]

The behaviour with the pick_place_demo remains the same. The process consistently fails as previously observed.
I forgot to add, because how rarely it occurs, but: Very occasionally (after numerous attempts), the real robot initiates movement, but travels only a few centimetres before the process terminates (the image shows, small relocation).

error this time in demo.launch.py:

[ros2_control_node-5] [INFO] [1748871802.724904447] [panda_hand_controller]: Received & accepted new action goal
[move_group-4] #10   Object "", at 0xffffffffffffffff, in
[move_group-4] #9    Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x73701f529c3b, in
[move_group-4] #8    Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x73701f49caa3, in
[move_group-4] #7    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.33", at 0x73701f8ecdb3, in
[move_group-4] #6    Object "/home/marcel3245/ros2_ws/build/moveit_task_constructor_capabilities/libmoveit_task_constructor_capabilities.so", at 0x736fea501157, in std::__future_base::_Async_state_impl<std::thread::_Invoker<std::tuple<void (move_group::ExecuteTaskSolutionCapability::*)(std::shared_ptr<rclcpp_action::ServerGoalHandle<moveit_task_constructor_msgs::action::ExecuteTaskSolution> > const&), move_group::ExecuteTaskSolutionCapability*, std::shared_ptr<rclcpp_action::ServerGoalHandle<moveit_task_constructor_msgs::action::ExecuteTaskSolution> > > >, void>::_M_run()
[move_group-4] #5    Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x73701f4a1ed2, in
[move_group-4] [INFO] [1748871802.776429149] [move_group.moveit.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list
[move_group-4] [INFO] [1748871802.776515220] [move_group.moveit.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list
[move_group-4] [INFO] [1748871802.776667332] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to panda_arm_controller
[ros2_control_node-5] [INFO] [1748871802.777088393] [panda_arm_controller]: Received new action goal
[ros2_control_node-5] [INFO] [1748871802.777155391] [panda_arm_controller]: Accepted new action goal
[move_group-4] [INFO] [1748871802.777354468] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: panda_arm_controller started execution
[move_group-4] [INFO] [1748871802.777386194] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted!
[move_group-4] #4  | Source "/usr/include/c++/13/future", line 589, in operator()
[move_group-4]     |   587:       _M_do_set(function<_Ptr_type()>* __f, bool* __did_set)
[move_group-4]     |   588:       {
[move_group-4]     | > 589:         _Ptr_type __res = (*__f)();
[move_group-4]     |   590:         // Notify the caller that we did try to set; if we do not throw an
[move_group-4]     |   591:         // exception, the caller will be aware that it did set (e.g., see
[move_group-4]       Source "/usr/include/c++/13/bits/std_function.h", line 591, in _M_do_set [0x73702012a7f1]
[move_group-4]         588:       {
[move_group-4]         589:     if (_M_empty())
[move_group-4]         590:       __throw_bad_function_call();
[move_group-4]       > 591:     return _M_invoker(_M_functor, std::forward<_ArgTypes>(__args)...);
[move_group-4]         592:       }
[move_group-4]         593:
[move_group-4]         594: #if __cpp_rtti
[move_group-4] #3    Object "/home/marcel3245/ros2_ws/build/moveit_task_constructor_capabilities/libmoveit_task_constructor_capabilities.so", at 0x736fea4ffd1f, in std::_Function_handler<std::unique_ptr<std::__future_base::_Result_base, std::__future_base::_Result_base::_Deleter> (), std::__future_base::_Task_setter<std::unique_ptr<std::__future_base::_Result<void>, std::__future_base::_Result_base::_Deleter>, std::thread::_Invoker<std::tuple<void (move_group::ExecuteTaskSolutionCapability::*)(std::shared_ptr<rclcpp_action::ServerGoalHandle<moveit_task_constructor_msgs::action::ExecuteTaskSolution> > const&), move_group::ExecuteTaskSolutionCapability*, std::shared_ptr<rclcpp_action::ServerGoalHandle<moveit_task_constructor_msgs::action::ExecuteTaskSolution> > > >, void> >::_M_invoke(std::_Any_data const&)
[move_group-4] #2    Object "/home/marcel3245/ros2_ws/build/moveit_task_constructor_capabilities/libmoveit_task_constructor_capabilities.so", at 0x736fea4ff29e, in move_group::ExecuteTaskSolutionCapability::execCallback(std::shared_ptr<rclcpp_action::ServerGoalHandle<moveit_task_constructor_msgs::action::ExecuteTaskSolution> > const&)
[move_group-4] #1    Source "/usr/src/ros-jazzy-moveit-ros-planning-2.12.3-1noble.20250520.213359/plan_execution/src/plan_execution.cpp", line 475, in executeAndMonitor [0x73701fea403c]
[move_group-4] #0  | Source "/usr/src/ros-jazzy-moveit-core-2.12.3-1noble.20250520.202002/robot_state/src/robot_state.cpp", line 1226, in begin
[move_group-4]     | Source "/usr/include/c++/13/bits/stl_map.h", line 378, in begin
[move_group-4]     |   376:       const_iterator
[move_group-4]     |   377:       begin() const _GLIBCXX_NOEXCEPT
[move_group-4]     | > 378:       { return _M_t.begin(); }
[move_group-4]     |   379:
[move_group-4]     |   380:       /**
[move_group-4]       Source "/usr/include/c++/13/bits/stl_tree.h", line 1002, in getAttachedBodies [0x73701fbc474b]
[move_group-4]        1000:       const_iterator
[move_group-4]        1001:       begin() const _GLIBCXX_NOEXCEPT
[move_group-4]       >1002:       { return const_iterator(this->_M_impl._M_header._M_left); }
[move_group-4]        1003:
[move_group-4]        1004:       iterator
[move_group-4]        1005:       end() _GLIBCXX_NOEXCEPT
[move_group-4] Segmentation fault (Address not mapped to object [0xe8])
[ros2_control_node-5] [INFO] [1748871804.512629200] [panda_arm_controller]: Goal reached, success!
[ERROR] [move_group-4]: process has died [pid 4561, exit code -11, cmd '/opt/ros/jazzy/lib/moveit_ros_move_group/move_group --ros-args --params-file /tmp/launch_params__cipn_ii --params-file /tmp/launch_params_4fpp30ts'].
[ros2_control_node-5] [WARN] [1748872032.820191852] [controller_manager]: Overrun detected! The controller manager missed its desired rate of 100 Hz. The loop took 12.975886 ms (missed cycles : 2).