MTC tutorial not working on main
sjahr opened this issue ยท 9 comments
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
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
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
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?
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
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
... and likely same for other launch files
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'
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.
Just set up a fresh build on a recently updated rolling -- both work for me with @Abishalini's #832 suggestions.