00001 #ifndef bwi_krexec_TimeReward_h__guard 00002 #define bwi_krexec_TimeReward_h__guard 00003 00004 #include "actasp/ExecutionObserver.h" 00005 #include "actasp/AspFluent.h" 00006 00007 #include "RewardFunction.h" 00008 00009 #include <map> 00010 00011 #include <ros/time.h> 00012 #include <ros/console.h> 00013 00014 namespace bwi_krexec { 00015 00016 template <typename State> 00017 class TimeReward : public RewardFunction<State>, public actasp::ExecutionObserver { 00018 public: 00019 double r(const State &, const actasp::AspFluent &action, const State &) const throw() { 00020 00021 ROS_INFO_STREAM("REWARD checking for: " << action.toString()); 00022 00023 std::map<actasp::AspFluent, ros::Time>::const_iterator element = startingTimes.find(action); 00024 00025 if(element == startingTimes.end()) 00026 return 0; 00027 00028 double reward = ros::Duration(ros::Time::now() - element->second).toSec(); 00029 00030 return -reward ; 00031 00032 } 00033 00034 void actionStarted(const actasp::AspFluent& action) throw() { 00035 00036 if( startingTimes.find(action) != startingTimes.end()) 00037 startingTimes.erase(action); 00038 00039 startingTimes.insert(std::make_pair(action,ros::Time::now())); 00040 00041 } 00042 00043 void actionTerminated(const actasp::AspFluent& action) throw() { 00044 00045 } 00046 00047 virtual ~TimeReward() {} 00048 private: 00049 std::map<actasp::AspFluent, ros::Time, actasp::ActionComparator> startingTimes; 00050 }; 00051 00052 } 00053 00054 #endif