diff --git a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp index 2a93fe0c71..964c4d269d 100644 --- a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp +++ b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp @@ -794,7 +794,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl if (code != rclcpp_action::ResultCode::SUCCEEDED) { RCLCPP_ERROR_STREAM(LOGGER, "MoveGroupInterface::plan() failed or timeout reached"); - return false; + return res->error_code; } plan.trajectory_ = res->planned_trajectory; @@ -873,7 +873,6 @@ class MoveGroupInterface::MoveGroupInterfaceImpl if (code != rclcpp_action::ResultCode::SUCCEEDED) { RCLCPP_ERROR_STREAM(LOGGER, "MoveGroupInterface::move() failed or timeout reached"); - return false; } return res->error_code; } @@ -941,7 +940,6 @@ class MoveGroupInterface::MoveGroupInterfaceImpl if (code != rclcpp_action::ResultCode::SUCCEEDED) { RCLCPP_ERROR_STREAM(LOGGER, "MoveGroupInterface::execute() failed or timeout reached"); - return false; } return res->error_code; }