00001 #include <vigir_footstep_planning_msgs/footstep_planning_msgs.h>
00002
00003 namespace vigir_footstep_planning
00004 {
00005
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
00034 if (step_plan.steps.empty())
00035 return msgs::ErrorStatus();
00036
00037 msgs::ErrorStatus status;
00038
00039
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 }