MTC tutorial not working on main

[sjahr]

Description

When I try to run the MTC demos it fails. Error seems to be related to an rclcpp bug 🙈

Your environment

• ROS Distro: [Rolling]
• OS Version: Ubuntu 22.04
• Source or Binary build? Source
• If source, which git commit or tag? main 71ee840

Steps to reproduce

Terminal 1

ros2 launch moveit_task_constructor_demo demo.launch.py

ros2 run moveit_task_constructor_demo cartesian

Expected behaviour

Demo starts

Backtrace or Console output

^^/ws_moveit_tutorials >>> ros2 run moveit_task_constructor_demo cartesian                                                                                                                            (250) 12:04:36
^C[INFO] [1701342365.008491532] [rclcpp]: signal_handler(signum=2)
^C[INFO] [1701342365.848192500] [rclcpp]: signal_handler(signum=2)
^C[INFO] [1701342366.056476257] [rclcpp]: signal_handler(signum=2)
^C[INFO] [1701342366.665401331] [rclcpp]: signal_handler(signum=2)
[ERROR] [1701342374.386122241] [mtc_tutorial]: Could not find parameter robot_description and did not receive robot_description via std_msgs::msg::String subscription within 10.000000 seconds.
terminate called after throwing an instance of 'rclcpp::exceptions::RCLError'
  what():  failed to create guard condition: the given context is not valid, either rcl_init() was not called or rcl_shutdown() was called., at ./src/rcl/guard_condition.c:67
Stack trace (most recent call last):
#23   Object "", at 0xffffffffffffffff, in
#22   Object "/home/sebastian/ws_moveit_tutorials/install/moveit_task_constructor_demo/lib/moveit_task_constructor_demo/cartesian", at 0x564135063904, in _start
#21   Source "../csu/libc-start.c", line 392, in __libc_start_main_impl [0x7f273bc29e3f]
#20   Source "../sysdeps/nptl/libc_start_call_main.h", line 58, in __libc_start_call_main [0x7f273bc29d8f]
#19   Object "/home/sebastian/ws_moveit_tutorials/install/moveit_task_constructor_demo/lib/moveit_task_constructor_demo/cartesian", at 0x5641350635c4, in main
#18   Object "/home/sebastian/ws_moveit_tutorials/install/moveit_task_constructor_demo/lib/moveit_task_constructor_demo/cartesian", at 0x5641350641d3, in createTask(std::shared_ptr<rclcpp::Node> const&)
#17   Object "/home/sebastian/ws_moveit_tutorials/install/moveit_task_constructor_core/lib/libmoveit_task_constructor_core.so", at 0x7f273caa2fce, in moveit::task_constructor::Task::loadRobotModel(std::shared_ptr<rclcpp::Node> const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)
#16   Object "/home/sebastian/ws_moveit_tutorials/install/moveit_ros_planning/lib/libmoveit_robot_model_loader.so.2.8.0", at 0x7f273c695205, in robot_model_loader::RobotModelLoader::RobotModelLoader(std::shared_ptr<rclcpp::Node> const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool)
#15   Object "/home/sebastian/ws_moveit_tutorials/install/moveit_ros_planning/lib/libmoveit_robot_model_loader.so.2.8.0", at 0x7f273c69206b, in robot_model_loader::RobotModelLoader::configure(robot_model_loader::RobotModelLoader::Options const&)
#14   Object "/home/sebastian/ws_moveit_tutorials/install/moveit_ros_planning/lib/libmoveit_rdf_loader.so.2.8.0", at 0x7f273b6a51b3, in rdf_loader::RDFLoader::RDFLoader(std::shared_ptr<rclcpp::Node> const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool, double)
#13   Object "/home/sebastian/ws_moveit_tutorials/install/moveit_ros_planning/lib/libmoveit_rdf_loader.so.2.8.0", at 0x7f273b6adc3f, in rdf_loader::SynchronizedStringParameter::loadInitialValue(std::shared_ptr<rclcpp::Node> const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::function<void (std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)> const&, bool, double)
#12   Object "/home/sebastian/ws_moveit_tutorials/install/moveit_ros_planning/lib/libmoveit_rdf_loader.so.2.8.0", at 0x7f273b6ac4fc, in rdf_loader::SynchronizedStringParameter::waitForMessage(rclcpp::Duration const&)
#11   Object "/opt/ros/rolling/lib/librclcpp.so", at 0x7f273c532487, in rclcpp::Node::Node(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, rclcpp::NodeOptions const&)
#10   Object "/opt/ros/rolling/lib/librclcpp.so", at 0x7f273c531017, in rclcpp::Node::Node(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, rclcpp::NodeOptions const&)
#9    Object "/opt/ros/rolling/lib/librclcpp.so", at 0x7f273c530951, in rclcpp::node_interfaces::NodeBase::NodeBase(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::shared_ptr<rclcpp::Context>, rcl_node_options_s const&, bool, bool, std::shared_ptr<rclcpp::CallbackGroup>)
#8    Object "/opt/ros/rolling/lib/librclcpp.so", at 0x7f273c527f6d, in rclcpp::GuardCondition::GuardCondition(std::shared_ptr<rclcpp::Context>, rcl_guard_condition_options_s)
#7    Object "/opt/ros/rolling/lib/librclcpp.so", at 0x7f273c4fe6d8, in rclcpp::exceptions::throw_from_rcl_error(int, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, rcutils_error_state_s const*, void (*)())
#6    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x7f273c0ae1fd, in std::rethrow_exception(std::__exception_ptr::exception_ptr)
#5    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x7f273c0ae276, in std::terminate()
#4    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x7f273c0ae20b, in
#3    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x7f273c0a2b9d, in
#2    Source "./stdlib/abort.c", line 79, in abort [0x7f273bc287f2]
#1    Source "../sysdeps/posix/raise.c", line 26, in raise [0x7f273bc42475]
#0  | Source "./nptl/pthread_kill.c", line 89, in __pthread_kill_internal
    | Source "./nptl/pthread_kill.c", line 78, in __pthread_kill_implementation
      Source "./nptl/pthread_kill.c", line 44, in __pthread_kill [0x7f273bc969fc]
Aborted (Signal sent by tkill() 185638 1000)
[ros2run]: Aborted

[moriarty]

I believe this is a duplicate of:
ros2/rclcpp#2163

I will to reproduce and test with this open PR ros2/rclcpp#2142 and see if that fixes it

[Abishalini]

I am seeing a different error when I run the same

abishalini@abishalini:~/ws_moveit2$ ros2 run moveit_task_constructor_demo cartesian
[ERROR] [1701724929.798188400] [mtc_tutorial]: Could not find parameter robot_description_semantic and did not receive robot_description_semantic via std_msgs::msg::String subscription within 10.000000 seconds.
Error:   Could not parse the SRDF XML File. Error=XML_ERROR_EMPTY_DOCUMENT ErrorID=13 (0xd) Line number=0
         at line 715 in ./src/model.cpp
[ERROR] [1701724929.801930529] [moveit_3110922876.rdf_loader]: Unable to parse SRDF
[ERROR] [1701724929.802134735] [moveit_task_constructor.task]: Cartesian Path: received invalid robot model
[ERROR] [1701724929.802148746] [moveit_exceptions.exceptions]: Task failed to construct RobotModel
Exception thrown.
terminate called after throwing an instance of 'moveit::Exception'
  what():  Task failed to construct RobotModel
Stack trace (most recent call last):
#13   Object "", at 0xffffffffffffffff, in 
#12   Object "/home/abishalini/ws_moveit2/install/moveit_task_constructor_demo/lib/moveit_task_constructor_demo/cartesian", at 0x55791a5238f4, in _start
#11   Source "../csu/libc-start.c", line 392, in __libc_start_main_impl [0x7fe7d6e29e3f]
#10   Source "../sysdeps/nptl/libc_start_call_main.h", line 58, in __libc_start_call_main [0x7fe7d6e29d8f]
#9    Object "/home/abishalini/ws_moveit2/install/moveit_task_constructor_demo/lib/moveit_task_constructor_demo/cartesian", at 0x55791a5235b4, in main
#8    Object "/home/abishalini/ws_moveit2/install/moveit_task_constructor_demo/lib/moveit_task_constructor_demo/cartesian", at 0x55791a5241c3, in createTask(std::shared_ptr<rclcpp::Node> const&)
#7    Object "/home/abishalini/ws_moveit2/install/moveit_task_constructor_core/lib/libmoveit_task_constructor_core.so", at 0x7fe7d7a94e78, in moveit::task_constructor::Task::loadRobotModel(std::shared_ptr<rclcpp::Node> const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&) [clone .cold]
#6    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x7fe7d72ae4d7, in __cxa_throw
#5    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x7fe7d72ae276, in std::terminate()
#4    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x7fe7d72ae20b, in 
#3    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x7fe7d72a2b9d, in 
#2    Source "./stdlib/abort.c", line 79, in abort [0x7fe7d6e287f2]
#1    Source "../sysdeps/posix/raise.c", line 26, in raise [0x7fe7d6e42475]
#0  | Source "./nptl/pthread_kill.c", line 89, in __pthread_kill_internal
    | Source "./nptl/pthread_kill.c", line 78, in __pthread_kill_implementation
      Source "./nptl/pthread_kill.c", line 44, in __pthread_kill [0x7fe7d6e969fc]
Aborted (Signal sent by tkill() 3753511 1000)
[ros2run]: Aborted

[moriarty]

yes not a duplicate the error @sjahr posted was after he pressed Ctrl+C I assume he copied the wrong log lines.

@Abishalini are you seein that same error every time?

[Abishalini]

That was the error I saw consistently until I figured out I had to set publish_robot_description and publish_robot_description_semantic to True in demo.launch.py

    moveit_config = (
        MoveItConfigsBuilder("moveit_resources_panda")
        .planning_pipelines(pipelines=["ompl", "chomp", "pilz_industrial_motion_planner", "stomp"])
        .robot_description(file_path="config/panda.urdf.xacro")
        .robot_description_semantic(file_path="config/panda.srdf")
        .trajectory_execution(file_path="config/gripper_moveit_controllers.yaml")
        .planning_scene_monitor(
            publish_robot_description=True, publish_robot_description_semantic=True
        )
        .to_moveit_configs()
    )

Now I see this error consistently

abishalini@abishalini:~/ws_moveit2$ ros2 run moveit_task_constructor_demo cartesian
[INFO] [1701793271.883236217] [moveit_4036328846.rdf_loader]: Loaded robot model in 0.742049 seconds
[INFO] [1701793271.883347823] [moveit_robot_model.robot_model]: Loading robot model 'panda'...
[WARN] [1701793271.891255766] [moveit_4036328846.robot_model_loader]: No kinematics plugins defined. Fill and load kinematics.yaml!
[WARN] [1701793271.897002890] [Properties]: Unregistered property type: std::map<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, double, std::less<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >, std::allocator<std::pair<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const, double> > >
[ERROR] [1701793271.897379243] [moveit_robot_state.robot_state]: No kinematics solver instantiated for group 'panda_arm'
[ERROR] [1701793271.897444806] [moveit_trajectory_processing.time_optimal_trajectory_generation]: No acceleration limit was defined for joint panda_joint1! You have to define acceleration limits in the URDF or joint_limits.yaml
  0  - ←   0 →   -  0 / Cartesian Path
    1  - ←   1 →   -  0 / initial state
    -  0 →   0 →   -  0 / x +0.2
    -  0 →   0 →   -  0 / y -0.3
    -  0 →   0 →   -  0 / rz +45°
    -  0 →   0 →   -  0 / joint offset
    -  0 →   0 ←   1  - / connect
    1  - ←   1 →   -  1 / final state
Failing stage(s):
x +0.2 (0/1): failed to move full distance

[sea-bass]

That's a strange error, as the acceleration limits ARE in the config.
https://github.com/ros-planning/moveit_resources/blob/ros2/panda_moveit_config/config/joint_limits.yaml#L9

I think those launch files in the MTC repo need to also include

moveit_config.joint_limits

https://github.com/ros-planning/moveit_task_constructor/blob/ros2/demo/launch/cartesian.launch.py#L18-L20

... and likely same for other launch files

[Abishalini]

I made another mistake of running the cartesian executable instead of using the launch file. So the cartesian node was missing parameters causing the error [ERROR] [1701793271.897379243] [moveit_robot_state.robot_state]: No kinematics solver instantiated for group 'panda_arm'

[Abishalini]

@sjahr and @moriarty
I got this working for me after #832.
Let me know if you are still able to reproduce the segfault.

[Abishalini]

The Pick and Place demo is broken. Running ros2 launch moveit2_tutorials mtc_demo.launch.py and ros2 launch moveit_task_constructor_demo pickplace.launch.py produces the following segfault

It looks like it's coming from child loggers?

abishalini@abishalini:~/ws_moveit2$ ros2 launch moveit_task_constructor_demo pickplace.launch.py 
[INFO] [launch]: All log files can be found below /home/abishalini/.ros/log/2023-12-05-14-08-05-574440-abishalini-629405
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [pick_place_demo-1]: process started with pid [629408]
[pick_place_demo-1] [INFO] [1701810486.846157908] [moveit_task_constructor_demo]: Initializing task pipeline
[pick_place_demo-1] terminate called after throwing an instance of 'rclcpp::exceptions::RCLError'
[pick_place_demo-1]   what():  failed to call rcl_logging_rosout_add_sublogger: The entry of logger 'planning_scene_interface_94706073699856' not exist., at ./src/rcl/logging_rosout.c:450
[pick_place_demo-1] Stack trace (most recent call last):
[pick_place_demo-1] #17   Object "", at 0xffffffffffffffff, in 
[pick_place_demo-1] #16   Object "/home/abishalini/ws_moveit2/install/moveit_task_constructor_demo/lib/moveit_task_constructor_demo/pick_place_demo", at 0x5622783ae9e4, in _start
[pick_place_demo-1] #15   Source "../csu/libc-start.c", line 392, in __libc_start_main_impl [0x7f5a2b429e3f]
[pick_place_demo-1] #14   Source "../sysdeps/nptl/libc_start_call_main.h", line 58, in __libc_start_call_main [0x7f5a2b429d8f]
[pick_place_demo-1] #13   Object "/home/abishalini/ws_moveit2/install/moveit_task_constructor_demo/lib/moveit_task_constructor_demo/pick_place_demo", at 0x5622783ae16b, in main
[pick_place_demo-1] #12   Object "/home/abishalini/ws_moveit2/install/moveit_task_constructor_demo/lib/libmoveit_task_constructor_demo_pick_place_task.so", at 0x7f5a2bebdd4d, in moveit_task_constructor_demo::PickPlaceTask::init(std::shared_ptr<rclcpp::Node> const&, pick_place_task_demo::Params const&)
[pick_place_demo-1] #11   Object "/home/abishalini/ws_moveit2/install/moveit_task_constructor_core/lib/libmoveit_task_constructor_core.so", at 0x7f5a2b7a40ce, in moveit::task_constructor::Task::loadRobotModel(std::shared_ptr<rclcpp::Node> const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)
[pick_place_demo-1] #10   Object "/home/abishalini/ws_moveit2/install/moveit_ros_planning/lib/libmoveit_robot_model_loader.so.2.8.0", at 0x7f5a2ad53169, in robot_model_loader::RobotModelLoader::RobotModelLoader(std::shared_ptr<rclcpp::Node> const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool)
[pick_place_demo-1] #9    Object "/home/abishalini/ws_moveit2/install/moveit_core/lib/libmoveit_utils.so.2.8.0", at 0x7f5a2b0a1974, in moveit::makeChildLogger(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)
[pick_place_demo-1] #8    Object "/opt/ros/rolling/lib/librclcpp.so", at 0x7f5a2bd26afc, in rclcpp::Logger::get_child(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)
[pick_place_demo-1] #7    Object "/opt/ros/rolling/lib/librclcpp.so", at 0x7f5a2bcfd7b8, in rclcpp::exceptions::throw_from_rcl_error(int, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, rcutils_error_state_s const*, void (*)())
[pick_place_demo-1] #6    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x7f5a2b8ae1fd, in std::rethrow_exception(std::__exception_ptr::exception_ptr)
[pick_place_demo-1] #5    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x7f5a2b8ae276, in std::terminate()
[pick_place_demo-1] #4    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x7f5a2b8ae20b, in 
[pick_place_demo-1] #3    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x7f5a2b8a2b9d, in 
[pick_place_demo-1] #2    Source "./stdlib/abort.c", line 79, in abort [0x7f5a2b4287f2]
[pick_place_demo-1] #1    Source "../sysdeps/posix/raise.c", line 26, in raise [0x7f5a2b442475]
[pick_place_demo-1] #0  | Source "./nptl/pthread_kill.c", line 89, in __pthread_kill_internal
[pick_place_demo-1]     | Source "./nptl/pthread_kill.c", line 78, in __pthread_kill_implementation
[pick_place_demo-1]       Source "./nptl/pthread_kill.c", line 44, in __pthread_kill [0x7f5a2b4969fc]
[pick_place_demo-1] Aborted (Signal sent by tkill() 629408 1000)

EDIT - I rebuilt my workspace and no longer see this.

[sea-bass]

Just set up a fresh build on a recently updated rolling -- both work for me with @Abishalini's #832 suggestions.