Sat Nov 4 19:07:07 UTC 2006 John S. Kinnebrew <john.s.kinnebrew@vanderbilt.edu>

12 files changed, 241 insertions, 29 deletions

diff --git a/ChangeLog b/ChangeLog
index 1eb204e167d..ed80ef617d9 100644
--- a/ChangeLog
+++ b/ChangeLog
@@ -1,3 +1,20 @@
+Sat Nov  4 19:07:07 UTC 2006  John S. Kinnebrew  <john.s.kinnebrew@vanderbilt.edu>
+
+        * SA_POP/Driver.cpp:
+        * SA_POP/Planner.h:
+        * SA_POP/Planner.cpp:
+        * SA_POP/SA_POP_Types.h:
+        * SA_POP/SA_WorkingPlan.h:
+        * SA_POP/SA_WorkingPlan.cpp:
+        * SA_POP/SPACE.san.xml:
+        * SA_POP/WorkingPlan.h:
+        * SA_POP/SANet/SANet.cpp:
+        * SA_POP/SANet/SANode.h:
+        * SA_POP/SANet/SANode.cpp:
+
+        Updated SA-POP goal handling.
+
+
 Thu Nov  2 23:41:56 UTC 2006  Nishanth Shankaran  <nshankar@nospam.com>
 
         * SA_POP/server:
@@ -14,7 +31,7 @@ Thu Nov  2 19:25:01 UTC 2006  John S. Kinnebrew  <john.s.kinnebrew@vanderbilt.ed
         * SA_POP/SA_POP_Types.h:
         * SA_POP/SA_PlanCommands.cpp:
         * SA_POP/SA_WorkingPlan.cpp:
-        
+
         Revised code to correct errors in Linux compilation.
 
 Thu Nov  2 17:48:07 UTC 2006  Nishanth Shankaran  <nshanka@nospam.com>
diff --git a/SA_POP/Driver.cpp b/SA_POP/Driver.cpp
index 4e220ed4e5d..c87d7db95a5 100644
--- a/SA_POP/Driver.cpp
+++ b/SA_POP/Driver.cpp
@@ -53,7 +53,7 @@ int main (int argc, char **argv)
   planner->add_out_adapter (&screen_out);
 
   // Run planner (with a maximum of 50 steps of spreading activation).
-  planner->plan (50);
+  planner->replan (50);
 
   SA_POP::Plan plan = planner->get_plan ();
   
diff --git a/SA_POP/Planner.cpp b/SA_POP/Planner.cpp
index 783e2506eaa..5fadfda4f0d 100644
--- a/SA_POP/Planner.cpp
+++ b/SA_POP/Planner.cpp
@@ -105,12 +105,72 @@ void Planner::add_out_adapter (OutAdapter *out)
 };
 
 // Run planning.
-bool Planner::plan (size_t sa_max_steps)
+bool Planner::plan (size_t sa_max_steps, SA_POP::Goal goal)
 {
+  // Add goal to working plan and task network.
+  this->working_plan_->set_goal (goal);
+  this->sanet_->update_goals (goal.goal_conds);
+
+  // Run spreading activation.
+  this->sanet_->update (sa_max_steps);
+
+  // Set planning strategy goals and satisfy open conditions.
+  this->plan_strat_->set_goals (goal.goal_conds);
+  if (this->plan_strat_->satisfy_open_conds ()) {
+    this->plan_ = this->working_plan_->get_plan ();
+    this->notify_plan_changed ();
+    return true;
+  }
+
+  return false;
+};
+
+// Replan with new goal.
+bool Planner::replan (size_t sa_max_steps, SA_POP::Goal goal)
+{
+  //****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****
+  // Full replanning not implemented, so just restart planning.
+  //****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****
+
+  // Clear plan.
+  this->plan_.causal_links.clear ();
+  this->plan_.connections.clear ();
+  this->plan_.sched_links.clear ();
+  this->plan_.task_insts.clear ();
+  this->plan_.threat_links.clear ();
+
+  // Add goal to working plan and task network.
+  //****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****
+  // Need to reset working plan.
+  //****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****
+  this->working_plan_->set_goal (goal);
+  this->sanet_->update_goals (goal.goal_conds);
+
+  // Run spreading activation.
   this->sanet_->update (sa_max_steps);
-	std::ofstream fout("output.txt");
-	this->sanet_->print(fout,true);
-	fout.close();
+
+  // Set planning strategy goals and satisfy open conditions.
+  this->plan_strat_->set_goals (goal.goal_conds);
+  if (this->plan_strat_->satisfy_open_conds ()) {
+    this->plan_ = this->working_plan_->get_plan ();
+    this->notify_plan_changed ();
+    return true;
+  }
+
+  return false;
+};
+
+// Replan with existing goal.
+bool Planner::replan (size_t sa_max_steps)
+{
+  //****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****
+  // Full replanning not implemented, so just restart planning.
+  //****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****
+
+  // Run spreading activation.
+  this->sanet_->update (sa_max_steps);
+
+  // Set planning strategy goals and satisfy open conditions.
   this->plan_strat_->set_goals (this->sanet_->get_goals ());
   if (this->plan_strat_->satisfy_open_conds ()) {
     this->plan_ = this->working_plan_->get_plan ();
diff --git a/SA_POP/Planner.h b/SA_POP/Planner.h
index 1a904158c89..6f3c34902da 100644
--- a/SA_POP/Planner.h
+++ b/SA_POP/Planner.h
@@ -83,9 +83,29 @@ namespace SA_POP {
     /**
      * @param sa_max_steps  Maximum steps to run spreading activation.
      *
+     * @param goal  Goal for which to plan.
+     *
+     * @return  True if planning succeeded, false otherwise.
+     */
+    virtual bool plan (size_t sa_max_steps, SA_POP::Goal goal);
+    
+    /// Replan with new goal.
+    /**
+     * @param sa_max_steps  Maximum steps to run spreading activation.
+     *
+     * @param goal  Goal for which to plan.
+     *
+     * @return  True if planning succeeded, false otherwise.
+     */
+    virtual bool replan (size_t sa_max_steps, SA_POP::Goal goal);
+
+    /// Replan with current goal.
+    /**
+     * @param sa_max_steps  Maximum steps to run spreading activation.
+     *
      * @return  True if planning succeeded, false otherwise.
      */
-    virtual bool plan (size_t sa_max_steps);
+    virtual bool replan (size_t sa_max_steps);
 
     /// Get current plan.
     /**
diff --git a/SA_POP/SANet/SANet.cpp b/SA_POP/SANet/SANet.cpp
index e46f0d94f23..f442e771fb6 100644
--- a/SA_POP/SANet/SANet.cpp
+++ b/SA_POP/SANet/SANet.cpp
@@ -257,17 +257,36 @@ void SANet::Network::update_cond_val (CondID cond_id, Probability true_prob)
 };
 
 // Update a condition's (goal) utility.
-void SANet::Network::update_cond_util (CondID cond_id, Probability utility)
+void SANet::Network::update_cond_util (CondID cond_id, Utility utility)
 {
-  // ****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP
-  throw "Plan monitoring/updating/replanning not yet implemented: SANet::SANet::Network::updated_cond_val ().";
+  CondNodeMap::iterator cond_iter = this->cond_nodes_.find (cond_id);
+  if (cond_iter == this->cond_nodes_.end ())
+    throw "SANet::Network::update_cond_util(): Could not find condition by ID.";
+  cond_iter->second->set_goal_util (utility);
 };
 
 // Update all condition utilities based on new goal set.
 void SANet::Network::update_goals (GoalMap goals)
 {
-  // ****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP****TEMP
-  throw "Plan monitoring/updating/replanning not yet implemented: SANet::SANet::Network::update_goals ().";
+  SA_POP::GoalMap goal;
+
+  // Remove all old goals.
+  for (GoalMap::iterator old_goal_iter = this->goals_.begin ();
+    old_goal_iter != this->goals_.end ();
+    old_goal_iter++)
+  {
+    this->update_cond_util (old_goal_iter->first, 0);
+  }
+  this->goals_.clear ();
+
+  // Add all new goals.
+  for (GoalMap::iterator new_goal_iter = goals.begin ();
+    new_goal_iter != goals.end ();
+    new_goal_iter++)
+  {
+    this->update_cond_util (new_goal_iter->first, new_goal_iter->second);
+  }
+  this->goals_ = goals;
 };
 
 // Get a condition's current value (probability of being true).
diff --git a/SA_POP/SANet/SANode.cpp b/SA_POP/SANet/SANode.cpp
index 4ce882526d0..952026e6290 100644
--- a/SA_POP/SANet/SANode.cpp
+++ b/SA_POP/SANet/SANode.cpp
@@ -584,12 +584,15 @@ CondNode::CondNode (CondID ID, std::string name, MultFactor atten_factor,
   // Set prob_changed_ flag.
   prob_changed_ = true;
 
-  // If this node is a goal, set util_changed_ flag and add goal utility
-  // to positive utilities.
+  // If this node is a goal, set util_changed_ flag and add goal utility.
   if (goal_util > 0) {
-    util_changed_ = true;
-    pos_util_.utility = goal_util;
-    pos_util_.common.insert (std::make_pair (ID, goal_util));
+    this->util_changed_ = true;
+    this->pos_util_.utility = goal_util;
+    this->pos_util_.common.insert (std::make_pair (ID, goal_util));
+  } else if (goal_util < 0) {
+    this->util_changed_ = true;
+    this->neg_util_.utility = goal_util;
+    this->neg_util_.common.insert (std::make_pair (ID, goal_util));
   }
 };
 
@@ -598,6 +601,33 @@ CondNode::~CondNode (void)
   // Nothing to do.
 };
 
+// Update goal utility.
+void CondNode::set_goal_util (Utility util)
+{
+  // Remove old goal utility.
+  if (this->goal_util_ > 0) {
+    this->pos_util_.utility -= this->goal_util_;
+    this->pos_util_.common.erase (this->ID_);
+  } else if (this->goal_util_ < 0) {
+    this->neg_util_.utility -= this->goal_util_;
+    this->neg_util_.common.erase (this->ID_);
+  }
+
+  // Update total utility and utility maps for new utility.
+  if (util > 0) {
+    this->pos_util_.utility += util;
+    this->pos_util_.common.insert (std::make_pair (this->ID_, util));
+  } else if (util < 0) {
+    this->neg_util_.utility += util;
+    this->neg_util_.common.insert (std::make_pair (this->ID_, util));
+  }
+
+  // Update utility changed flag and goal utility.
+  this->util_changed_ = true;
+  this->goal_util_ = util;
+};
+
+
 // Get initial/current probability.
 Probability CondNode::get_init_prob (bool value)
 {
diff --git a/SA_POP/SANet/SANode.h b/SA_POP/SANet/SANode.h
index 92788ddb91e..bd285cf9e4c 100644
--- a/SA_POP/SANet/SANode.h
+++ b/SA_POP/SANet/SANode.h
@@ -314,6 +314,12 @@ namespace SANet {
     /// Destructor.
     virtual ~CondNode (void);
 
+    /// Update goal utility.
+    /**
+     * @param util  Goal utility.
+     */
+    virtual void set_goal_util (Utility util);
+
     /// Get initial/current probability.
     /**
      * @param value  Value for which to get probability (default = true).
diff --git a/SA_POP/SA_POP_Types.h b/SA_POP/SA_POP_Types.h
index 4e4ff0c0837..b215d9eb2e9 100644
--- a/SA_POP/SA_POP_Types.h
+++ b/SA_POP/SA_POP_Types.h
@@ -424,6 +424,37 @@ namespace SA_POP {
     };
   };
 
+  /// Type of a set of conditions with associated time constraints.
+  typedef std::map<CondID, TimeValue> TimeConstraintMap;
+
+  /// Type of a goal for SA-POP.
+  struct Goal
+  {
+    // Unique ID of goal.
+    GoalID goal_id;
+
+    // Human-readable name of goal (used for debugging, not identification).
+    std::string name;
+
+    // Goal conditions with utility values.
+    GoalMap goal_conds;
+
+    // Start-by and end-by constraints on conditions, in relative time
+    // (relative to start of opstring to achieve this goal).
+    TimeConstraintMap rel_time_constraints;
+
+    // Start-by and end-by constraints on conditions, in clock time.
+    TimeConstraintMap abs_time_constraints;
+
+    // Expected time range for start of opstring (for achieving this goal)
+    // execution.
+    TimeWindow start_window;
+
+    bool operator== (const Goal &s) const { return (this->goal_id == s.goal_id); };
+    bool operator!= (const Goal &s) const { return !(*this == s); };
+    bool operator< (const Goal &s) const { return this->goal_id < s.goal_id; };
+  };
+
   inline std::string to_string(int x)
   {
     std::ostringstream o;
diff --git a/SA_POP/SA_WorkingPlan.cpp b/SA_POP/SA_WorkingPlan.cpp
index 368ff002c02..b0860dd1b66 100644
--- a/SA_POP/SA_WorkingPlan.cpp
+++ b/SA_POP/SA_WorkingPlan.cpp
@@ -38,6 +38,14 @@ add_threats_cmd_ (0)
   this->task_impls_.clear ();
   this->causal_links_.clear ();
 
+  // Clear goal.
+  this->goal_.goal_id = "NULL";
+  this->goal_.name = "NULL";
+  this->goal_.abs_time_constraints.clear ();
+  this->goal_.rel_time_constraints.clear ();
+  this->goal_.goal_conds.clear ();
+  this->goal_.start_window = std::make_pair (0, 0);
+
   // Clear plan.
   this->plan_.causal_links.clear ();
   this->plan_.connections.clear ();
@@ -236,6 +244,12 @@ SA_WorkingPlan::~SA_WorkingPlan (void)
   // Nothing to do.
 };
 
+// Set goal.
+void SA_WorkingPlan::set_goal (const SA_POP::Goal &goal)
+{
+  this->goal_ = goal; 
+};
+
 // Set command prototypes to use.
 void SA_WorkingPlan::set_commands (AddOpenThreatsCmd *add_threats_cmd)
 {
diff --git a/SA_POP/SA_WorkingPlan.h b/SA_POP/SA_WorkingPlan.h
index e9944e5fea0..0ecf1137b54 100644
--- a/SA_POP/SA_WorkingPlan.h
+++ b/SA_POP/SA_WorkingPlan.h
@@ -48,6 +48,12 @@ namespace SA_POP {
      */
     virtual void set_commands (AddOpenThreatsCmd *add_threats_cmd);
 
+    /// Set goal.
+    /**	
+     * @param goal  Goal of this working plan.
+     */
+    virtual void set_goal (const SA_POP::Goal &goal);
+
     /// Get current plan.
     /**
      * @return  Reference to current plan.
diff --git a/SA_POP/SPACE.san.xml b/SA_POP/SPACE.san.xml
index 1f512f0b569..bc74682f96a 100644
--- a/SA_POP/SPACE.san.xml
+++ b/SA_POP/SPACE.san.xml
@@ -184,7 +184,7 @@
     <nodeID>1141</nodeID>
     <name>G1_Data_Recvd</name>
     <probTrue>0</probTrue>
-    <utility>10</utility>
+    <utility>0</utility>
     <kind />
     <attenFactor />
   </condNode>
@@ -192,7 +192,7 @@
     <nodeID>1142</nodeID>
     <name>G2_G3_Data_Recvd</name>
     <probTrue>0</probTrue>
-    <utility>10</utility>
+    <utility>0</utility>
     <kind />
     <attenFactor />
   </condNode>
diff --git a/SA_POP/WorkingPlan.h b/SA_POP/WorkingPlan.h
index 48fe05a0540..6921e94433a 100644
--- a/SA_POP/WorkingPlan.h
+++ b/SA_POP/WorkingPlan.h
@@ -41,6 +41,12 @@ namespace SA_POP {
     /// Destructor.
     virtual ~WorkingPlan (void) { };
 
+    /// Set goal.
+    /**	
+     * @param goal  Goal of this working plan.
+     */
+    virtual void set_goal (const SA_POP::Goal &goal) = 0;
+
     /// Get current plan.
     /**	
      * @return  Reference to current plan.
@@ -73,15 +79,15 @@ namespace SA_POP {
      *
      * @return  The task implementation id of this task instance.
      */
-	virtual TaskImplID get_task_impl_from_inst (TaskInstID inst_id) = 0;
+    virtual TaskImplID get_task_impl_from_inst (TaskInstID inst_id) = 0;
 
-	/// Get all current causal link threats.
+    /// Get all current causal link threats
     /**
      * @return  Set of all current causal link threats.
      */
     virtual CLThreatSet get_all_threats (void) = 0;
 
-    //Get the Start Window for a task instance
+    /// Get the Start Window for a task instance
     /**
      * @param task_inst The task instance of which the start window is returned
      *
@@ -89,7 +95,7 @@ namespace SA_POP {
      */
     virtual TimeWindow get_start_window(TaskInstID task_inst) = 0;
 
-    //Get the End Window for a task instance
+    /// Get the End Window for a task instance
     /**
      * @param task_inst The task instance of which the end window is returned
      *
@@ -97,7 +103,7 @@ namespace SA_POP {
      */
     virtual TimeWindow get_end_window(TaskInstID task_inst) = 0;
 
-    //Get the duration of a task instance
+    /// Get the duration of a task instance
     /**
      * @param task_inst The task instance of which the duration is returned
      *
@@ -105,20 +111,20 @@ namespace SA_POP {
      */
     virtual TimeValue get_duration(TaskInstID task_inst) = 0;
 
-     /// Get the Causal and Scheduling orderings to this task instance
+    /// Get the Causal and Scheduling orderings to this task instance
     /**
      * @param inst_id The task instance to which all orderings are required
      */
-  virtual TaskInstSet before_orderings (TaskInstID inst_id) = 0;
+    virtual TaskInstSet before_orderings (TaskInstID inst_id) = 0;
 
     /// Get the Causal and Scheduling orderings from this task instance
     /**
      * @param inst_id The task instance from which all orderings are required
      */
-  virtual TaskInstSet after_orderings (TaskInstID inst_id) = 0;
+    virtual TaskInstSet after_orderings (TaskInstID inst_id) = 0;
 
-	/// Get all the task instances
-	virtual TaskInstSet get_all_insts() = 0;
+    /// Get all the task instances
+    virtual TaskInstSet get_all_insts() = 0;
     
     /// Check if the instance id already exists and is being reused.
     /**
@@ -185,6 +191,9 @@ namespace SA_POP {
   protected:
     /// Pointer to Planner object.
     SA_POP::Planner *planner_;
+
+    /// Goal of this working plan.
+    SA_POP::Goal goal_;
   };
 
 };  /* SA_POP namespace */