Go to the documentation of this file.00001 #include "continual_planning_executive/statusPublisher.h"
00002 #include <sstream>
00003 using std::stringstream;
00004 using namespace continual_planning_executive;
00005
00006 StatusPublisher::StatusPublisher()
00007 {
00008 ros::NodeHandle nh;
00009 _pubStatus = nh.advertise<ContinualPlanningStatus>(
00010 "continual_planning_status", 10);
00011 _enabled = true;
00012 }
00013
00014 StatusPublisher::~StatusPublisher()
00015 {
00016 }
00017
00018 void StatusPublisher::startedStateEstimation()
00019 {
00020 publishStatus(ContinualPlanningStatus::STATE_ESTIMATION, ContinualPlanningStatus::ACTIVE, "");
00021 }
00022
00023 void StatusPublisher::finishedStateEstimation(bool success, const SymbolicState & state)
00024 {
00025 SymbolicState::OStreamMode::forceNewlines = true;
00026 stringstream ss;
00027 ss << state;
00028 SymbolicState::OStreamMode::forceNewlines = false;
00029 publishStatus(ContinualPlanningStatus::STATE_ESTIMATION,
00030 success ? int(ContinualPlanningStatus::SUCCESS) : int(ContinualPlanningStatus::FAILURE), ss.str());
00031 }
00032
00033
00034 void StatusPublisher::startedMonitoring()
00035 {
00036 publishStatus(ContinualPlanningStatus::MONITORING, ContinualPlanningStatus::ACTIVE, "");
00037 }
00038
00039 void StatusPublisher::finishedMonitoring(bool success)
00040 {
00041 publishStatus(ContinualPlanningStatus::MONITORING,
00042 success ? int(ContinualPlanningStatus::SUCCESS) : int(ContinualPlanningStatus::FAILURE), "");
00043 }
00044
00045
00046 void StatusPublisher::startedPlanning()
00047 {
00048 publishStatus(ContinualPlanningStatus::PLANNING, ContinualPlanningStatus::ACTIVE, "");
00049 }
00050
00051 void StatusPublisher::finishedPlanning(bool success, const Plan & plan)
00052 {
00053 stringstream ss;
00054 ss << std::fixed << std::setprecision(2) << plan;
00055 publishStatus(ContinualPlanningStatus::PLANNING,
00056 success ? int(ContinualPlanningStatus::SUCCESS) : int(ContinualPlanningStatus::FAILURE), ss.str());
00057 }
00058
00059
00060 void StatusPublisher::startedExecution(const DurativeAction & a)
00061 {
00062 stringstream ss;
00063 ss << std::fixed << std::setprecision(2);
00064 ss << a;
00065 publishStatus(ContinualPlanningStatus::EXECUTION, ContinualPlanningStatus::ACTIVE, ss.str());
00066 }
00067
00068 void StatusPublisher::finishedExecution(bool success, const DurativeAction & a)
00069 {
00070 stringstream ss;
00071 ss << std::fixed << std::setprecision(2);
00072 ss << a;
00073 publishStatus(ContinualPlanningStatus::EXECUTION,
00074 success ? int(ContinualPlanningStatus::SUCCESS) : int(ContinualPlanningStatus::FAILURE), ss.str());
00075 }
00076
00077
00078 void StatusPublisher::updateCurrentPlan(const Plan & plan)
00079 {
00080 stringstream ss;
00081 ss << std::fixed << std::setprecision(2) << plan;
00082 publishStatus(ContinualPlanningStatus::CURRENT_PLAN, ContinualPlanningStatus::ACTIVE, ss.str());
00083 }
00084
00085
00086 void StatusPublisher::finishedContinualPlanning(bool success, const std::string & result)
00087 {
00088 publishStatus(ContinualPlanningStatus::CONTINUAL_PLANNING_FINISHED,
00089 success ? int(ContinualPlanningStatus::SUCCESS) : int(ContinualPlanningStatus::FAILURE), result);
00090 }
00091
00092 void StatusPublisher::publishStatus(int component, int status, const std::string & description)
00093 {
00094 ContinualPlanningStatus msg;
00095 msg.component = component;
00096 msg.status = status;
00097 msg.description = description;
00098
00099 if(_enabled && _pubStatus)
00100 _pubStatus.publish(msg);
00101 }
00102