Thu Jul 2 05:38:02 UTC 2009 Ben Podgursky <bpodgursky@gmail.com>

7 files changed, 66 insertions, 32 deletions

diff --git a/ChangeLog b/ChangeLog
index 0be39f7c072..f4163bb6a09 100644
--- a/ChangeLog
+++ b/ChangeLog
@@ -1,9 +1,20 @@
+Thu Jul  2 05:38:02 UTC 2009  Ben Podgursky  <bpodgursky@gmail.com>
+
+        * PlanCommands.h:
+        * SA_PlanCommands.h:
+        * SA_PlanCommands.cpp:
+        * SA_PlanStrategy.h:
+        * SA_PlanStrategy.cpp:
+        * SA_WorkingPlan.cpp:
+
+	Changed open condition resolution to resolve conditions for one task at a time.
+
 Wed Jul  1 22:13:25 UTC 2009  Daniel L.C. Mack  <daniel.l.mack@vanderbilt.edu>
 
         * SA_PlanStrategy.cpp:
         * SA_WorkingPlan.cpp:
-	Updated to allow for goal_task_inst to be part of threats. 
-	Small aesthetic changes
+        Updated to allow for goal_task_inst to be part of threats.
+        Small aesthetic changes
 
 Tue Jun 30 20:10:07 UTC 2009  Daniel L.C. Mack  <daniel.l.mack@vanderbilt.edu>
 
diff --git a/PlanCommands.h b/PlanCommands.h
index 3754ab5d4be..ab30a0f268f 100644
--- a/PlanCommands.h
+++ b/PlanCommands.h
@@ -194,6 +194,8 @@ namespace SA_POP {
      * @return  True if this command is using an already existing instance id.
      */
     virtual bool inst_exists (void) = 0;
+
+	virtual TaskInstSet get_satisfied_tasks(void) = 0;
   };
 
   /**
@@ -269,7 +271,7 @@ namespace SA_POP {
     /**
      * @param conds  Set of open conditions to remove.
      */
-    virtual void set_conds (const CondSet &conds) = 0;
+    virtual void set_conds (const CondSet &conds, const TaskInstSet &tasks) = 0;
   };
 
   /**
diff --git a/SA_PlanCommands.cpp b/SA_PlanCommands.cpp
index 8c0804f06b8..30dceb711e1 100644
--- a/SA_PlanCommands.cpp
+++ b/SA_PlanCommands.cpp
@@ -193,7 +193,6 @@ bool SA_AddTaskCmd::execute_next (void)
     isInitial = true;
   }
 
-
   this->working_plan_->execute (this);
   this->num_tries_++;
 
@@ -282,6 +281,10 @@ TaskInstID SA_AddTaskCmd::get_task_inst (void)
   return this->last_task_inst_;
 };
 
+TaskInstSet SA_AddTaskCmd::get_satisfied_tasks(void){
+  return this->task_insts_;
+}
+
 /// Check if the instance id used by the task of this command already exists.
 bool SA_AddTaskCmd::inst_exists (void)
 {
@@ -539,7 +542,7 @@ std::string SA_RemoveOpenCondsCmd::get_log_text (void)
 };
 
 // Set the open conditions to remove.
-void SA_RemoveOpenCondsCmd::set_conds (const CondSet &conds)
+void SA_RemoveOpenCondsCmd::set_conds (const CondSet &conds, const TaskInstSet &tasks)
 {
   if (!this->conds_.empty ())
     throw "SA_POP::SA_RemoveOpenCondsCmd::set_conds (): called while current condition set is not empty.";
@@ -548,6 +551,7 @@ void SA_RemoveOpenCondsCmd::set_conds (const CondSet &conds)
     throw "SA_POP::SA_RemoveOpenCondsCmd::set_conds (): called before last execution undone.";
 
   this->conds_ = conds;
+  this->tasks_ = tasks;
 };
 
 
diff --git a/SA_PlanCommands.h b/SA_PlanCommands.h
index e4024155afe..4eb39e15a96 100644
--- a/SA_PlanCommands.h
+++ b/SA_PlanCommands.h
@@ -248,6 +248,8 @@ namespace SA_POP {
      */
     virtual bool inst_exists (void);
 
+	virtual TaskInstSet get_satisfied_tasks(void);
+
   protected:
     /// WorkingPlan object that this command works on.
     SA_WorkingPlan *working_plan_;
@@ -478,7 +480,9 @@ namespace SA_POP {
     /**
      * @param conds  Set of open conditions to remove.
      */
-    virtual void set_conds (const CondSet &conds);
+    virtual void set_conds (const CondSet &conds, const TaskInstSet &tasks);
+
+
 
   protected:
     /// PlanStrategy object that this command works on.
@@ -487,6 +491,8 @@ namespace SA_POP {
     /// Set of conditions to remove from open conditions.
     CondSet conds_;
 
+	TaskInstSet tasks_;
+
     /// Map from removed conditions to (removed) task instances.
     OpenCondMap removed_;
   };
diff --git a/SA_PlanStrategy.cpp b/SA_PlanStrategy.cpp
index ea55dcdacc8..8c0643696db 100644
--- a/SA_PlanStrategy.cpp
+++ b/SA_PlanStrategy.cpp
@@ -142,6 +142,7 @@ bool SA_PlanStrategy::satisfy_open_conds (void)
   if (this->open_conds_.empty ())
 	  return this->planner_->full_sched();
 
+
   // Increment step counter.
   this->cur_step_++;
 
@@ -165,7 +166,7 @@ bool SA_PlanStrategy::satisfy_open_conds (void)
     this->cur_task_ = add_task_cmd->get_task ();
     this->cur_task_inst_ = add_task_cmd->get_task_inst ();
     // Remove open condition.
-    CommandID rmv_cond_cmd_id = this->rmv_open_cond (open_cond);
+	CommandID rmv_cond_cmd_id = this->rmv_open_cond (open_cond, add_task_cmd->get_satisfied_tasks());
 
     // Add preconditions of this task of we didn't reuse the task instance.
 	    CommandID add_preconds_cmd_id;
@@ -181,7 +182,6 @@ bool SA_PlanStrategy::satisfy_open_conds (void)
     this->cur_seq_num_ = 1;
 
 
-
     //Deal with threats
 
     //Actually build the list of threats
@@ -322,8 +322,7 @@ bool SA_PlanStrategy::get_next_threat_resolution(){
         resolve_threat_cmd->set_id (this->get_next_cmd_id ());
         resolve_threat_cmd->set_threat (threat);
         this->planner_->add_command (resolve_threat_cmd);
-        
-   //     if(this->planner_->try_next (resolve_threat_cmd->get_id ()))
+       
 
 
 
@@ -338,23 +337,6 @@ bool SA_PlanStrategy::get_next_threat_resolution(){
          this->cur_decision_pt_ = SA_PlanStrategy::THREAT_DECISION;
          this->planner_->undo_command (resolve_threat_cmd->get_id ());
 
-  //      resolve_threat_cmd =
- //         static_cast<ResolveCLThreatCmd *> (this->resolve_threat_cmd_->clone ());
-  //      resolve_threat_cmd->set_id (this->get_next_cmd_id ());
-  //      resolve_threat_cmd->set_threat (threat);
- //       this->planner_->add_command (resolve_threat_cmd);
-
-  //      resolve_threat_cmd->choices = 1;
-
-        //Second time will try reverse direection
- //       if(this->planner_->try_next (resolve_threat_cmd->get_id ()))
- //         if (this->get_next_threat_resolution ())
- //           return true;
-
-
-        // Undo threat resolution.
- //       this->planner_->undo_command (resolve_threat_cmd->get_id ());
-
 
 
         this->open_threats_.insert(threat);
@@ -433,7 +415,9 @@ void SA_PlanStrategy::undo (SA_AddOpenCondsCmd *cmd)
 void SA_PlanStrategy::execute (SA_RemoveOpenCondsCmd *cmd)
 {
   // Remove open conditions, keeping track of removed cond->inst mapping.
-  for (CondSet::iterator cond_iter = cmd->conds_.begin ();
+
+	/*
+	for (CondSet::iterator cond_iter = cmd->conds_.begin ();
     cond_iter != cmd->conds_.end (); cond_iter++)
   {
     for (OpenCondMap::iterator open_iter =
@@ -447,6 +431,33 @@ void SA_PlanStrategy::execute (SA_RemoveOpenCondsCmd *cmd)
       this->open_conds_.erase (prev_iter);
     }
   }
+  */
+
+	for(CondSet::iterator cond_it = cmd->conds_.begin(); cond_it != cmd->conds_.end();
+		cond_it++){
+			for(TaskInstSet::iterator task_it = cmd->tasks_.begin(); 
+			task_it != cmd->tasks_.end(); task_it++){
+
+			  for(OpenCondMap::iterator o_it = open_conds_.lower_bound(*cond_it);
+				  o_it != open_conds_.upper_bound(*cond_it); ){
+			
+				  if(o_it->second == *task_it){
+
+					OpenCondMap::iterator prev_iter = o_it;
+					o_it++;
+					cmd->removed_.insert (std::make_pair
+						(*cond_it, *task_it));
+					
+					open_conds_.erase(prev_iter);
+				  }else{
+					  o_it++;
+				  }
+				  
+			  }
+		}
+	}
+
+
 };
 
 // Undo a command to remove open conditions from planning.
@@ -535,6 +546,7 @@ AddTaskCmd *SA_PlanStrategy::satisfy_cond (Condition open_cond)
     iter != this->open_conds_.upper_bound (open_cond); iter++)
   {
     inst_set.insert (iter->second);
+	break;
   }
   add_task_cmd->set_causal_info (open_cond, inst_set);
 
@@ -563,14 +575,14 @@ CommandID SA_PlanStrategy::add_open_conds (const CondSet &open_conds,
 };
 
 // Remove open condition.
-CommandID SA_PlanStrategy::rmv_open_cond (Condition open_cond)
+CommandID SA_PlanStrategy::rmv_open_cond (Condition open_cond, TaskInstSet tasks)
 {
   // Get commmand to remove open conditions.
   RemoveOpenCondsCmd *rmv_conds_cmd =
     static_cast<RemoveOpenCondsCmd *> (this->rmv_conds_cmd_->clone ());
   CondSet conds;
   conds.insert (open_cond);
-  rmv_conds_cmd->set_conds (conds);
+  rmv_conds_cmd->set_conds (conds, tasks);
   CommandID cmd_id = this->get_next_cmd_id ();
   rmv_conds_cmd->set_id (cmd_id);
 
diff --git a/SA_PlanStrategy.h b/SA_PlanStrategy.h
index 1cab88cf5e7..9887e6761b8 100644
--- a/SA_PlanStrategy.h
+++ b/SA_PlanStrategy.h
@@ -325,7 +325,7 @@ namespace SA_POP {
      *
      * @return  ID of command passed to planner to remove open condition.
      */
-    virtual CommandID rmv_open_cond (Condition open_cond);
+    virtual CommandID rmv_open_cond (Condition open_cond, TaskInstSet tasks);
 
     /// Add open causal link threats.
     /**
diff --git a/SA_WorkingPlan.cpp b/SA_WorkingPlan.cpp
index 5bb69a959aa..816eaa40831 100644
--- a/SA_WorkingPlan.cpp
+++ b/SA_WorkingPlan.cpp
@@ -1153,7 +1153,6 @@ bool SA_WorkingPlan::execute (SA_ResolveCLThreatCmd * cmd)
  // cl.first = first_task_inst;
  // cl.second = second_task_inst;
 
- // this->causal_links_.insert(std::pair<Condition, CausalLink>(condition, cl));
   this->ordering_links.insert(std::pair<TaskInstID, TaskInstID>(first_task_inst, second_task_inst));
   this->reverse_ordering_links.insert(std::pair<TaskInstID, TaskInstID>(second_task_inst, first_task_inst));