[Personalrobots-commit] SF.net SVN: personalrobots:[12107] pkg/trunk

[<mc...@us...>]

Revision: 12107
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=12107&view=rev
Author:   mcgann
Date:     2009-03-04 14:43:36 +0000 (Wed, 04 Mar 2009)

Log Message:
-----------
Updates for new Highlevel Controller Message

Modified Paths:
--------------
    pkg/trunk/highlevel/executive_trex/executive_trex_pr2/include/ROSControllerAdapter.hh
    pkg/trunk/highlevel/executive_trex/executive_trex_pr2/src/ROSAdapter.cc
    pkg/trunk/highlevel/highlevel_controllers/include/highlevel_controllers/highlevel_controller.hh
    pkg/trunk/highlevel/highlevel_controllers/msg/RechargeState.msg
    pkg/trunk/highlevel/highlevel_controllers/src/move_arm.cpp
    pkg/trunk/highlevel/highlevel_controllers/src/recharge_controller.cpp
    pkg/trunk/mapping/door_handle_detector/test/open_door_executive_test.cpp
    pkg/trunk/nav/wavefront_player/cli.cpp
    pkg/trunk/nav/wavefront_player/wavefront_player.cc
    pkg/trunk/pr2_msgs/msg/MoveArmState.msg
    pkg/trunk/pr2_msgs/msg/MoveEndEffectorState.msg
    pkg/trunk/prcore/robot_msgs/msg/Planner2DState.msg

Added Paths:
-----------
    pkg/trunk/prcore/robot_msgs/msg/ControllerStatus.msg

Modified: pkg/trunk/highlevel/executive_trex/executive_trex_pr2/include/ROSControllerAdapter.hh
===================================================================
--- pkg/trunk/highlevel/executive_trex/executive_trex_pr2/include/ROSControllerAdapter.hh	2009-03-04 12:37:20 UTC (rev 12106)
+++ pkg/trunk/highlevel/executive_trex/executive_trex_pr2/include/ROSControllerAdapter.hh	2009-03-04 14:43:36 UTC (rev 12107)
@@ -4,25 +4,16 @@
 #include "ROSAdapter.hh"
 #include "Token.hh"
 #include "TokenVariable.hh"
-#include "BoolDomain.hh"
+#include "SymbolDomain.hh"
 
 namespace TREX {
 
-  /**
-   * @brief The last reported state of the controller
-   */
-  enum ControllerState {
-    UNDEFINED = 0,
-    INACTIVE,
-    ACTIVE
-  };
-
   template <class S, class G> class ROSControllerAdapter: public ROSAdapter {
   public:
 
     ROSControllerAdapter(const LabelStr& agentName, const TiXmlElement& configData)
       : ROSAdapter(agentName, configData, 1), inactivePredicate(timelineType + ".Inactive"), activePredicate(timelineType + ".Active"),
-	state(UNDEFINED), lastPublished(-1), lastUpdated(0), goalTopic(extractData(configData, "goalTopic").toString()) {}
+	is_active(false), lastPublished(-1), lastUpdated(0), goalTopic(extractData(configData, "goalTopic").toString()) {}
 
     virtual ~ROSControllerAdapter(){}
 
@@ -39,28 +30,31 @@
 
       // If we have already changed the state in this tick, we do not want to over-ride that. This will ensure we do not miss a state change
       // where the goal to move is accompished almost instantly, for example if the robot is already at the goal.
-      if(lastUpdated == getCurrentTick() && state != UNDEFINED)
+      if(lastUpdated == getCurrentTick())
 	return;
 
-      if(stateMsg.status == stateMsg.ACTIVE && state != ACTIVE){
-	state = ACTIVE;
-	lastUpdated = getCurrentTick();
-	ROS_DEBUG("Received transition to ACTIVE");
+      if( (stateMsg.status.value == stateMsg.status.ACTIVE) && !is_active){
+	ROS_DEBUG("[%s][%d]Received transition to ACTIVE %d", 
+		  nameString().c_str(), getCurrentTick(), stateMsg.status.value);
       }
-      else if(stateMsg.status != stateMsg.ACTIVE && state != INACTIVE){
-	state = INACTIVE;
-	lastUpdated = getCurrentTick();
-	ROS_DEBUG("Received transition to INACTIVE");
+      else if((stateMsg.status.value != stateMsg.status.ACTIVE) && is_active){
+	ROS_DEBUG("[%s][%d]Received transition to INACTIVE", nameString().c_str(), getCurrentTick());
       }
+
+      lastUpdated = getCurrentTick();
     }
 
     void registerSubscribers() {
-      debugMsg("ROS:", "Registering subscriber for " << timelineName << " on topic:" << stateTopic);
+      ROS_INFO("[%s][%d]Registering subscriber for %s on topic: %s", 
+		nameString().c_str(), getCurrentTick(), timelineName.c_str(), stateTopic.c_str());
+
       m_node->registerSubscriber(stateTopic, stateMsg, &TREX::ROSControllerAdapter<S, G>::handleCallback, this, QUEUE_MAX());
     }
 
     void registerPublishers(){
-      debugMsg("ROS:", "Registering publisher for " << timelineName << " on topic:" << goalTopic);
+      ROS_INFO("[%s][%d]Registering publisher for %s on topic: %s", 
+	       nameString().c_str(), getCurrentTick(), timelineName.c_str(), goalTopic.c_str());
+
       m_node->registerPublisher<G>(goalTopic, QUEUE_MAX());
     }
 
@@ -69,26 +63,27 @@
       if(((int) lastUpdated) == lastPublished)
 	return NULL;
 
-      // We shuold never be getting an observation when in an undefined state. If we are, it means we failed to
-      // initialize, which will likely be a message subscription error or an absence of an expected publisher
-      // to initialize state.
-      if(state == UNDEFINED){
-	ROS_DEBUG("ROSControllerAdapter <%s> failed to get an observation with no initial state set yet", timelineName.c_str());
-	throw "ROSControllerAdapter: Tried to get an observation on with no initial state set yet.";
-      }
-
       ObservationByValue* obs = NULL;
 
       stateMsg.lock();
-      if(state == INACTIVE){
+
+      ROS_DEBUG("[%s][%d]Checking for new observations. Currently we are %s. Latest observation is %s (%d)",
+		nameString().c_str(),
+		getCurrentTick(),
+		(is_active ? "ACTIVE" : "INACTIVE"),
+		(stateMsg.status.value == stateMsg.status.ACTIVE ? "ACTIVE" : "INACTIVE"),
+		stateMsg.status.value);
+
+      if(stateMsg.status.value != stateMsg.status.ACTIVE && (is_active || (getCurrentTick() == 0))){
 	obs = new ObservationByValue(timelineName, inactivePredicate);
 	fillInactiveObservationParameters(obs);
-	obs->push_back("aborted", new BoolDomain(stateMsg.status == stateMsg.ABORTED));
-	obs->push_back("preempted", new BoolDomain(stateMsg.status == stateMsg.PREEMPTED));
+	obs->push_back("status", getResultStatus(stateMsg).copy());
+	is_active = false;
       }
-      else {
+      else if(stateMsg.status.value == stateMsg.status.ACTIVE && !is_active){
 	obs = new ObservationByValue(timelineName, activePredicate);
 	fillActiveObservationParameters(obs);
+	is_active = true;
       }
       stateMsg.unlock();
 
@@ -96,12 +91,26 @@
       return obs;
     }
 
+    const SymbolDomain& getResultStatus(const S& stateMsg){
+      static std::vector<EUROPA::SymbolDomain> RESULT_STATES;
+      if(RESULT_STATES.empty()){
+	RESULT_STATES.push_back(SymbolDomain(LabelStr("UNDEFINED"), "ResultStatus"));
+	RESULT_STATES.push_back(SymbolDomain(LabelStr("SUCCESS"), "ResultStatus"));
+	RESULT_STATES.push_back(SymbolDomain(LabelStr("ABORTED"), "ResultStatus"));
+	RESULT_STATES.push_back(SymbolDomain(LabelStr("PREEMPTED"), "ResultStatus"));
+	RESULT_STATES.push_back(SymbolDomain(LabelStr("ACTIVE"), "ResultStatus"));
+      }
+
+      return RESULT_STATES[stateMsg.status.value];
+    }
+
     /**
      * The goal can be enabled or disabled.
      * The predicate can be active or inactive
      */
     bool dispatchRequest(const TokenId& goal, bool enabled){
-      ROS_DEBUG("Received dispatch request for %s",goal->toString().c_str());
+      ROS_DEBUG("[%s][%d]Received dispatch request for %s",
+		nameString().c_str(), getCurrentTick(), goal->toString().c_str());
 
       bool enableController = enabled;
 
@@ -124,7 +133,10 @@
 	  goalMsg.timeout = dom.getSingletonValue();
 	}
       }
-      ROS_DEBUG("[%d}Dispatching %s", getCurrentTick(), goal->toString().c_str());
+
+      ROS_DEBUG("[%s][%d]%s goal %s", 
+		nameString().c_str(), getCurrentTick(), (enableController ? "Dispatching" : "Recalling"), goal->toString().c_str());
+
       m_node->publishMsg<G>(goalTopic, goalMsg);
 
       // Ensure pre-emption is controllable. We do not want to rely on the asynchronous
@@ -132,7 +144,7 @@
       // synchronizing
       if(!enableController){
 	stateMsg.lock();
-	stateMsg.status = stateMsg.PREEMPTED;
+	stateMsg.status.value = stateMsg.status.PREEMPTED;
 	handleCallback();
 	stateMsg.unlock();
       }
@@ -149,7 +161,7 @@
   private:
     const LabelStr inactivePredicate;
     const LabelStr activePredicate;
-    ControllerState state;
+    bool is_active;
     int lastPublished;
     TICK lastUpdated;
     const std::string goalTopic;

Modified: pkg/trunk/highlevel/executive_trex/executive_trex_pr2/src/ROSAdapter.cc
===================================================================
--- pkg/trunk/highlevel/executive_trex/executive_trex_pr2/src/ROSAdapter.cc	2009-03-04 12:37:20 UTC (rev 12106)
+++ pkg/trunk/highlevel/executive_trex/executive_trex_pr2/src/ROSAdapter.cc	2009-03-04 14:43:36 UTC (rev 12107)
@@ -40,13 +40,13 @@
 
     registerSubscribers();
 
-    // Wait till we get a message before starting the agent
+    // Wait till we are initialized before moving ahead
     while(!isInitialized() && ros::Node::instance()->ok()){
-      std::cout << "Waiting to connect for " << timelineName << ". If this is taking too long then the expected message is not being published." << std::endl;
+      ROS_INFO("Waiting to connect for %s. If this is taking too long then the expected message is not being published.", timelineName.c_str());
       sleep(1);
     }
 
-    std::cout << "Connection established for " << timelineName << std::endl;
+    ROS_INFO("Connection established for %s", timelineName.c_str());
   }
 
   ROSAdapter::~ROSAdapter() {

Modified: pkg/trunk/highlevel/highlevel_controllers/include/highlevel_controllers/highlevel_controller.hh
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/include/highlevel_controllers/highlevel_controller.hh	2009-03-04 12:37:20 UTC (rev 12106)
+++ pkg/trunk/highlevel/highlevel_controllers/include/highlevel_controllers/highlevel_controller.hh	2009-03-04 14:43:36 UTC (rev 12107)
@@ -48,14 +48,10 @@
  * @brief Abstract base class for a high level controller node which is parameterized by the state update
  * and goal messages exchanged with a client. The controller utilizes a pattern where the controller is tasked with
  * goals, and can transition between an active state (when pursuing a goal), and an inactive state when is its effectively
- * idle and should not be imposing any control. A high level control must also handle goal recalls.
+ * idle and should not be imposing any control. A high level control must also handle goal recalls, which is termed 'preemption'
  */
 template <class S, class G> class HighlevelController {
 public:
-  enum State {
-    INACTIVE = 0,
-    ACTIVE
-  };
 
   /**
    * @brief Used to set queue max parameter for subscribe and advertise of ROS topics
@@ -71,7 +67,7 @@
   HighlevelController(const std::string& nodeName, const std::string& _stateTopic,  const std::string& _goalTopic): 
     initialized(false), terminated(false), stateTopic(_stateTopic), 
     goalTopic(_goalTopic), controllerCycleTime_(0.1), plannerCycleTime_(0.0), plannerThread_(NULL), 
-    timeout(0, 0), done_(false), valid_(false) {
+    timeout(0, 0), valid_(false) {
 
     // Obtain the control frequency for this node
     double controller_frequency(10);
@@ -99,6 +95,8 @@
     ros::Node::instance()->subscribe("highlevel_controllers/shutdown", shutdownMsg_, &HighlevelController<S, G>::shutdownCallback, this, 1);
 
     // Initially inactive
+    this->stateMsg.status.value = this->stateMsg.status.UNDEFINED;
+
     deactivate();
 
     // Start planner loop on a separate thread
@@ -110,21 +108,7 @@
     delete plannerThread_;
   }
 
-  /**
-   * @brief Test if the node has received required inputs allowing it to commence business as usual.
-   * @see initialize()
-   */
-  bool isInitialized() const {
-    return initialized;
-  }
 
-  void terminate() {
-    terminated = true;
-  }
-
-  bool isTerminated() const {
-    return terminated;
-  }
   /**
    * @brief The main run loop of the controller
    */
@@ -144,53 +128,25 @@
   }
 
   /**
-   * @brief Runs the planning loop. If the node is not initialized or if inactive, then it will do nothing. Otherwise
-   * it will call the planner with the given timeout.
+   * @brief Test if the HLC has received required inputs allowing it to commence business as usual.
+   * @see initialize()
    */
-  void plannerLoop(){
-    ros::Time lastPlan = ros::Time::now();
+  bool isInitialized() const {
+    return initialized;
+  }
 
-    while(ros::Node::instance()->ok() && !isTerminated()){
-      ros::Time curr = ros::Time::now();
+  /**
+   * @brief Call this to permanently decommision the controller
+   */
+  void terminate() {
+    terminated = true;
+  }
 
-      // Check for bogus time value and update to correct. This can happen for example where the node starts up
-      // and uses system time but then switches to listen to a time message based on simulated time. Just makes the code robust
-      // to such startup errors. If we do find some other timing anomaly it will correct by recomputing the last planning time.
-      // This has the effect of resetting the timeout monotor but that should only introduce a minor delay
-      if(curr < lastPlan)
-	lastPlan = curr;
-
-      if(isInitialized() && isActive()){
-
-	// If the plannerCycleTime is 0 then we only call the planner when we need to
-	if(plannerCycleTime_ != 0 || !isValid()){
-	  setValid(makePlan());
-	  if(!isValid()){
-	    // Could use a refined locking scheme but for now do not want to delegate that to a derived class
-	    lock();	 
-  
-	    ros::Duration elapsed_time = ros::Time::now() - lastPlan;
-	    ROS_DEBUG("Last current value at %f seconds:", curr.toSec());
-	    ROS_DEBUG("Last plan at %f seconds:", lastPlan.toSec());
-	    ROS_DEBUG("Elapsed time is %f seconds:", elapsed_time.toSec());
-	    if ((elapsed_time > timeout) && timeout.toSec() != 0.0) {
-	      this->stateMsg.status = this->stateMsg.ABORTED;
-	      ROS_INFO("Controller timed out.");
-	      deactivate();
-	    }
-	    handlePlanningFailure();
-	    unlock();	    
-	  } else {
-	    lastPlan = ros::Time::now();
-	  }
-	}
-      }
-
-      if(plannerCycleTime_ >= 0)
-	sleep(curr, std::max(plannerCycleTime_, controllerCycleTime_));
-      else
-	sleep(curr, curr.toSec() + 0.001);
-    }
+  /**
+   * @brief Test if the HLC has been terminated (decommissioned permanently)
+   */
+  bool isTerminated() const {
+    return terminated;
   }
 
 protected:
@@ -199,7 +155,7 @@
    * @brief Accessor for state of the controller
    */
   bool isActive() {
-    return this->state == ACTIVE;
+    return this->stateMsg.status.value == this->stateMsg.status.ACTIVE;
   }
 
   /**
@@ -212,46 +168,27 @@
    * @brief Access aborted state of the planner.
    */
   bool isAborted() {
-    return this->stateMsg.status == this->stateMsg.ABORTED;
+    return this->stateMsg.status.value == this->stateMsg.status.ABORTED;
   }
 
   /**
    * @brief Access preempted state of the planner.
    */
   bool isPreempted() {
-    return this->stateMsg.status == this->stateMsg.PREEMPTED;
+    return this->stateMsg.status.value == this->stateMsg.status.PREEMPTED;
   }
 
   /**
-   * @brief Activation of the controller will set the state, the stateMsg but indicate that the
-   * goal has not yet been accomplished and that no plan has been constructed yet.
+   * @brief Abort the controller. This is the only state transition that can be explicitly initiated
+   * by a derived class. Otherwise the state transitions are governed by implementations of methods
+   * for planning and dispatching commands, and checking of a goal has been reached.
    */
-  void activate(){
-    ROS_INFO("Activating controller with timeout of %f seconds\n", this->goalMsg.timeout);
-
-    this->state = ACTIVE;
-    this->stateMsg.status = this->stateMsg.ACTIVE;
-    done_ = 0;
-
-    handleActivation();
+  void abort(){
+    ROS_INFO("Aborting controller\n");
+    this->stateMsg.status.value = this->stateMsg.status.ABORTED;
   }
 
   /**
-   * @brief Deactivation of the controller will set the state to inactive, and clear the valid flag.
-   */
-  void deactivate(){
-    ROS_INFO("Deactivating controller\n");
-
-    this->state = INACTIVE;
-    if (this->stateMsg.status == this->stateMsg.ACTIVE) {
-      this->stateMsg.status = this->stateMsg.INACTIVE;
-    }
-    valid_ = 0;
-
-    handleDeactivation();
-  }
-
-  /**
    * @brief Marks the node as initialized. Shoud be called by subclass when expected inbound messages
    * are received to make sure it only publishes meaningful states
    */
@@ -259,7 +196,6 @@
     initialized = true;
   }
 
-
   /**
    * @brief Subclass will implement this method to update goal data based on new values in goalMsg. Derived class should
    * be sure to lock and unlock when accessing member variables
@@ -345,18 +281,56 @@
   G goalMsg; /*!< Message populated by callback */
   S stateMsg; /*!< Message published. Will be populated in the control loop */
 
-  /**
-   * @brief Setter for state msg done flag
-   */
-  void setDone(bool isDone){
-    done_ = isDone;
-  }
 
+private:
+
   /**
-   * @brief Get the done flag
+   * @brief Runs the planning loop. If the node is not initialized or if inactive, then it will do nothing. Otherwise
+   * it will call the planner with the given timeout.
    */
-  bool getDone() {
-    return done_;
+  void plannerLoop(){
+    ros::Time lastPlan = ros::Time::now();
+
+    while(ros::Node::instance()->ok() && !isTerminated()){
+      ros::Time curr = ros::Time::now();
+
+      // Check for bogus time value and update to correct. This can happen for example where the node starts up
+      // and uses system time but then switches to listen to a time message based on simulated time. Just makes the code robust
+      // to such startup errors. If we do find some other timing anomaly it will correct by recomputing the last planning time.
+      // This has the effect of resetting the timeout monotor but that should only introduce a minor delay
+      if(curr < lastPlan)
+	lastPlan = curr;
+
+      if(isInitialized() && isActive()){
+
+	// If the plannerCycleTime is 0 then we only call the planner when we need to
+	if(plannerCycleTime_ != 0 || !isValid()){
+	  setValid(makePlan());
+	  if(!isValid()){
+	    // Could use a refined locking scheme but for now do not want to delegate that to a derived class
+	    lock();	 
+  
+	    ros::Duration elapsed_time = ros::Time::now() - lastPlan;
+	    ROS_DEBUG("Last current value at %f seconds:", curr.toSec());
+	    ROS_DEBUG("Last plan at %f seconds:", lastPlan.toSec());
+	    ROS_DEBUG("Elapsed time is %f seconds:", elapsed_time.toSec());
+
+	    if ((elapsed_time > timeout) && timeout.toSec() != 0.0)
+	      abort();
+
+	    handlePlanningFailure();
+	    unlock();    
+	  } else {
+	    lastPlan = ros::Time::now();
+	  }
+	}
+      }
+
+      if(plannerCycleTime_ >= 0)
+	sleep(curr, std::max(plannerCycleTime_, controllerCycleTime_));
+      else
+	sleep(curr, curr.toSec() + 0.001);
+    }
   }
 
   /**
@@ -366,9 +340,33 @@
     valid_ = isValid;
   }
 
+  /**
+   * @brief Activation of the controller will set the state, the stateMsg but indicate that the
+   * goal has not yet been accomplished and that no plan has been constructed yet.
+   */
+  void activate(){
+    ROS_INFO("Activating controller with timeout of %f seconds\n", this->goalMsg.timeout);
 
-private:
+    valid_ = 0;
+    this->stateMsg.status.value = this->stateMsg.status.ACTIVE;
 
+    handleActivation();
+  }
+
+  void preempt(){
+    ROS_INFO("Controller preempted.");
+    this->stateMsg.status.value = this->stateMsg.status.PREEMPTED;
+    deactivate();
+  }
+
+  /**
+   * @brief Deactivation of the controller will set the state to inactive, and clear the valid flag.
+   */
+  void deactivate(){
+    ROS_INFO("Deactivating controller");
+    handleDeactivation();
+  }
+
   void goalCallback(){
     if (goalMsg.timeout < 0) {
       ROS_ERROR("Controller was given negative timeout, setting to zero.");
@@ -383,17 +381,16 @@
 
     lock();
 
-    if(state == INACTIVE && goalMsg.enable){
+    if(!isActive() && goalMsg.enable){
       activate();
     }
-    else if(state == ACTIVE){
-      ROS_INFO("Controller preempted.");
-      this->stateMsg.status = this->stateMsg.PREEMPTED;
-      deactivate();
+    else if(isActive()){
+      preempt();
 
       // If we are active, and this is a goal, publish the state message and activate. This allows us
       // to over-ride a new goal, but still forces the transition between active and inactive states
       if(goalMsg.enable){
+	ROS_DEBUG("Publishing state %d", stateMsg.status.value);
 	ros::Node::instance()->publish(stateTopic, stateMsg);
 	activate();
       }
@@ -402,7 +399,6 @@
     // Call to allow derived class to update goal member variables
     updateGoalMsg();
 
-
     unlock();
   }
 
@@ -454,14 +450,15 @@
     // when the planner invalidates the plan, which can happen since planning is interleaved.
     if(isActive()){
       if(isValid() && goalReached()){
-	  setDone(true);
-	  deactivate();
-	}
-	else {
-	  // Dispatch plans to accomplish goal, according to the plan. If there is a failure in 
-	  // dispatching, it should return false, which will force re-planning
-	  setValid(dispatchCommands());
-	}
+	ROS_INFO("Success! Goal achieved :)\n");
+	this->stateMsg.status.value = this->stateMsg.status.SUCCESS;
+	deactivate();
+      }
+      else {
+	// Dispatch plans to accomplish goal, according to the plan. If there is a failure in 
+	// dispatching, it should return false, which will force re-planning
+	setValid(dispatchCommands());
+      }
     }
 
     unlock();
@@ -472,14 +469,12 @@
   bool terminated; /*!< Marks if the node has been terminated. */
   const std::string stateTopic; /*!< The topic on which state updates are published */
   const std::string goalTopic; /*!< The topic on which it subscribes for goal requests and recalls */
-  State state; /*!< Tracks the overall state of the controller */
   double controllerCycleTime_; /*!< The duration for each control cycle */
   double plannerCycleTime_; /*!< The duration for each planner cycle. */
   boost::recursive_mutex lock_; /*!< Lock for access to class members in callbacks */
   boost::thread* plannerThread_; /*!< Thread running the planner loop */
   highlevel_controllers::Ping shutdownMsg_; /*!< For receiving shutdown from executive */
   ros::Duration timeout; /*< The time limit for planning failure. */
-  bool done_; /*< True if the action is done */
   bool valid_; /*< True if it is valid */
 };
 

Modified: pkg/trunk/highlevel/highlevel_controllers/msg/RechargeState.msg
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/msg/RechargeState.msg	2009-03-04 12:37:20 UTC (rev 12106)
+++ pkg/trunk/highlevel/highlevel_controllers/msg/RechargeState.msg	2009-03-04 14:43:36 UTC (rev 12107)
@@ -1,20 +1,6 @@
 Header header
+robot_msgs/ControllerStatus status
 
-#Status options
-byte INACTIVE=0
-byte ACTIVE=1
-byte ABORTED=2
-byte PREEMPTED=3
-
-# Status of the controller = {INACTIVE, ACTIVE, ABORTED, PREEMPTED}
-byte status
-
-#Id of the goal
-int32 goal_id
-
-#Comment for debug
-string comment
-
 # Actual recharge level
 float32 recharge_level
 

Modified: pkg/trunk/highlevel/highlevel_controllers/src/move_arm.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/move_arm.cpp	2009-03-04 12:37:20 UTC (rev 12106)
+++ pkg/trunk/highlevel/highlevel_controllers/src/move_arm.cpp	2009-03-04 14:43:36 UTC (rev 12107)
@@ -133,9 +133,7 @@
 
   int                              plan_id_;
   int                              traj_id_;
-  bool                             new_goal_;
   bool                             new_trajectory_;
-  bool                             plan_valid_;
   robot_msgs::KinematicPath        current_trajectory_;
   double                           angle_tolerance_;
 
@@ -175,8 +173,7 @@
     controller_topic_(side + "_arm_trajectory_command"),
     controller_name_(side + "_arm_trajectory_controller"),
     robot_model_(NULL), plan_id_(-1), traj_id_(-1), 
-    new_goal_(false), new_trajectory_(false), 
-    plan_valid_(false)
+    new_trajectory_(false)
 {
   // Adjust logging level
   log4cxx::LoggerPtr my_logger = log4cxx::Logger::getLogger(ROSCONSOLE_DEFAULT_NAME);
@@ -255,7 +252,6 @@
   lock();
   stateMsg.goal = goalMsg.goal_configuration;
   ROS_DEBUG("Received new goal");
-  new_goal_ = true;
   unlock();
 }
 
@@ -333,13 +329,15 @@
 
 bool MoveArm::makePlan()
 {
-  if(!new_goal_)
+  // Only want to re-plan if we have not successfully obtained a plan already. Note that this is actually covered in
+  // how the node is configured since it is defined by the timiong of the plan update loop. This node conflates global planning and incremental
+  // replanning.
+  if(isValid())
     return true;
 
-  new_goal_ = false;
-
   ROS_DEBUG("Starting to plan...");
   
+  // CMG: Does this really need to be locked throughout? Just wondering.
   goalMsg.lock();
 
   robot_srvs::KinematicReplanState::Request  req;
@@ -429,28 +427,33 @@
 
   //req.params.volume* are left empty, because we're not planning for the base.
 
-  bool ret = ros::service::call("replan_kinematic_path_state", req, res);
-  if (!ret)
-  {
-    ROS_ERROR("Service call on plan_kinematic_path_state failed");
-    plan_id_ = -1;
-  }
-  else
-  {
+  if(ros::service::call("replan_kinematic_path_state", req, res)){
     plan_id_ = res.id;
     ROS_DEBUG("Requested plan, got id %d\n", plan_id_);
     plan_status_.lock();
     plan_status_.unlock();
   }
+  else {
+    ROS_ERROR("Service call on plan_kinematic_path_state failed");
+    plan_id_ = -1;
+  }
 
   goalMsg.unlock();
 
-  return true;
+  return plan_id_ != -1;
 }
 
 bool MoveArm::goalReached()
 {
-  return getDone();
+  plan_status_.lock();
+  // Should never get to call this method if the plan is invalid. See HighlevelController::doOneCycle() for details
+  ROS_ASSERT(plan_status_.id != -1);
+  bool result = plan_status_.done || (!new_trajectory_ && traj_id_ >= 0 && isTrajectoryDone());
+  ROS_DEBUG("Execution is complete");
+  plan_id_ = -1;
+  traj_id_ = -1;
+  plan_status_.unlock();
+  return result;
 }
 
 bool MoveArm::isTrajectoryDone()
@@ -508,36 +511,27 @@
   return done;
 }
 
+/**
+ * @note This method will not be called if we are done.
+ */
 bool MoveArm::dispatchCommands()
 {
-  // Force replanning if we have a new goal
-  if(new_goal_)
-    return false;
+  bool ok(true);
 
   plan_status_.lock();
 
   // HACK: sometimes we get the status for the new plan before we receive
   // the response to the service call that gives us the new plan id.  It
   // happens that the planner increments the id by 1 with each request.
+  // CMG: Since this cannot be called unless we completed the service call - do we need this?
   if(((plan_status_.id != plan_id_) &&
       (plan_status_.id != plan_id_ + 1)) ||
      (traj_id_ < 0 && !new_trajectory_))
   {
     // NOOP
+    // CMG: Is this a NOOP? We are basically continuing our prior command - no?
     ROS_DEBUG("Idle");
   }
-  else if(plan_status_.done || (traj_id_ >= 0 && !new_trajectory_))
-  {
-    if(isTrajectoryDone())
-    {
-      ROS_DEBUG("Plan completed.");
-      plan_id_ = -1;
-      traj_id_ = -1;
-      ROS_DEBUG("Execution is complete");
-      stateMsg.status = pr2_msgs::MoveArmState::INACTIVE;
-      plan_valid_ = true;
-    }
-  }
   else if(plan_status_.valid && 
           !plan_status_.unsafe &&
           (plan_status_.distance < 0.1) &&
@@ -545,18 +539,18 @@
   {
     ROS_DEBUG("sending new trajectory");
     sendArmCommand(current_trajectory_, kinematic_model_);
-    plan_valid_ = true;
     new_trajectory_ = false;
   }
   else
   {
     ROS_DEBUG("Plan invalid or unsafe");
     stopArm();
-    plan_valid_ = false;
+    ok = false;
   }
+
   plan_status_.unlock();
 
-  return plan_valid_;
+  return ok;
 }
 
 void MoveArm::printKinematicPath(robot_msgs::KinematicPath &path)

Modified: pkg/trunk/highlevel/highlevel_controllers/src/recharge_controller.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/recharge_controller.cpp	2009-03-04 12:37:20 UTC (rev 12106)
+++ pkg/trunk/highlevel/highlevel_controllers/src/recharge_controller.cpp	2009-03-04 14:43:36 UTC (rev 12107)
@@ -164,9 +164,9 @@
     // Set to initalized (benign if already true)
     initialize();
 
-    // If the power consumption is positive, then energy is flowing into the battery, so it must be plugged in.
-    if(connected() && !isActive())
-      activate();
+    // If not activated, do nothing
+    if(!isActive())
+      return;
 
     // If connected, we can reset notification to plug in. We also clear it to be releasable, meaning
     if(connected()){

Modified: pkg/trunk/mapping/door_handle_detector/test/open_door_executive_test.cpp
===================================================================
--- pkg/trunk/mapping/door_handle_detector/test/open_door_executive_test.cpp	2009-03-04 12:37:20 UTC (rev 12106)
+++ pkg/trunk/mapping/door_handle_detector/test/open_door_executive_test.cpp	2009-03-04 14:43:36 UTC (rev 12107)
@@ -455,10 +455,10 @@
   
   void plannerCallback()
   {
-    if (planner_state_.status == planner_state_.ACTIVE)
+    if (planner_state_.status.value == planner_state_.status.ACTIVE)
       planner_running_ = true;
 
-    if (planner_running_ && planner_state_.status == planner_state_.INACTIVE){
+    if (planner_running_ && planner_state_.status.value != planner_state_.status.ACTIVE){
       planner_running_ = false;
       planner_finished_ = true;
     }

Modified: pkg/trunk/nav/wavefront_player/cli.cpp
===================================================================
--- pkg/trunk/nav/wavefront_player/cli.cpp	2009-03-04 12:37:20 UTC (rev 12106)
+++ pkg/trunk/nav/wavefront_player/cli.cpp	2009-03-04 14:43:36 UTC (rev 12107)
@@ -24,9 +24,9 @@
   }
   void state_cb()
   {
-    if (state == WF_IDLE && wf_state.status == wf_state.ACTIVE)
+    if (state == WF_IDLE && wf_state.status.value == wf_state.status.ACTIVE)
       state = WF_SEEKING_GOAL;
-    else if (state == WF_SEEKING_GOAL && wf_state.status != wf_state.ACTIVE)
+    else if (state == WF_SEEKING_GOAL && wf_state.status.value != wf_state.status.ACTIVE)
       state = WF_DONE;
   }
   void deactivate_goal()

Modified: pkg/trunk/nav/wavefront_player/wavefront_player.cc
===================================================================
--- pkg/trunk/nav/wavefront_player/wavefront_player.cc	2009-03-04 12:37:20 UTC (rev 12106)
+++ pkg/trunk/nav/wavefront_player/wavefront_player.cc	2009-03-04 14:43:36 UTC (rev 12107)
@@ -401,14 +401,14 @@
     this->planner_state = NEW_GOAL;
 
     // Set state for actively pursuing a goal
-    this->pstate.status = this->pstate.ACTIVE;
+    this->pstate.status.value = this->pstate.status.ACTIVE;
   }
   else {
     // Clear goal data
     this->planner_state = NO_GOAL;
 
     // Set state inactive
-    this->pstate.status = this->pstate.INACTIVE;
+    this->pstate.status.value = this->pstate.status.SUCCESS;
   }
 
   double yaw,pitch,roll;
@@ -751,8 +751,11 @@
   btMatrix3x3 mat =  global_pose.getBasis();
   mat.getEulerZYX(yaw, pitch, roll);
 
-  this->pstate.status = (this->enable && 
-			 (this->planner_state == PURSUING_GOAL)) ? 1 : 0;
+  if(this->enable && (this->planner_state == PURSUING_GOAL))
+    this->pstate.status.value = this->pstate.status.ACTIVE;
+  else
+    this->pstate.status.value = this->pstate.status.SUCCESS;
+
   this->pstate.pos.x = global_pose.getOrigin().x();
   this->pstate.pos.y = global_pose.getOrigin().y();
   this->pstate.pos.th = yaw;

Modified: pkg/trunk/pr2_msgs/msg/MoveArmState.msg
===================================================================
--- pkg/trunk/pr2_msgs/msg/MoveArmState.msg	2009-03-04 12:37:20 UTC (rev 12106)
+++ pkg/trunk/pr2_msgs/msg/MoveArmState.msg	2009-03-04 14:43:36 UTC (rev 12107)
@@ -1,21 +1,6 @@
 Header header
+robot_msgs/ControllerStatus status
 
-
-#Status options
-byte INACTIVE=0
-byte ACTIVE=1
-byte ABORTED=2
-byte PREEMPTED=3
-
-# Status of the controller = {INACTIVE, ACTIVE, ABORTED, PREEMPTED}
-byte status
-
-#Id of the goal
-int32 goal_id
-
-#Comment for debug
-string comment
-
 #Current arm configuration
 robot_msgs/JointState[] configuration
 #Goal arm configuration

Modified: pkg/trunk/pr2_msgs/msg/MoveEndEffectorState.msg
===================================================================
--- pkg/trunk/pr2_msgs/msg/MoveEndEffectorState.msg	2009-03-04 12:37:20 UTC (rev 12106)
+++ pkg/trunk/pr2_msgs/msg/MoveEndEffectorState.msg	2009-03-04 14:43:36 UTC (rev 12107)
@@ -1,20 +1,6 @@
 Header header
+robot_msgs/ControllerStatus status
 
-#Status options
-byte INACTIVE=0
-byte ACTIVE=1
-byte ABORTED=2
-byte PREEMPTED=3
-
-# Status of the controller = {INACTIVE, ACTIVE, ABORTED, PREEMPTED}
-byte status
-
-#Id of the goal
-int32 goal_id
-
-#Comment for debug
-string comment
-
 #Current arm configuration
 robot_msgs/JointState[] configuration
 #Goal arm configuration

Added: pkg/trunk/prcore/robot_msgs/msg/ControllerStatus.msg
===================================================================
--- pkg/trunk/prcore/robot_msgs/msg/ControllerStatus.msg	                        (rev 0)
+++ pkg/trunk/prcore/robot_msgs/msg/ControllerStatus.msg	2009-03-04 14:43:36 UTC (rev 12107)
@@ -0,0 +1,13 @@
+# This message defines the expected format for Controller Statuss messages
+# Embed this in the feedback state message of highlevel controllers
+byte UNDEFINED=0
+byte SUCCESS=1
+byte ABORTED=2
+byte PREEMPTED=3
+byte ACTIVE=4
+
+# Status of the controller = {UNDEFINED, SUCCESS, ABORTED, PREEMPTED, ACTIVE}
+byte value
+
+#Comment for debug
+string comment
\ No newline at end of file

Modified: pkg/trunk/prcore/robot_msgs/msg/Planner2DState.msg
===================================================================
--- pkg/trunk/prcore/robot_msgs/msg/Planner2DState.msg	2009-03-04 12:37:20 UTC (rev 12106)
+++ pkg/trunk/prcore/robot_msgs/msg/Planner2DState.msg	2009-03-04 14:43:36 UTC (rev 12107)
@@ -1,21 +1,6 @@
 Header header
+ControllerStatus status
 
-
-#Status options
-byte INACTIVE=0
-byte ACTIVE=1
-byte ABORTED=2
-byte PREEMPTED=3
-
-# Status of the controller = {INACTIVE, ACTIVE, ABORTED, PREEMPTED}
-byte status
-
-#Id of the goal
-int32 goal_id
-
-#Comment for debug
-string comment
-
 # Current location (m,m,rad)
 deprecated_msgs/Pose2DFloat32 pos
 # Goal location (m,m,rad)


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.