footstep_planning_msgs.cpp
Go to the documentation of this file.
00001 #include <vigir_footstep_planning_msgs/footstep_planning_msgs.h>
00002 
00003 namespace vigir_footstep_planning
00004 {
00005 // Extension to ErrorStatus message
00006 msgs::ErrorStatus operator+(const msgs::ErrorStatus& lhs, const msgs::ErrorStatus& rhs)
00007 {
00008   msgs::ErrorStatus result;
00009 
00010   result.error = lhs.error | rhs.error;
00011   result.error_msg = lhs.error_msg;
00012   if (result.error_msg.size() && rhs.error_msg.size())
00013     result.error_msg += "\n";
00014   result.error_msg += rhs.error_msg;
00015 
00016   result.warning = lhs.warning | rhs.warning;
00017   result.warning_msg = lhs.warning_msg;
00018   if (result.warning_msg.size() && rhs.warning_msg.size())
00019     result.warning_msg += "\n";
00020   result.warning_msg += rhs.warning_msg;
00021 
00022   return result;
00023 }
00024 
00025 msgs::ErrorStatus operator+=(msgs::ErrorStatus& lhs, const msgs::ErrorStatus& rhs)
00026 {
00027   lhs = operator+(lhs, rhs);
00028   return lhs;
00029 }
00030 
00031 msgs::ErrorStatus isConsistent(const msgs::StepPlan& step_plan)
00032 {
00033   // an empty plan is always consistent
00034   if (step_plan.steps.empty())
00035     return msgs::ErrorStatus();
00036 
00037   msgs::ErrorStatus status;
00038 
00039   // check if steps are ordered and closed
00040   unsigned int step_index = step_plan.steps.front().step_index;
00041   for (std::vector<msgs::Step>::const_iterator itr = step_plan.steps.begin(); itr != step_plan.steps.end(); itr++)
00042   {
00043     if (itr->header.frame_id != step_plan.header.frame_id)
00044       status += ErrorStatusError(msgs::ErrorStatus::ERR_INCONSISTENT_STEP_PLAN, "isConsistent", "Frame id mismatch! Plan: " + step_plan.header.frame_id + " vs. step: " + itr->header.frame_id);
00045 
00046     if (itr->step_index != step_index++)
00047       status += ErrorStatusWarning(msgs::ErrorStatus::WARN_INCOMPLETE_STEP_PLAN, "isConsistent", "Wrong step index order: Expected " + boost::lexical_cast<std::string>(step_index-1) + " but got " + boost::lexical_cast<std::string>(itr->step_index) + "!");
00048 
00049     if (!itr->valid)
00050       status += ErrorStatusWarning(msgs::ErrorStatus::WARN_INVALID_STEP_PLAN, "isConsistent", "Step " + boost::lexical_cast<std::string>(itr->step_index) + " invalid!");
00051   }
00052 
00053   return status;
00054 }
00055 
00056 std::string ErrorStatusCodeToString(unsigned int error)
00057 {
00058   switch (error)
00059   {
00060     case msgs::ErrorStatus::NO_ERROR: return "NO_ERROR";
00061     case msgs::ErrorStatus::ERR_UNKNOWN: return "ERR_UNKNOWN";
00062     case msgs::ErrorStatus::ERR_NO_SOLUTION: return "ERR_NO_SOLUTION";
00063     case msgs::ErrorStatus::ERR_INVALID_START: return "ERR_INVALID_START";
00064     case msgs::ErrorStatus::ERR_INVALID_GOAL: return "ERR_INVALID_GOAL";
00065     case msgs::ErrorStatus::ERR_INVALID_GRID_MAP: return "ERR_INVALID_GRID_MAP";
00066     case msgs::ErrorStatus::ERR_INVALID_TERRAIN_MODEL: return "ERR_INVALID_TERRAIN_MODEL";
00067     case msgs::ErrorStatus::ERR_INVALID_STEP: return "ERR_INVALID_STEP";
00068     case msgs::ErrorStatus::ERR_INCONSISTENT_STEP_PLAN: return "ERR_INCONSISTENT_STEP_PLAN";
00069     case msgs::ErrorStatus::ERR_INVALID_PARAMETERS: return "ERR_INVALID_PARAMETERS";
00070     case msgs::ErrorStatus::ERR_NO_PLUGIN_AVAILABLE: return "ERR_NO_PLUGIN_AVAILABLE";
00071     case msgs::ErrorStatus::ERR_INCONSISTENT_REQUEST: return "ERR_INCONSISTENT_REQUEST";
00072     default: return "ERR_UNKNOWN";
00073   }
00074 }
00075 
00076 std::string WarningStatusCodeToString(unsigned int warning)
00077 {
00078   switch (warning)
00079   {
00080     case msgs::ErrorStatus::NO_WARNING: return "NO_WARNING";
00081     case msgs::ErrorStatus::WARN_UNKNOWN: return "WARN_UNKNOWN";
00082     case msgs::ErrorStatus::WARN_INCOMPLETE_STEP_PLAN: return "WARN_INCOMPLETE_STEP_PLAN";
00083     case msgs::ErrorStatus::WARN_INVALID_STEP_PLAN: return "WARN_INVALID_STEP_PLAN";
00084     default: return "WARN_UNKNOWN";
00085   }
00086 }
00087 
00088 msgs::ErrorStatus createErrorStatus(const std::string& context, unsigned int error, const std::string& error_msg, unsigned int warning, const std::string& warning_msg, bool output, double throttle_rate)
00089 {
00090   if (output)
00091   {
00092     if (throttle_rate > 0.0)
00093     {
00094       if (error)
00095         ROS_ERROR_THROTTLE(throttle_rate, "[%s][%s] %s", ErrorStatusCodeToString(error).c_str(), context.c_str(), error_msg.c_str());
00096       if (warning)
00097         ROS_WARN_THROTTLE(throttle_rate, "[%s][%s] %s", WarningStatusCodeToString(warning).c_str(), context.c_str(), warning_msg.c_str());
00098     }
00099     else
00100     {
00101       if (error)
00102         ROS_ERROR("[%s][%s] %s", ErrorStatusCodeToString(error).c_str(), context.c_str(), error_msg.c_str());
00103       if (warning)
00104         ROS_WARN("[%s][%s] %s", WarningStatusCodeToString(warning).c_str(), context.c_str(), warning_msg.c_str());
00105     }
00106   }
00107 
00108   msgs::ErrorStatus status;
00109   status.error = error;
00110   status.error_msg = error != msgs::ErrorStatus::NO_ERROR ? "[" + ErrorStatusCodeToString(error) + "][" + context + "] " + error_msg : error_msg;
00111   status.warning = warning;
00112   status.warning_msg = warning != msgs::ErrorStatus::NO_WARNING ? "[" + WarningStatusCodeToString(warning) + "][" + context + "] " + warning_msg : warning_msg;
00113   return status;
00114 }
00115 
00116 msgs::ErrorStatus ErrorStatusError(unsigned int error, const std::string& context, const std::string& error_msg, bool output, double throttle_rate)
00117 {
00118   return createErrorStatus(context, error, error_msg, msgs::ErrorStatus::NO_WARNING, "", output, throttle_rate);
00119 }
00120 
00121 msgs::ErrorStatus ErrorStatusWarning(unsigned int warning, const std::string& context, const std::string& warning_msg, bool output, double throttle_rate)
00122 {
00123   return createErrorStatus(context, msgs::ErrorStatus::NO_ERROR, "", warning, warning_msg, output, throttle_rate);
00124 }
00125 
00126 bool hasError(const msgs::ErrorStatus& status)
00127 {
00128   return status.error != msgs::ErrorStatus::NO_ERROR;
00129 }
00130 
00131 bool hasWarning(const msgs::ErrorStatus& status)
00132 {
00133   return status.warning != msgs::ErrorStatus::NO_WARNING;
00134 }
00135 
00136 bool isOk(const msgs::ErrorStatus& status)
00137 {
00138   return !hasError(status) && !hasWarning(status);
00139 }
00140 
00141 std::string toString(const msgs::ErrorStatus& error_status)
00142 {
00143   std::string msg;
00144 
00145   if (error_status.error_msg.size())
00146     msg = error_status.error_msg;
00147 
00148   if (error_status.warning_msg.size())
00149   {
00150     if (msg.size())
00151       msg += "\n";
00152     msg = error_status.warning_msg;
00153   }
00154 
00155   return msg;
00156 }
00157 
00158 std::string toString(const msgs::FootstepExecutionStatus& execution_status)
00159 {
00160   if (!execution_status.status)
00161     return "NO_ERROR";
00162 
00163   std::string msg;
00164 
00165   if (execution_status.status & msgs::FootstepExecutionStatus::REACHED_GOAL)
00166     msg += "REACHED_GOAL ";
00167   if (execution_status.status & msgs::FootstepExecutionStatus::WAITING_FOR_ATLAS)
00168     msg += "WAITING_FOR_ATLAS ";
00169   if (execution_status.status & msgs::FootstepExecutionStatus::EXECUTING_STEP_PLAN)
00170     msg += "EXECUTING_STEP_PLAN ";
00171   if (execution_status.status & msgs::FootstepExecutionStatus::PREMATURE_HALT)
00172     msg += "PREMATURE_HALT ";
00173   if (execution_status.status & msgs::FootstepExecutionStatus::UPDATED_STEP_PLAN)
00174     msg += "UPDATED_STEP_PLAN ";
00175   if (execution_status.status & msgs::FootstepExecutionStatus::EMERGENCY_HALT)
00176     msg += "EMERGENCY_HALT ";
00177   if (execution_status.status & msgs::FootstepExecutionStatus::ERR_UNKNOWN)
00178     msg += "ERR_UNKNOWN ";
00179   if (execution_status.status & msgs::FootstepExecutionStatus::ERR_EMPTY_PLAN)
00180     msg += "ERR_EMPTY_PLAN ";
00181   if (execution_status.status & msgs::FootstepExecutionStatus::ERR_INVALID_BEHAVIOR_MODE)
00182     msg += "ERR_INVALID_BEHAVIOR_MODE ";
00183   if (execution_status.status & msgs::FootstepExecutionStatus::ERR_INVALID_PLAN)
00184     msg += "ERR_INVALID_PLAN ";
00185   if (execution_status.status & msgs::FootstepExecutionStatus::ERR_INVALID_INCONSISTENT_INDICES)
00186     msg += "ERR_INVALID_INCONSISTENT_INDICES ";
00187   if (execution_status.status & msgs::FootstepExecutionStatus::ERR_INCONSISTENT_STEP_PLAN)
00188     msg += "ERR_INCONSISTENT_STEP_PLAN ";
00189   if (execution_status.status & msgs::FootstepExecutionStatus::ERR_CONTROLLER_NOT_READY)
00190     msg += "ERR_CONTROLLER_NOT_READY ";
00191   if (execution_status.status & msgs::FootstepExecutionStatus::ERR_FALLEN)
00192     msg += "ERR_FALLEN ";
00193   if (execution_status.status & msgs::FootstepExecutionStatus::ERR_NON_ZERO_INITIAL_STEP)
00194     msg += "ERR_NON_ZERO_INITIAL_STEP ";
00195   if (execution_status.status & msgs::FootstepExecutionStatus::ERR_NEXT_ERROR)
00196     msg += "ERR_NEXT_ERROR ";
00197 
00198   return msg;
00199 }
00200 }


vigir_footstep_planning_msgs
Author(s): Alexander Stumpf
autogenerated on Sat Jul 8 2017 02:21:30