Added flag to get instances service to include constants or not, default is not.

rosplan_knowledge_base/src/KnowledgeBase.cpp

@@ -429,16 +429,39 @@ namespace KCL_rosplan {
 			}
 
 			// check if function already exists
+            bool function_exists = false;
 			std::vector<rosplan_knowledge_msgs::KnowledgeItem>::iterator pit;
 			for(pit=model_functions.begin(); pit!=model_functions.end(); pit++) {
 				if(KnowledgeComparitor::containsKnowledge(msg, *pit)) {
-					ROS_INFO("KCL: (%s) Updating function (= (%s%s) %f)", ros::this_node::getName().c_str(), msg.attribute_name.c_str(), param_str.c_str(), msg.function_value);
-					pit->function_value = msg.function_value;
-					return;
+            		switch(msg.assign_op) {
+                    case rosplan_knowledge_msgs::KnowledgeItem::AP_ASSIGN:
+    					ROS_INFO("KCL: (%s) Updating function (= (%s%s) %f)", ros::this_node::getName().c_str(), msg.attribute_name.c_str(), param_str.c_str(), msg.function_value);
+                        pit->function_value = msg.function_value;
+                        break;                        
+                    case rosplan_knowledge_msgs::KnowledgeItem::AP_SCALE_UP:
+                    case rosplan_knowledge_msgs::KnowledgeItem::AP_INCREASE:
+                        pit->function_value = pit->function_value + msg.function_value;
+    					ROS_INFO("KCL: (%s) Increasing function (= (%s%s) %f)", ros::this_node::getName().c_str(), msg.attribute_name.c_str(), param_str.c_str(), pit->function_value);
+                        break;
+                    case rosplan_knowledge_msgs::KnowledgeItem::AP_SCALE_DOWN:
+                    case rosplan_knowledge_msgs::KnowledgeItem::AP_DECREASE:
+                        pit->function_value = pit->function_value - msg.function_value;
+    					ROS_INFO("KCL: (%s) Decreasing function (= (%s%s) %f)", ros::this_node::getName().c_str(), msg.attribute_name.c_str(), param_str.c_str(), pit->function_value);
+                        break;
+                    case rosplan_knowledge_msgs::KnowledgeItem::AP_ASSIGN_CTS:
+    					ROS_WARN("KCL: (%s) Continuous numeric effects not implemented in function updating.", ros::this_node::getName().c_str());
+                        break;
+                    }
+					function_exists = true;
+                    break;
 				}
 			}
-			ROS_INFO("KCL: (%s) Adding function (= (%s%s) %f)", ros::this_node::getName().c_str(), msg.attribute_name.c_str(), param_str.c_str(), msg.function_value);
-			model_functions.push_back(msg);
+            if (!function_exists && msg.assign_op==rosplan_knowledge_msgs::KnowledgeItem::AP_ASSIGN) {
+    			ROS_INFO("KCL: (%s) Adding function (= (%s%s) %f)", ros::this_node::getName().c_str(), msg.attribute_name.c_str(), param_str.c_str(), msg.function_value);
+	    		model_functions.push_back(msg);
+            } else if (!function_exists) {
+    			ROS_INFO("KCL: (%s) Ignoring function update (%s%s) function does not exist", ros::this_node::getName().c_str(), msg.attribute_name.c_str(), param_str.c_str());
+            }
 		}
 		break;
 
@@ -493,10 +516,12 @@ namespace KCL_rosplan {
 					res.instances.push_back(iit->second[j]);
 			}
             // constants
-			for(iit=domain_constants.begin(); iit != domain_constants.end(); iit++) {
-				for(size_t j=0; j<iit->second.size(); j++)
-					res.instances.push_back(iit->second[j]);
-			}
+            if (req.include_constants) {
+			    for(iit=domain_constants.begin(); iit != domain_constants.end(); iit++) {
+				    for(size_t j=0; j<iit->second.size(); j++)
+					    res.instances.push_back(iit->second[j]);
+			    }
+            }
 		} else {
 			std::map<std::string,std::vector<std::string> >::iterator iit;
             // objects
@@ -506,11 +531,13 @@ namespace KCL_rosplan {
 					res.instances.push_back(iit->second[j]);
 			}
             // constants
-			iit = domain_constants.find(req.type_name);
-			if(iit != domain_constants.end()) {
-				for(size_t j=0; j<iit->second.size(); j++)
-					res.instances.push_back(iit->second[j]);
-			}
+            if (req.include_constants) {
+			    iit = domain_constants.find(req.type_name);
+			    if(iit != domain_constants.end()) {
+				    for(size_t j=0; j<iit->second.size(); j++)
+					    res.instances.push_back(iit->second[j]);
+			    }
+            }
 		}
 
 		return true;

rosplan_knowledge_msgs/msg/KnowledgeItem.msg

@@ -37,6 +37,16 @@ diagnostic_msgs/KeyValue[] values
 # function value
 float64 function_value
 
+# assignment operator
+uint8 AP_ASSIGN = 0 
+uint8 AP_INCREASE = 1
+uint8 AP_DECREASE = 2
+uint8 AP_SCALE_UP = 3
+uint8 AP_SCALE_DOWN = 4
+uint8 AP_ASSIGN_CTS = 5
+
+uint8 assign_op
+
 #-----------
 # EXPRESSION
 #-----------

rosplan_knowledge_msgs/srv/GetInstanceService.srv

@@ -1,5 +1,6 @@
 # PDDL problem generation; service(1/3):
 # Get all instances of the type with the name 'typeName'.
 string type_name
+bool include_constants
 ---
 string[] instances