<commit_before>#include <ros/ros.h>
#include <vector>
#include <iostream>
#include <fstream>
#include "rosplan_knowledge_base/KnowledgeBase.h"

namespace KCL_rosplan {


	/*-----------------*/
	/* knowledge query */
	/*-----------------*/

	bool KnowledgeBase::queryKnowledge(rosplan_knowledge_msgs::KnowledgeQueryService::Request  &req, rosplan_knowledge_msgs::KnowledgeQueryService::Response &res) {

		std::vector<rosplan_knowledge_msgs::KnowledgeItem>::iterator iit;
		res.all_true = true;
		for(iit = req.knowledge.begin(); iit!=req.knowledge.end(); iit++) {

			bool present = false;
			if(iit->knowledge_type == rosplan_knowledge_msgs::KnowledgeItem::INSTANCE) {

				// check if instance exists
				std::vector<std::string>::iterator sit;
				sit = find(domain_instances[iit->instance_type].begin(), domain_instances[iit->instance_type].end(), iit->instance_name);
				present = (sit!=domain_instances[iit->instance_type].end());
				
			} else if(iit->knowledge_type == rosplan_knowledge_msgs::KnowledgeItem::DOMAIN_FUNCTION) {
	
				// check if function exists; TODO inequalities
				std::vector<rosplan_knowledge_msgs::KnowledgeItem>::iterator pit;
				for(pit=domain_functions.begin(); pit!=domain_functions.end(); pit++) {
					if(containsKnowledge(*iit, *pit)) {
						present = true;
						pit = domain_functions.end();
					}
				}

			} else if(iit->knowledge_type == rosplan_knowledge_msgs::KnowledgeItem::DOMAIN_ATTRIBUTE) {

				// check if fact is true
				std::vector<rosplan_knowledge_msgs::KnowledgeItem>::iterator pit;
				for(pit=domain_attributes.begin(); pit!=domain_attributes.end(); pit++) {
					if(containsKnowledge(*iit, *pit)) {
						present = true;
						break;
					}
				}
			}

			if(!present) {
				res.all_true = false;
				res.false_knowledge.push_back(*iit);
			}
		}

		return true;
	}

	/*------------------*/
	/* knowledge update */
	/*------------------*/

	bool KnowledgeBase::updateKnowledge(rosplan_knowledge_msgs::KnowledgeUpdateService::Request  &req, rosplan_knowledge_msgs::KnowledgeUpdateService::Response &res) {

		if(req.update_type == rosplan_knowledge_msgs::KnowledgeUpdateService::Request::ADD_KNOWLEDGE)
			addKnowledge(req.knowledge);
		else if(req.update_type == rosplan_knowledge_msgs::KnowledgeUpdateService::Request::ADD_GOAL)
			addMissionGoal(req.knowledge);
		else if(req.update_type == rosplan_knowledge_msgs::KnowledgeUpdateService::Request::REMOVE_KNOWLEDGE)
			removeKnowledge(req.knowledge);

		res.success = true;
		return true;
	}

	/*----------------*/
	/* removing items */
	/*----------------*/

	void KnowledgeBase::removeKnowledge(rosplan_knowledge_msgs::KnowledgeItem &msg) {

		if(msg.knowledge_type == rosplan_knowledge_msgs::KnowledgeItem::INSTANCE) {		

			// search for instance
			std::vector<std::string>::iterator iit;
			for(iit = domain_instances[msg.instance_type].begin(); iit!=domain_instances[msg.instance_type].end(); iit++) {

				std::string name = *iit;

				if(name.compare(msg.instance_name)==0 || msg.instance_name.compare("")==0) {

					// remove instance from knowledge base
					ROS_INFO("KCL: (KB) Removing instance (%s, %s)", msg.instance_type.c_str(), (msg.instance_name.compare("")==0) ? "ALL" : msg.instance_name.c_str());
					checkFilters(msg, false);
					domain_instances[msg.instance_type].erase(iit);

					// remove affected domain attributes
					std::vector<rosplan_knowledge_msgs::KnowledgeItem>::iterator pit;
					for(pit=domain_attributes.begin(); pit!=domain_attributes.end(); pit++) {
						if(containsInstance(*pit, name)) {
							ROS_INFO("KCL: (KB) Removing domain attribute (%s)", pit->attribute_name.c_str());
							checkFilters(*pit, false);
							pit = domain_attributes.erase(pit);
							if(pit==domain_attributes.end()) break;
						}
					}

					// remove affected instance attributes
					for(pit=instance_attributes[name].begin(); pit!=instance_attributes[name].end(); pit++) {
						if(containsInstance(*pit, name)) {
							ROS_INFO("KCL: (KB) Removing instance attribute (%s, %s)", name.c_str(), pit->attribute_name.c_str());
							checkFilters(*pit, false);
							pit = instance_attributes[name].erase(pit);
							if(pit==instance_attributes[name].end()) break;
						}
					}

					// finish
					if(iit==domain_instances[msg.instance_type].end()) break;
				}
			}

		} else if(msg.knowledge_type == rosplan_knowledge_msgs::KnowledgeItem::DOMAIN_FUNCTION) {

			// remove domain attribute (function) from knowledge base
			std::vector<rosplan_knowledge_msgs::KnowledgeItem>::iterator pit;
			for(pit=domain_functions.begin(); pit!=domain_functions.end(); pit++) {
				if(containsKnowledge(msg, *pit)) {
					ROS_INFO("KCL: (KB) Removing domain attribute (%s)", msg.attribute_name.c_str());
					checkFilters(msg, false);
					pit = domain_functions.erase(pit);
					if(pit==domain_functions.end()) break;
				}
			}

		} else if(msg.knowledge_type == rosplan_knowledge_msgs::KnowledgeItem::DOMAIN_ATTRIBUTE) {

			// remove domain attribute (predicate) from knowledge base
			std::vector<rosplan_knowledge_msgs::KnowledgeItem>::iterator pit;
			for(pit=domain_attributes.begin(); pit!=domain_attributes.end(); pit++) {
				if(containsKnowledge(msg, *pit)) {
					ROS_INFO("KCL: (KB) Removing domain attribute (%s)", msg.attribute_name.c_str());
					checkFilters(msg, false);
					pit = domain_attributes.erase(pit);
					if(pit==domain_attributes.end()) break;
				}
			}

		} else if(msg.knowledge_type == rosplan_knowledge_msgs::KnowledgeItem::INSTANCE_ATTRIBUTE) {

			// remove instance attribute (non-symbolic) from knowledge base
			std::vector<rosplan_knowledge_msgs::KnowledgeItem>::iterator pit;
			for(pit=instance_attributes[msg.instance_name].begin(); pit!=instance_attributes[msg.instance_name].end(); pit++) {
				if(containsKnowledge(msg, *pit)) {
					ROS_INFO("KCL: (KB) Removing instance attribute (%s, %s)", msg.instance_name.c_str(), msg.attribute_name.c_str());
					checkFilters(msg, false);
					pit = instance_attributes[msg.instance_name].erase(pit);
					if(pit==instance_attributes[msg.instance_name].end()) break;
				}
			}
		}
	}

	/*--------------*/
	/* adding items */
	/*--------------*/

	/*
	 * add an instance, domain predicate, or function to the knowledge base
	 */
	void KnowledgeBase::addKnowledge(rosplan_knowledge_msgs::KnowledgeItem &msg) {<commit_msg>Fix for removal loops<commit_after>#include <ros/ros.h>
#include <vector>
#include <iostream>
#include <fstream>
#include "rosplan_knowledge_base/KnowledgeBase.h"

namespace KCL_rosplan {


	/*-----------------*/
	/* knowledge query */
	/*-----------------*/

	bool KnowledgeBase::queryKnowledge(rosplan_knowledge_msgs::KnowledgeQueryService::Request  &req, rosplan_knowledge_msgs::KnowledgeQueryService::Response &res) {

		std::vector<rosplan_knowledge_msgs::KnowledgeItem>::iterator iit;
		res.all_true = true;
		for(iit = req.knowledge.begin(); iit!=req.knowledge.end(); iit++) {

			bool present = false;
			if(iit->knowledge_type == rosplan_knowledge_msgs::KnowledgeItem::INSTANCE) {

				// check if instance exists
				std::vector<std::string>::iterator sit;
				sit = find(domain_instances[iit->instance_type].begin(), domain_instances[iit->instance_type].end(), iit->instance_name);
				present = (sit!=domain_instances[iit->instance_type].end());
				
			} else if(iit->knowledge_type == rosplan_knowledge_msgs::KnowledgeItem::DOMAIN_FUNCTION) {
	
				// check if function exists; TODO inequalities
				std::vector<rosplan_knowledge_msgs::KnowledgeItem>::iterator pit;
				for(pit=domain_functions.begin(); pit!=domain_functions.end(); pit++) {
					if(containsKnowledge(*iit, *pit)) {
						present = true;
						pit = domain_functions.end();
					}
				}

			} else if(iit->knowledge_type == rosplan_knowledge_msgs::KnowledgeItem::DOMAIN_ATTRIBUTE) {

				// check if fact is true
				std::vector<rosplan_knowledge_msgs::KnowledgeItem>::iterator pit;
				for(pit=domain_attributes.begin(); pit!=domain_attributes.end(); pit++) {
					if(containsKnowledge(*iit, *pit)) {
						present = true;
						break;
					}
				}
			}

			if(!present) {
				res.all_true = false;
				res.false_knowledge.push_back(*iit);
			}
		}

		return true;
	}

	/*------------------*/
	/* knowledge update */
	/*------------------*/

	bool KnowledgeBase::updateKnowledge(rosplan_knowledge_msgs::KnowledgeUpdateService::Request  &req, rosplan_knowledge_msgs::KnowledgeUpdateService::Response &res) {

		if(req.update_type == rosplan_knowledge_msgs::KnowledgeUpdateService::Request::ADD_KNOWLEDGE)
			addKnowledge(req.knowledge);
		else if(req.update_type == rosplan_knowledge_msgs::KnowledgeUpdateService::Request::ADD_GOAL)
			addMissionGoal(req.knowledge);
		else if(req.update_type == rosplan_knowledge_msgs::KnowledgeUpdateService::Request::REMOVE_KNOWLEDGE)
			removeKnowledge(req.knowledge);

		res.success = true;
		return true;
	}

	/*----------------*/
	/* removing items */
	/*----------------*/

	void KnowledgeBase::removeKnowledge(rosplan_knowledge_msgs::KnowledgeItem &msg) {

		if(msg.knowledge_type == rosplan_knowledge_msgs::KnowledgeItem::INSTANCE) {		

			// search for instance
			std::vector<std::string>::iterator iit;
			for(iit = domain_instances[msg.instance_type].begin(); iit!=domain_instances[msg.instance_type].end(); iit++) {

				std::string name = *iit;

				if(name.compare(msg.instance_name)==0 || msg.instance_name.compare("")==0) {

					// remove instance from knowledge base
					ROS_INFO("KCL: (KB) Removing instance (%s, %s)", msg.instance_type.c_str(), (msg.instance_name.compare("")==0) ? "ALL" : msg.instance_name.c_str());
					checkFilters(msg, false);
					domain_instances[msg.instance_type].erase(iit);

					// remove affected domain attributes
					std::vector<rosplan_knowledge_msgs::KnowledgeItem>::iterator pit;
					for(pit=domain_attributes.begin(); pit!=domain_attributes.end(); pit++) {
						if(containsInstance(*pit, name)) {
							ROS_INFO("KCL: (KB) Removing domain attribute (%s)", pit->attribute_name.c_str());
							checkFilters(*pit, false);
							pit = domain_attributes.erase(pit);
							if(pit!=domain_attributes.begin()) pit--;
							if(pit==domain_attributes.end()) break;
						}
					}

					// remove affected instance attributes
					for(pit=instance_attributes[name].begin(); pit!=instance_attributes[name].end(); pit++) {
						if(containsInstance(*pit, name)) {
							ROS_INFO("KCL: (KB) Removing instance attribute (%s, %s)", name.c_str(), pit->attribute_name.c_str());
							checkFilters(*pit, false);
							pit = instance_attributes[name].erase(pit);
							if(pit!=instance_attributes[name].begin()) pit--;
							if(pit==instance_attributes[name].end()) break;
						}
					}

					// finish
					if(iit==domain_instances[msg.instance_type].end()) break;
				}
			}

		} else if(msg.knowledge_type == rosplan_knowledge_msgs::KnowledgeItem::DOMAIN_FUNCTION) {

			// remove domain attribute (function) from knowledge base
			std::vector<rosplan_knowledge_msgs::KnowledgeItem>::iterator pit;
			for(pit=domain_functions.begin(); pit!=domain_functions.end(); pit++) {
				if(containsKnowledge(msg, *pit)) {
					ROS_INFO("KCL: (KB) Removing domain attribute (%s)", msg.attribute_name.c_str());
					checkFilters(msg, false);
					pit = domain_functions.erase(pit);
					if(pit!=domain_functions.begin()) pit--;
					if(pit==domain_functions.end()) break;
				}
			}

		} else if(msg.knowledge_type == rosplan_knowledge_msgs::KnowledgeItem::DOMAIN_ATTRIBUTE) {

			// remove domain attribute (predicate) from knowledge base
			std::vector<rosplan_knowledge_msgs::KnowledgeItem>::iterator pit;
			for(pit=domain_attributes.begin(); pit!=domain_attributes.end(); pit++) {
				if(containsKnowledge(msg, *pit)) {
					ROS_INFO("KCL: (KB) Removing domain attribute (%s)", msg.attribute_name.c_str());
					checkFilters(msg, false);
					pit = domain_attributes.erase(pit);
					if(pit!=domain_attributes.begin()) pit--;
					if(pit==domain_attributes.end()) break;
				}
			}

		} else if(msg.knowledge_type == rosplan_knowledge_msgs::KnowledgeItem::INSTANCE_ATTRIBUTE) {

			// remove instance attribute (non-symbolic) from knowledge base
			std::vector<rosplan_knowledge_msgs::KnowledgeItem>::iterator pit;
			for(pit=instance_attributes[msg.instance_name].begin(); pit!=instance_attributes[msg.instance_name].end(); pit++) {
				if(containsKnowledge(msg, *pit)) {
					ROS_INFO("KCL: (KB) Removing instance attribute (%s, %s)", msg.instance_name.c_str(), msg.attribute_name.c_str());
					checkFilters(msg, false);
					pit = instance_attributes[msg.instance_name].erase(pit);
					if(pit!=instance_attributes[msg.instance_name].begin()) pit--;
					if(pit==instance_attributes[msg.instance_name].end()) break;
				}
			}
		}
	}

	/*--------------*/
	/* adding items */
	/*--------------*/

	/*
	 * add an instance, domain predicate, or function to the knowledge base
	 */
	void KnowledgeBase::addKnowledge(rosplan_knowledge_msgs::KnowledgeItem &msg) {
{'fraction_non_alphanumeric': 0.16901734104046243, 'fraction_numerical': 0.00046242774566473987, 'mean_word_length': 4.217531162042621, 'pattern_counts': {'":': 0, '<': 29, '<?xml version=': 0, '>': 47, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '967886', 'n_tokens_mistral': 5016, 'n_tokens_neox': 4079, 'n_words': 629}