rule_learner_alg_node.cpp

Go to the documentation of this file.

#include "rule_learner_alg_node.h"

RuleLearnerAlgNode::RuleLearnerAlgNode(void) :
  algorithm_base::IriBaseAlgorithm<RuleLearnerAlgorithm>()
{
  //init class attributes if necessary
  this->loop_rate_ = 0.2;//in [Hz]

  // [init publishers]
  
  // [init subscribers]
  
  // [init services]
  this->learn_with_pasula_server_ = this->public_node_handle_.advertiseService("learn_with_pasula", &RuleLearnerAlgNode::learn_with_pasulaCallback, this);
  this->add_state_server_ = this->public_node_handle_.advertiseService("add_state", &RuleLearnerAlgNode::add_stateCallback, this);
  this->add_action_server_ = this->public_node_handle_.advertiseService("add_action", &RuleLearnerAlgNode::add_actionCallback, this);
  
  // [init clients]
  learn_pasula_client_ = this->public_node_handle_.serviceClient<iri_learning_msgs::LearnRulesFromTransitions>("learn_pasula");
  representation_to_string_client_ = this->public_node_handle_.serviceClient<estirabot_msgs::RepresentationToString>("representation_to_string");
  get_state_changes_client_ = this->public_node_handle_.serviceClient<estirabot_msgs::StateRepresentationChanges>("get_state_changes");
  
  // [init action servers]
  
  // [init action clients]
  
  pasula_time_ = ros::Duration(0);
  pasula_iter_ = 0;
}

RuleLearnerAlgNode::~RuleLearnerAlgNode(void)
{
  // [free dynamic memory]
}

void RuleLearnerAlgNode::mainNodeThread(void)
{
  // [fill msg structures]
  
  // [fill srv structure and make request to the server]
  
  // [fill action structure and make request to the action server]

  // [publish messages]
        std::vector<std::string> actions = this->alg_.actions_needing_pasula_learning();
        if (actions.size() > 0) {
                ROS_INFO("RuleLearnerAlgNode:: Requesting pasula rules."); 
                
                this->alg_.lock();
                TransitionList transitions = this->alg_.get_transitions_with_actions(actions);
                this->alg_.write_transitions(remove_unused_predicates(transitions));
                this->alg_.reset_new_transitions();
                this->alg_.unlock();
                
                ros::Time begin = ros::Time::now();
                // learn_pasula_srv_.request.data = my_var; 
                if (learn_pasula_client_.call(learn_pasula_srv_)) 
                { 
                        ROS_INFO_STREAM("RuleLearnerAlgNode:: Response: " << learn_pasula_srv_.response.ok); 
                } 
                else 
                { 
                        ROS_ERROR("RuleLearnerAlgNode:: Failed to Call Server on topic learn_pasula "); 
                        return;
                }
                this->alg_.lock();
                std::vector<int> number_rules = this->alg_.update_with_pasula_rules(actions, transitions);
                this->alg_.unlock();
                
                // ugly logging
                pasula_time_ += ros::Time::now() - begin;
                pasula_iter_++;
                
                std::string pasula_logs_file_path = "/tmp/pasula_logs.txt";
                std::ofstream pasula_logs_file(pasula_logs_file_path.c_str());
                if (pasula_logs_file.is_open())
                {
                        pasula_logs_file << pasula_time_ << std::endl;
                        pasula_logs_file << pasula_iter_ << std::endl;
                        pasula_logs_file << number_rules[2] << std::endl;
                        pasula_logs_file << number_rules[0] << std::endl;
                        pasula_logs_file << number_rules[1] << std::endl;
                        
                        
                }
                else
                        ROS_ERROR_STREAM("Couldn't write pasula logs to " << pasula_logs_file_path);
                pasula_logs_file.close();
        }
        
}

/*  [subscriber callbacks] */

/*  [service callbacks] */
/*bool RuleLearnerAlgNode::learn_with_pasulaCallback(iri_rule_learner::LearnWithPasula::Request &req, iri_rule_learner::LearnWithPasula::Response &res) 
{ 
        ROS_INFO("RuleLearnerAlgNode::learn_with_pasulaCallback: New Request Received!"); */
/*
        std::vector<std::string> actions = this->alg_.actions_needing_pasula_learning();
        if (actions.size() > 0) {
                ROS_INFO("RuleLearnerAlgNode:: Requesting pasula rules."); 
                
                this->alg_.lock();
                TransitionList transitions = this->alg_.get_transitions_with_actions(actions);
                this->alg_.write_transitions(remove_unused_predicates(transitions));
                this->alg_.reset_new_transitions();
                this->alg_.unlock();
                
                ros::Time begin = ros::Time::now();
                // learn_pasula_srv_.request.data = my_var; 
                if (learn_pasula_client_.call(learn_pasula_srv_)) 
                { 
                        ROS_INFO_STREAM("RuleLearnerAlgNode:: Response: " << learn_pasula_srv_.response.ok); 
                } 
                else 
                { 
                        ROS_ERROR("RuleLearnerAlgNode:: Failed to Call Server on topic learn_pasula "); 
                        return false;
                }
                this->alg_.lock();
                std::vector<int> number_rules = this->alg_.update_with_pasula_rules(actions, transitions);
                this->alg_.unlock();
                res.num_pasula_rules = number_rules[0];
                res.num_learned_rules = number_rules[1];
                res.num_total_rules = number_rules[2];
                res.time = ros::Time::now() - begin;
        }

        return true; */
//}
bool RuleLearnerAlgNode::add_stateCallback(estirabot_msgs::AddState::Request &req, estirabot_msgs::AddState::Response &res) 
{ 
        ROS_DEBUG("RuleLearnerAlgNode::add_stateCallback: New Request Received!"); 
        
        //use appropiate mutex to shared variables if necessary 
        //this->alg_.lock(); 
        //this->add_state_mutex_.enter(); 

        res.rules_updated = this->alg_.add_state(req.state);

        //unlock previously blocked shared variables 
        //this->alg_.unlock(); 
        //this->add_state_mutex_.exit(); 
        
        return true; 
}


bool RuleLearnerAlgNode::add_actionCallback(estirabot_msgs::AddAction::Request &req, estirabot_msgs::AddAction::Response &res) 
{ 
        ROS_DEBUG("RuleLearnerAlgNode::add_actionCallback: New Request Received!"); 
        
        //use appropiate mutex to shared variables if necessary 
        //this->alg_.lock(); 
        //this->add_action_mutex_.enter(); 
        
        res.needs_learning = this->alg_.add_action(req.action, req.target_dirty_areas.indexes);
        
        //unlock previously blocked shared variables 
        //this->alg_.unlock(); 
        //this->add_action_mutex_.exit(); 
        
        return true; 
}

/*  [action callbacks] */

/*  [action requests] */

void RuleLearnerAlgNode::node_config_update(Config &config, uint32_t level)
{
        this->alg_.lock();

        this->alg_.unlock();
}

void RuleLearnerAlgNode::addNodeDiagnostics(void)
{
}

/* main function */
int main(int argc,char *argv[])
{
        return algorithm_base::main<RuleLearnerAlgNode>(argc, argv, "rule_learner_alg_node");
}