predicates.cpp

Go to the documentation of this file.

#include <predicates.h>

// Operators
bool operator==(Predicate &p1, Predicate &p2)
{
    return ((*static_cast<Symbol*>(&p1) == p2) && (p1.positive_ == p2.positive_));
//     return (( *static_cast<const Symbol*>(&p1)==*static_cast<const Symbol*>(&p2) ) && (p1.positive_ == p2.positive_));
}

std::ostream& operator<<(std::ostream &out, const Predicate& p)
{
    if (not p.positive_)
        out << "-";
    out << *static_cast<const Symbol*>(&p);
    return out;
}

std::ostream& operator<<(std::ostream &out, const PredicateGroup& p)
{
    for ( size_t i = 0; i < p.predicates_.size(); i++ ) {
        out << p.predicates_[i];
        if ((i+1) < p.predicates_.size())
            out << ",";
    }
    return out;
}

std::ostream& operator<<(std::ostream &out, const Outcome& o)
{
    out << o.current_probability_ << " ";
    if (o.noisy_) {
        out << "<noise>";
    }
    else {
        out << *static_cast<const PredicateGroup*>(&o);
    }
    return out;
}

// Predicate

// PredicateGroup
PredicateGroup::PredicateGroup(PredicateList predicates) :
        predicates_(predicates)
{ }
        
PredicateGroup::PredicateGroup(std::string predicates) :
        predicates_(get_predicates_from_string(predicates))
{ }

bool PredicateGroup::is_satisfied(PredicateList predicates)
{   
        bool satisfied = true;
        for ( PredicateList::iterator it_pred=predicates_.begin() ; it_pred != predicates_.end(); it_pred++ ) {
        bool pred_satisfied = false;
        for ( PredicateList::iterator it_pred2=predicates.begin() ; it_pred2 != predicates.end(); it_pred2++ ) {
                if (*it_pred == *it_pred2) {
                pred_satisfied = true;
                }
        }
        if (!pred_satisfied) {
                satisfied = false;
        }
        }
        ROS_DEBUG_STREAM("Checking predicates\nPreconditions:" << *this << "\nState:" << PredicateGroup(predicates) << "\nResult: " << satisfied);
        return satisfied;
}

int PredicateGroup::count_satisfied(PredicateList predicates)
{
        int n_satisfied = 0;
        for ( PredicateList::iterator it_pred=predicates_.begin() ; it_pred != predicates_.end(); it_pred++ ) {
                bool pred_satisfied = false;
                for ( PredicateList::iterator it_pred2=predicates.begin() ; it_pred2 != predicates.end(); it_pred2++ ) {
                        if (*it_pred == *it_pred2) {
                                pred_satisfied = true;
                        }
                }
                if (pred_satisfied) {
                        n_satisfied++;
                }
        }
        ROS_DEBUG_STREAM("Counting predicates\nPreconditions:" << *this << "\nState:" << PredicateGroup(predicates) << "\nResult: " << n_satisfied);
        return n_satisfied;
}

int PredicateGroup::number_parameters()
{
        std::set<std::string> parameters;
        
        for ( PredicateList::iterator it_pred=predicates_.begin() ; it_pred != predicates_.end(); it_pred++ ) {
                for ( std::vector<std::string>::iterator it_param = it_pred->param_names_.begin(); it_param != it_pred->param_names_.end(); it_param++) {
                        if (parameters.count(*it_param) == 0)
                                parameters.insert(*it_param);
                }
        }
        
        return parameters.size();
}

void PredicateGroup::change_variable(std::string orig_var, std::string new_var)
{
        for ( PredicateList::iterator it_pred=predicates_.begin() ; it_pred != predicates_.end(); it_pred++ ) {
                for ( std::vector<std::string>::iterator it_param = it_pred->param_names_.begin(); it_param != it_pred->param_names_.end(); it_param++) {
                        if (it_param->compare(orig_var) == 0) {
                                *it_param = new_var;
                                // TODO look replace if
                        }
                }
        }
}

PredicateGroup PredicateGroup::get_predicates_with_vars(std::vector< std::string > variables) 
{
        PredicateList preds_with_vars;
        for ( PredicateList::iterator it_pred=predicates_.begin() ; it_pred != predicates_.end(); it_pred++ ) {
                for ( std::vector<std::string>::iterator it_param = it_pred->param_names_.begin(); it_param != it_pred->param_names_.end(); it_param++) {
                        for ( std::vector<std::string>::iterator it_var = variables.begin(); it_var != variables.end(); it_var++) {
                                if (it_param->compare(*it_var) == 0) {
                                        preds_with_vars.push_back(*it_pred);
                                }
                        }
                }
        }
        
        PredicateGroup result(preds_with_vars);
        result.remove_duplicated();
        return result;
}

void PredicateGroup::add_positive_differences(PredicateGroup compared_group) 
{
        PredicateGroup differences = compared_group.get_differences(*this);
        for ( PredicateList::iterator it_pred=differences.predicates_.begin() ; it_pred != differences.predicates_.end(); it_pred++ ) {
                if (it_pred->positive_) {
                        this->predicates_.push_back(*it_pred);
                }
        }
//         this->predicates_.insert( this->predicates_.end(), differences.predicates_.begin(), differences.predicates_.end() );
        remove_duplicated();
}

PredicateGroup PredicateGroup::get_differences(const PredicateGroup compared_group) 
{
//             std::set<Predicate> s1(predicates_.begin(), predicates_.end());
//             std::set<Predicate> s2(compared_preds.predicates_.begin(), compared_preds.predicates_.end());
//             std::vector<Predicate> diffs;
//             std::set_difference (s1.begin(), s1.end(), s2.begin(), s2.end(), std::back_inserter(diffs));
//             return PredicateGroup(diffs);
        
        PredicateList compared_preds(compared_group.predicates_);
        PredicateGroup differences;
        bool is_present;
        for ( PredicateList::iterator it_pred=predicates_.begin() ; it_pred != predicates_.end(); it_pred++ ) {
                is_present = false;
                for ( PredicateList::iterator it_pred2=compared_preds.begin() ; it_pred2 != compared_preds.end();  ) {
                        if (*it_pred == *it_pred2) {
                                is_present = true;
                                it_pred2 = compared_preds.erase(it_pred2);
                        }
                        else {
                                ++it_pred2;
                        }
                }
                if (!is_present) {
                        differences.predicates_.push_back(it_pred->get_negated());
                }
        }
        
        differences.predicates_.insert( differences.predicates_.end(), compared_preds.begin(), compared_preds.end() );
        
        differences.remove_duplicated();
        ROS_DEBUG_STREAM("Differences: " << differences);
        return differences;
}

void PredicateGroup::remove_duplicated()
{
        for ( PredicateList::iterator it_pred=predicates_.begin() ; it_pred != predicates_.end(); it_pred++ ) {
                for ( PredicateList::iterator it_pred2=(it_pred+1) ; it_pred2 != predicates_.end(); ) {
                        if (*it_pred == *it_pred2) {
                                it_pred2 = predicates_.erase(it_pred2);
                        }
                        else {
                                ++it_pred2;
                        }
                }
        }
}

void PredicateGroup::remove_variables_not_present(std::vector<std::string> variables)
{
        for ( PredicateList::iterator it_pred=predicates_.begin() ; it_pred != predicates_.end(); ) {
                if (it_pred->has_param_names_in(variables)) {
                        ++it_pred;
                }
                else {
                        it_pred = predicates_.erase(it_pred);
                }
        }
}


// Outcome

// Other




PredicateList get_predicates_from_string(std::string line)
{
    std::string item, aux;
    bool positive;
    PredicateList pred_list;
    
    // remove spaces
//     line.erase(remove_if(line.begin(), line.end(), isspace), line.end());
    // remove leading spaces
//     line = line.substr(line.find_first_not_of(" \t"));
    // remove leading and trailing spaces
    line = trim(line);
    
    std::stringstream ss(line);
    
    // case of empty string
    if ( (line=="") || (line=="--") ) {
        return pred_list;
    }
    
    while (std::getline(ss, item, ')')) {
        //std::cout << "Generating predicate: " << std::endl;
        
        // add ) to the end to complete it
        item = item + ')';
        
        // if begins with , remove it
        aux = item.substr(0,1);
        if (aux.compare(",") == 0)
            item = item.substr(1);
        
        aux = item.substr(0,1);
        if (aux.compare("-") == 0) {
            item = item.substr(1);
            positive = false;
        }
        else
            positive = true;
        
        pred_list.push_back(Predicate(get_symbol_from_string(item), positive));
    }
    return pred_list;
}

Outcome get_outcome_from_string(std::string line)
{
    std::string item;
    float probability;
    
    // remove leading spaces
    line = line.substr(line.find_first_not_of(" \t"));
    std::stringstream ss(line);
    
    if(std::getline(ss, item, ' ')) {
        //std::cout << "Probability: " << item << std::endl;
    }
    probability = atof(item.c_str());
    
    if (ss.eof())
        return Outcome(probability);
    
    // get rest of the line
    std::getline(ss, item);
    if (item.find("<noise>")!=std::string::npos) {
        return Outcome(probability, true);
    }
    else if (item.find("<no-change>")!=std::string::npos) {
        return Outcome(probability);
    }
    else {
        return Outcome(get_predicates_from_string(item), probability);
    }
}