From 833cf6777ce9208c0414c735c34944f555cc37b1 Mon Sep 17 00:00:00 2001 From: Tyler Marr Date: Fri, 13 Dec 2024 14:43:51 -0600 Subject: [PATCH] Better OMPL error logging and fail on start/goal in collision --- .../src/profile/ompl_default_plan_profile.cpp | 32 ++++++++++++++----- 1 file changed, 24 insertions(+), 8 deletions(-) diff --git a/tesseract_motion_planners/ompl/src/profile/ompl_default_plan_profile.cpp b/tesseract_motion_planners/ompl/src/profile/ompl_default_plan_profile.cpp index 9002d1b27c..945a214292 100644 --- a/tesseract_motion_planners/ompl/src/profile/ompl_default_plan_profile.cpp +++ b/tesseract_motion_planners/ompl/src/profile/ompl_default_plan_profile.cpp @@ -474,6 +474,7 @@ void OMPLDefaultPlanProfile::applyGoalStates(OMPLProblem& prob, else { CONSOLE_BRIDGE_logDebug("In OMPLDefaultPlanProfile: Goal state has invalid bounds"); + throw std::runtime_error("In OMPLDefaultPlanProfile: Goal state has invalid bounds"); } // Get discrete contact manager for testing provided start and end position @@ -482,12 +483,19 @@ void OMPLDefaultPlanProfile::applyGoalStates(OMPLProblem& prob, tesseract_collision::ContactResultMap contact_map; if (checkStateInCollision(prob, solution, contact_map)) { - CONSOLE_BRIDGE_logError("In OMPLDefaultPlanProfile: Goal state is in collision"); + std::stringstream contact_msg; + contact_msg << "In OMPLPlannerFreespaceConfig: Goal state ("; + contact_msg << solution.transpose().matrix() << ") is in collision. "; + for (const auto& contact_vec : contact_map) for (const auto& contact : contact_vec.second) - CONSOLE_BRIDGE_logError(("Links: " + contact.link_names[0] + ", " + contact.link_names[1] + - " Distance: " + std::to_string(contact.distance)) - .c_str()); + { + contact_msg << "Links: " << contact.link_names[0] << ", " << contact.link_names[1] << + " Distance: " << std::to_string(contact.distance); + CONSOLE_BRIDGE_logError(contact_msg.str().c_str()); + } + + throw std::runtime_error(contact_msg.str()); } ompl::base::ScopedState<> goal_state(prob.simple_setup->getStateSpace()); @@ -611,6 +619,7 @@ void OMPLDefaultPlanProfile::applyStartStates(OMPLProblem& prob, else { CONSOLE_BRIDGE_logDebug("In OMPLDefaultPlanProfile: Start state is outside limits"); + throw std::runtime_error("In OMPLDefaultPlanProfile: Start state is outside limits"); } // Get discrete contact manager for testing provided start and end position @@ -619,12 +628,19 @@ void OMPLDefaultPlanProfile::applyStartStates(OMPLProblem& prob, tesseract_collision::ContactResultMap contact_map; if (checkStateInCollision(prob, joint_waypoint, contact_map)) { - CONSOLE_BRIDGE_logError("In OMPLPlannerFreespaceConfig: Start state is in collision"); + std::stringstream contact_msg; + contact_msg << "In OMPLPlannerFreespaceConfig: Start state ("; + contact_msg << solution.transpose().matrix() << ") is in collision. "; + for (const auto& contact_vec : contact_map) for (const auto& contact : contact_vec.second) - CONSOLE_BRIDGE_logError(("Links: " + contact.link_names[0] + ", " + contact.link_names[1] + - " Distance: " + std::to_string(contact.distance)) - .c_str()); + { + contact_msg << "Links: " << contact.link_names[0] << ", " << contact.link_names[1] << + " Distance: " << std::to_string(contact.distance); + CONSOLE_BRIDGE_logError(contact_msg.str().c_str()); + } + + throw std::runtime_error(contact_msg.str()); } ompl::base::ScopedState<> start_state(prob.simple_setup->getStateSpace());