'Use Cartesian Path' crashing RViz

[BP-95]

The 'Use Cartesian Path' option in the MotionPlanning plugin crashes RViz

When going through the MoveIt Quickstart in RViz tutorial I followed all of the steps in order. I am able to get the arm to move when not selecting 'Use Cartesian Path', but when that option is selected I get an error which I will include below and RViz becomes unresponsive. It is at this point in the tutorial that I get the error. I'll also add that the Show Trail checkbox does nothing when I select it with a path planned. It does not show the arm's path in the viewer. I'm not sure if that's related but the original issue is that 'Use Cartesian Path' is giving an error and crashing RViz.

Your environment

• ROS Distro: Iron
• OS Version: Ubuntu 22.04
• This binary release version. I followed the instructions to install Iron exactly as those instructions say.

Steps to reproduce

At this point in the tutorial I get the issue. I move the goal state to some desired location. Press 'Plan' in the MotionPlanning plugin and immediately get the error pasted below. I am able to both Plan and Execute a motion if 'Use Cartesian Path' is not selected.

Expected behaviour

The arm in the world should move from Start State to Goal State in a straight path in Cartesian space. It also should not crash my RViz application preventing me from making further motions using MotionPlanner.

Backtrace or Console output

[move_group-4] [INFO] [1705601002.786576436] [move_group.moveit.cartesian_path_service_capability]: Received request to compute Cartesian path
[move_group-4] [INFO] [1705601002.786843374] [move_group.moveit.cartesian_path_service_capability]: Attempting to follow 1 waypoints for link 'end_effector_link' using a step of 0.010000 m and jump threshold 0.000000 (in global reference frame)
[move_group-4] terminate called after throwing an instance of 'std::invalid_argument'
[move_group-4]   what():  invalid argument passed
[move_group-4] Stack trace (most recent call last) in thread 63337:
[move_group-4] #18   Object "", at 0xffffffffffffffff, in 
[move_group-4] #17   Source "../sysdeps/unix/sysv/linux/x86_64/clone3.S", line 81, in __clone3 [0x7f4fa232684f]
[move_group-4] #16   Source "./nptl/pthread_create.c", line 442, in start_thread [0x7f4fa2294ac2]
[move_group-4] #15   Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x7f4fa26dc252, in 
[move_group-4] #14   Object "/opt/ros/iron/lib/librclcpp.so", at 0x7f4fa2b0e9d9, in rclcpp::executors::MultiThreadedExecutor::run(unsigned long)
[move_group-4] #13   Object "/opt/ros/iron/lib/librclcpp.so", at 0x7f4fa2aff125, in rclcpp::Executor::execute_any_executable(rclcpp::AnyExecutable&)
[move_group-4] #12   Object "/opt/ros/iron/lib/librclcpp.so", at 0x7f4fa2b056dc, in rclcpp::Executor::execute_service(std::shared_ptr<rclcpp::ServiceBase>)
[move_group-4] #11   Object "/opt/ros/iron/lib/librclcpp.so", at 0x7f4fa2bcd579, in 
[move_group-4] #10   Object "/home/buck/ws_moveit/install/moveit_ros_move_group/lib/libmoveit_move_group_default_capabilities.so.2.9.0", at 0x7f4f90842a59, in rclcpp::Service<moveit_msgs::srv::GetCartesianPath>::handle_request(std::shared_ptr<rmw_request_id_s>, std::shared_ptr<void>)
[move_group-4] #9    Object "/home/buck/ws_moveit/install/moveit_ros_move_group/lib/libmoveit_move_group_default_capabilities.so.2.9.0", at 0x7f4f908423e5, in rclcpp::AnyServiceCallback<moveit_msgs::srv::GetCartesianPath>::dispatch(std::shared_ptr<rclcpp::Service<moveit_msgs::srv::GetCartesianPath> > const&, std::shared_ptr<rmw_request_id_s> const&, std::shared_ptr<moveit_msgs::srv::GetCartesianPath_Request_<std::allocator<void> > >)
[move_group-4] #8    Object "/home/buck/ws_moveit/install/moveit_ros_move_group/lib/libmoveit_move_group_default_capabilities.so.2.9.0", at 0x7f4f9082cb41, in move_group::MoveGroupCartesianPathService::computeService(std::shared_ptr<rmw_request_id_s> const&, std::shared_ptr<moveit_msgs::srv::GetCartesianPath_Request_<std::allocator<void> > > const&, std::shared_ptr<moveit_msgs::srv::GetCartesianPath_Response_<std::allocator<void> > > const&)
[move_group-4] #7    Object "/home/buck/ws_moveit/install/moveit_core/lib/libmoveit_robot_state.so.2.9.0", at 0x7f4fa2d42921, in moveit::core::JumpThreshold::relative(double) [clone .cold]
[move_group-4] #6    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x7f4fa26ae4d7, in __cxa_throw
[move_group-4] #5    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x7f4fa26ae276, in std::terminate()
[move_group-4] #4    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x7f4fa26ae20b, in 
[move_group-4] #3    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x7f4fa26a2b9d, in 
[move_group-4] #2    Source "./stdlib/abort.c", line 79, in abort [0x7f4fa22287f2]
[move_group-4] #1    Source "../sysdeps/posix/raise.c", line 26, in raise [0x7f4fa2242475]
[move_group-4] #0  | Source "./nptl/pthread_kill.c", line 89, in __pthread_kill_internal
[move_group-4]     | Source "./nptl/pthread_kill.c", line 78, in __pthread_kill_implementation
[move_group-4]       Source "./nptl/pthread_kill.c", line 44, in __pthread_kill [0x7f4fa22969fc]
[move_group-4] Aborted (Signal sent by tkill() 62832 1000)
[ERROR] [move_group-4]: process has died [pid 62832, exit code -6, cmd '/home/buck/ws_moveit/install/moveit_ros_move_group/lib/moveit_ros_move_group/move_group --ros-args --params-file /tmp/launch_params_ewmf8g4f'].

Use gist.github.com to copy-paste the console output or segfault backtrace using gdb.

[GarmischWg]

I am having the exact same problem.
I tested the that without using cartesian path, the planning can be done properly, but fails once the use cartesian path option is checked.

Tested with Ubuntu, ROS2 Humble, moveit2 with a custom robot arm.

[move_group-3] [INFO] [1705745748.146743477] [move_group.moveit.MoveGroupMoveAction]: Solution was found and executed.
[rviz2-4] [INFO] [1705745748.147269596] [moveit_1380153168.move_group_interface]: Plan and Execute request complete!
[rviz2-4] [WARN] [1705745749.148093912] [moveit_1380153168.planning_scene_monitor]: Maybe failed to update robot state, time diff: 0.022s
[move_group-3] [INFO] [1705745881.816698724] [move_group.moveit.cartesian_path_service_capability]: Received request to compute Cartesian path
[move_group-3] [INFO] [1705745881.816842509] [move_group.moveit.cartesian_path_service_capability]: Attempting to follow 1 waypoints for link 'link05' using a step of 0.010000 m and jump threshold 0.000000 (in global reference frame)
[move_group-3] terminate called after throwing an instance of 'std::invalid_argument'
[move_group-3]   what():  invalid argument passed
[move_group-3] Stack trace (most recent call last) in thread 143384:
[move_group-3] #18   Object "", at 0xffffffffffffffff, in 
[move_group-3] #17   Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7f51f8a3c84f, in 
[move_group-3] #16   Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7f51f89aaac2, in 
[move_group-3] #15   Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x7f51f8c3d252, in 
[move_group-3] #14   Object "/opt/ros/humble/lib/librclcpp.so", at 0x7f51f8ead279, in rclcpp::executors::MultiThreadedExecutor::run(unsigned long)
[move_group-3] #13   Object "/opt/ros/humble/lib/librclcpp.so", at 0x7f51f8ea5ff5, in rclcpp::Executor::execute_any_executable(rclcpp::AnyExecutable&)
[move_group-3] #12   Object "/opt/ros/humble/lib/librclcpp.so", at 0x7f51f8ea5c89, in rclcpp::Executor::execute_service(std::shared_ptr<rclcpp::ServiceBase>)
[move_group-3] #11   Object "/opt/ros/humble/lib/librclcpp.so", at 0x7f51f8ea82a5, in 
[move_group-3] #10   Object "/home/garmisch/RobotArmProject-MoveIt/install/moveit_ros_move_group/lib/libmoveit_move_group_default_capabilities.so.2.9.0", at 0x7f51c4650471, in rclcpp::Service<moveit_msgs::srv::GetCartesianPath>::handle_request(std::shared_ptr<rmw_request_id_s>, std::shared_ptr<void>)
[move_group-3] #9    Object "/home/garmisch/RobotArmProject-MoveIt/install/moveit_ros_move_group/lib/libmoveit_move_group_default_capabilities.so.2.9.0", at 0x7f51c464fdf5, in rclcpp::AnyServiceCallback<moveit_msgs::srv::GetCartesianPath>::dispatch(std::shared_ptr<rclcpp::Service<moveit_msgs::srv::GetCartesianPath> > const&, std::shared_ptr<rmw_request_id_s> const&, std::shared_ptr<moveit_msgs::srv::GetCartesianPath_Request_<std::allocator<void> > >)
[move_group-3] #8    Object "/home/garmisch/RobotArmProject-MoveIt/install/moveit_ros_move_group/lib/libmoveit_move_group_default_capabilities.so.2.9.0", at 0x7f51c46367c3, in move_group::MoveGroupCartesianPathService::computeService(std::shared_ptr<rmw_request_id_s> const&, std::shared_ptr<moveit_msgs::srv::GetCartesianPath_Request_<std::allocator<void> > > const&, std::shared_ptr<moveit_msgs::srv::GetCartesianPath_Response_<std::allocator<void> > > const&)
[move_group-3] #7    Object "/home/garmisch/RobotArmProject-MoveIt/install/moveit_core/lib/libmoveit_robot_state.so.2.9.0", at 0x7f51f880c82d, in moveit::core::JumpThreshold::relative(double) [clone .cold]
[move_group-3] #6    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x7f51f8c0f4d7, in __cxa_throw
[move_group-3] #5    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x7f51f8c0f276, in std::terminate()
[move_group-3] #4    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x7f51f8c0f20b, in 
[move_group-3] #3    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x7f51f8c03b9d, in 
[move_group-3] #2    Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7f51f893e7f2, in abort
[move_group-3] #1    Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7f51f8958475, in raise
[move_group-3] #0    Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7f51f89ac9fc, in pthread_kill
[move_group-3] Aborted (Signal sent by tkill() 143180 1000)

[GarmischWg]

I found a temporary solution, in Rviz, go to the context tab,
and configure the planner like below.

LIN stands for linear path, i think it is the same as cartesian path.
PTP stands for point to point trajectory planning.

[marioprats]

This is an issue introduced after a change to the JumpThreshold API. Working on a fix.