diff --git a/tesseract_task_composer/planning/src/nodes/continuous_contact_check_task.cpp b/tesseract_task_composer/planning/src/nodes/continuous_contact_check_task.cpp index af895391bb..3f83536e0c 100644 --- a/tesseract_task_composer/planning/src/nodes/continuous_contact_check_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/continuous_contact_check_task.cpp @@ -160,7 +160,18 @@ ContinuousContactCheckTask::runImpl(TaskComposerContext& context, OptionalTaskCo if (contactCheckProgram(contacts, *manager, *state_solver, ci, cur_composite_profile->config)) { info->status_code = 0; - info->status_message = "Results are not contact free for process input: " + ci.getDescription(); + std::stringstream ss; + ss << "Results are not contact free for process input: " << ci.getDescription() << "\n"; + // Iterate over each contact and add collision summary if not empty + for (std::size_t i = 0; i < contacts.size(); ++i) + { + const auto& contact_map = contacts[i]; + if (!contact_map.empty()) + { + ss << contact_map.getSummary() << "\n"; + } + } + info->status_message = ss.str(); CONSOLE_BRIDGE_logInform("%s", info->status_message.c_str()); // Save space diff --git a/tesseract_task_composer/planning/src/nodes/discrete_contact_check_task.cpp b/tesseract_task_composer/planning/src/nodes/discrete_contact_check_task.cpp index eb259369f8..bfa5457224 100644 --- a/tesseract_task_composer/planning/src/nodes/discrete_contact_check_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/discrete_contact_check_task.cpp @@ -156,7 +156,18 @@ std::unique_ptr DiscreteContactCheckTask::runImpl(TaskComp std::vector contacts; if (contactCheckProgram(contacts, *manager, *state_solver, ci, cur_composite_profile->config)) { - info->status_message = "Results are not contact free for process input: " + ci.getDescription(); + std::stringstream ss; + ss << "Results are not contact free for process input: " << ci.getDescription() << "\n"; + // Iterate over each contact and add collision summary if not empty + for (std::size_t i = 0; i < contacts.size(); ++i) + { + const auto& contact_map = contacts[i]; + if (!contact_map.empty()) + { + ss << contact_map.getSummary() << "\n"; + } + } + info->status_message = ss.str(); CONSOLE_BRIDGE_logInform("%s", info->status_message.c_str()); // Save space