void printFatalErrorMessage(FILE *fp, const std::exception &ex)
{
const char *title = "Unknown exception";
bool bPrintType = false;
const GromacsException *gmxEx = dynamic_cast<const GromacsException *>(&ex);
// TODO: Treat more of the standard exceptions
if (gmxEx != NULL)
{
title = getErrorCodeString(gmxEx->errorCode());
}
else if (dynamic_cast<const tMPI::system_error *>(&ex) != NULL)
{
title = "System error in thread synchronization";
}
else if (dynamic_cast<const std::bad_alloc *>(&ex) != NULL)
{
title = "Memory allocation failed";
}
else if (dynamic_cast<const std::logic_error *>(&ex) != NULL)
{
title = "Standard library logic error (bug)";
bPrintType = true;
}
else if (dynamic_cast<const std::runtime_error *>(&ex) != NULL)
{
title = "Standard library runtime error (possible bug)";
bPrintType = true;
}
else
{
bPrintType = true;
}
// We can't call get_error_info directly on ex since our internal boost
// needs to be compiled with BOOST_NO_RTTI. So we do the dynamic_cast
// here instead.
const char *const *funcPtr = NULL;
const char *const *filePtr = NULL;
const int *linePtr = NULL;
const boost::exception *boostEx = dynamic_cast<const boost::exception *>(&ex);
if (boostEx != NULL)
{
funcPtr = boost::get_error_info<boost::throw_function>(*boostEx);
filePtr = boost::get_error_info<boost::throw_file>(*boostEx);
linePtr = boost::get_error_info<boost::throw_line>(*boostEx);
}
internal::printFatalErrorHeader(fp, title,
funcPtr != NULL ? *funcPtr : NULL,
filePtr != NULL ? *filePtr : NULL,
linePtr != NULL ? *linePtr : 0);
if (bPrintType)
{
std::fprintf(fp, "(exception type: %s)\n", typeid(ex).name());
}
MessageWriterFileNoThrow writer(fp);
formatExceptionMessageInternal(&writer, ex, 0);
internal::printFatalErrorFooter(fp);
}
std::ostream& operator<<(std::ostream& os, const HTTPException& ex) {
os << "what=\"" << ex.what()
<< "\", direction=" << static_cast<int>(ex.getDirection())
<< ", proxygenError=" << getErrorString(ex.getProxygenError())
<< ", codecStatusCode=" << (ex.hasCodecStatusCode() ?
getErrorCodeString(ex.getCodecStatusCode()) :
"-1")
<< ", httpStatusCode=" << ex.getHttpStatusCode();
return os;
}
void HTTPCodecPrinter::onGoaway(uint64_t lastGoodStream, ErrorCode code) {
std::cout << "GOAWAY: lastGoodStream=" << lastGoodStream
<< ", error=" << getErrorCodeString(code) << std::endl;
callback_->onGoaway(lastGoodStream, code);
}
void HTTPCodecPrinter::onAbort(StreamID stream, ErrorCode code) {
std::cout << "RST_STREAM: stream_id=" << stream << ", error="
<< getErrorCodeString(code) << std::endl;
callback_->onAbort(stream, code);
}
void plan_execution::PlanExecution::planAndExecuteHelper(ExecutableMotionPlan &plan, const Options &opt)
{
// perform initial configuration steps & various checks
preempt_requested_ = false;
// run the actual motion plan & execution
unsigned int max_replan_attempts = opt.replan_ ? (opt.replan_attempts_ > 0 ? opt.replan_attempts_ : default_max_replan_attempts_) : 1;
unsigned int replan_attempts = 0;
bool previously_solved = false;
// run a planning loop for at most the maximum replanning attempts;
// re-planning is executed only in case of known types of failures (e.g., environment changed)
do
{
replan_attempts++;
ROS_INFO("Planning attempt %u of at most %u", replan_attempts, max_replan_attempts);
if (opt.before_plan_callback_)
opt.before_plan_callback_();
new_scene_update_ = false; // we clear any scene updates to be evaluated because we are about to compute a new plan, which should consider most recent updates already
// if we never had a solved plan, or there is no specified way of fixing plans, just call the planner; otherwise, try to repair the plan we previously had;
bool solved = (!previously_solved || !opt.repair_plan_callback_) ?
opt.plan_callback_(plan) :
opt.repair_plan_callback_(plan, trajectory_execution_manager_->getCurrentExpectedTrajectoryIndex());
if (preempt_requested_)
break;
// if planning fails in a manner that is not recoverable, we exit the loop,
// otherwise, we attempt to continue, if replanning attempts are left
if (plan.error_code_.val == moveit_msgs::MoveItErrorCodes::PLANNING_FAILED ||
plan.error_code_.val == moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN ||
plan.error_code_.val == moveit_msgs::MoveItErrorCodes::UNABLE_TO_AQUIRE_SENSOR_DATA)
{
if (plan.error_code_.val == moveit_msgs::MoveItErrorCodes::UNABLE_TO_AQUIRE_SENSOR_DATA && opt.replan_delay_ > 0.0)
{
ros::WallDuration d(opt.replan_delay_);
d.sleep();
}
continue;
}
// abort if no plan was found
if (solved)
previously_solved = true;
else
break;
if (plan.error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
{
if (opt.before_execution_callback_)
opt.before_execution_callback_();
if (preempt_requested_)
break;
// execute the trajectory, and monitor its executionm
plan.error_code_ = executeAndMonitor(plan);
}
// if we are done, then we exit the loop
if (plan.error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
break;
// if execution failed in a manner that we do not consider recoverable, we exit the loop (with failure)
if (plan.error_code_.val != moveit_msgs::MoveItErrorCodes::MOTION_PLAN_INVALIDATED_BY_ENVIRONMENT_CHANGE)
break;
else
{
// othewrise, we wait (if needed)
if (opt.replan_delay_ > 0.0)
{
ROS_INFO("Waiting for a %lf seconds before attempting a new plan ...", opt.replan_delay_);
ros::WallDuration d(opt.replan_delay_);
d.sleep();
ROS_INFO("Done waiting");
}
}
} while (!preempt_requested_ && max_replan_attempts > replan_attempts);
if (preempt_requested_)
{
ROS_DEBUG("PlanExecution was preempted");
plan.error_code_.val = moveit_msgs::MoveItErrorCodes::PREEMPTED;
}
if (opt.done_callback_)
opt.done_callback_();
if (plan.error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
ROS_DEBUG("PlanExecution finished successfully.");
else
ROS_DEBUG("PlanExecution terminating with error code %d - '%s'", plan.error_code_.val,
getErrorCodeString(plan.error_code_).c_str());
}
