Thread: [Personalrobots-commit] SF.net SVN: personalrobots:[21723] pkg/trunk/stacks (Page 3)

SourceForge Headquarters 1320 Columbia Street Suite 310 San Diego, CA 92101 +1 (858) 422-6466

Revision: 21723
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=21723&view=rev
Author:   vijaypradeep
Date:     2009-08-12 22:52:19 +0000 (Wed, 12 Aug 2009)

Log Message:
-----------
Adding a blocking execute call to SingleActionGoalServer. Porting move_base to the blocking execute

Modified Paths:
--------------
    pkg/trunk/stacks/common/actionlib/include/actionlib/server/single_goal_action_server.h
    pkg/trunk/stacks/common/actionlib/include/actionlib/server/single_goal_action_server_imp.h
    pkg/trunk/stacks/navigation/move_base/include/move_base/move_base.h
    pkg/trunk/stacks/navigation/move_base/src/move_base.cpp

Modified: pkg/trunk/stacks/common/actionlib/include/actionlib/server/single_goal_action_server.h
===================================================================

--- pkg/trunk/stacks/common/actionlib/include/actionlib/server/single_goal_action_server.h	2009-08-12 22:47:31 UTC (rev 21722)
+++ pkg/trunk/stacks/common/actionlib/include/actionlib/server/single_goal_action_server.h	2009-08-12 22:52:19 UTC (rev 21723)
@@ -37,6 +37,7 @@
 #ifndef ACTIONLIB_SINGLE_GOAL_ACTION_SERVER_H_
 #define ACTIONLIB_SINGLE_GOAL_ACTION_SERVER_H_
 
+#include <boost/thread/condition.hpp>
 #include <ros/ros.h>
 #include <actionlib/server/action_server.h>
 #include <actionlib/action_definition.h>
@@ -59,14 +60,20 @@
       ACTION_DEFINITION(ActionSpec);
 
       typedef typename ActionServer<ActionSpec>::GoalHandle GoalHandle;
+      typedef boost::function<void (const GoalConstPtr&)> ExecuteCallback;
 
       /**
        * @brief  Constructor for a SingleGoalActionServer
        * @param n A NodeHandle to create a namespace under
        * @param name A name for the action server
+       * @param execute_cb Optional callback that gets called in a separate thread whenever
+       *                   a new goal is received, allowing users to have blocking callbacks.
+       *                   Adding an execute callback also deactivates the goalCallback.
        */
-      SingleGoalActionServer(ros::NodeHandle n, std::string name);
+      SingleGoalActionServer(ros::NodeHandle n, std::string name, ExecuteCallback execute_cb = NULL);
 
+      ~SingleGoalActionServer();
+
       /**
        * @brief  Accepts a new goal when one is available The status of this
        * goal is set to active upon acceptance, and the status of any
@@ -140,6 +147,13 @@
        */
       void preemptCallback(GoalHandle preempt);
 
+      /**
+       * @brief  Called from a separate thread to call blocking execute calls
+       */
+      void executeLoop();
+
+      ros::NodeHandle n_;
+
       boost::shared_ptr<ActionServer<ActionSpec> > as_;
 
       GoalHandle current_goal_, next_goal_;
@@ -150,7 +164,10 @@
 
       boost::function<void ()> goal_callback_;
       boost::function<void ()> preempt_callback_;
+      ExecuteCallback execute_callback_;
 
+      boost::condition execute_condition_;
+      boost::thread* execute_thread_;
   };
 };
 

Modified: pkg/trunk/stacks/common/actionlib/include/actionlib/server/single_goal_action_server_imp.h
===================================================================
--- pkg/trunk/stacks/common/actionlib/include/actionlib/server/single_goal_action_server_imp.h	2009-08-12 22:47:31 UTC (rev 21722)
+++ pkg/trunk/stacks/common/actionlib/include/actionlib/server/single_goal_action_server_imp.h	2009-08-12 22:52:19 UTC (rev 21723)
@@ -38,17 +38,32 @@
 #define ACTION_LIB_SINGLE_GOAL_ACTION_SERVER_H_
 namespace actionlib {
   template <class ActionSpec>
-  SingleGoalActionServer<ActionSpec>::SingleGoalActionServer(ros::NodeHandle n, std::string name)
-    : new_goal_(false), preempt_request_(false), new_goal_preempt_request_(false) {
+  SingleGoalActionServer<ActionSpec>::SingleGoalActionServer(ros::NodeHandle n, std::string name, ExecuteCallback execute_callback)
+    : n_(n), new_goal_(false), preempt_request_(false), new_goal_preempt_request_(false), execute_callback_(execute_callback) {
 
-      //create the action server
-      as_ = boost::shared_ptr<ActionServer<ActionSpec> >(new ActionServer<ActionSpec>(n, name,
-            boost::bind(&SingleGoalActionServer::goalCallback, this, _1),
-            boost::bind(&SingleGoalActionServer::preemptCallback, this, _1)));
+    //create the action server
+    as_ = boost::shared_ptr<ActionServer<ActionSpec> >(new ActionServer<ActionSpec>(n, name,
+          boost::bind(&SingleGoalActionServer::goalCallback, this, _1),
+          boost::bind(&SingleGoalActionServer::preemptCallback, this, _1)));
 
+    if (execute_callback_ != NULL)
+    {
+      execute_thread_ = new boost::thread(boost::bind(&SingleGoalActionServer::executeLoop, this));
     }
+  }
 
   template <class ActionSpec>
+  SingleGoalActionServer<ActionSpec>::~SingleGoalActionServer()
+  {
+    if (execute_callback_)
+    {
+      assert(execute_thread_);
+      execute_thread_->join();
+      delete execute_thread_;
+    }
+  }
+
+  template <class ActionSpec>
   boost::shared_ptr<const typename SingleGoalActionServer<ActionSpec>::Goal> SingleGoalActionServer<ActionSpec>::acceptNewGoal(){
     boost::recursive_mutex::scoped_lock lock(lock_);
 
@@ -120,7 +135,11 @@
 
   template <class ActionSpec>
   void SingleGoalActionServer<ActionSpec>::registerGoalCallback(boost::function<void ()> cb){
-    goal_callback_ = cb;
+    // Cannot register a goal callback if an execute callback exists
+    if (execute_callback_)
+      ROS_WARN("Cannot call SingleGoalActionServer::registerGoalCallback() because an executeCallback exists. Not going to register it.");
+    else
+      goal_callback_ = cb;
   }
 
   template <class ActionSpec>
@@ -149,6 +168,9 @@
       //if the user has defined a goal callback, we'll call it now
       if(goal_callback_)
         goal_callback_();
+
+      // Trigger runLoop to call execute()
+      execute_condition_.notify_all();
     }
     else{
       //the goal requested has already been preempted by a different goal, so we're not going to execute it
@@ -176,5 +198,40 @@
       new_goal_preempt_request_ = true;
     }
   }
+
+  template <class ActionSpec>
+  void SingleGoalActionServer<ActionSpec>::executeLoop(){
+
+    ros::Duration loop_duration = ros::Duration().fromSec(.1);
+
+    while (n_.ok())
+    {
+      boost::recursive_mutex::scoped_lock lock(lock_);
+      if (isActive())
+        ROS_ERROR("Should never reach this code with an active goal");
+      else if (isNewGoalAvailable())
+      {
+        GoalConstPtr goal = acceptNewGoal();
+
+        ROS_FATAL_COND(!execute_callback_, "execute_callback_ must exist. This is a bug in SingleGoalActionServer");
+
+        // Make sure we're not locked when we call execute
+        lock.unlock();
+        execute_callback_(goal);
+        lock.lock();
+
+        if (isActive())
+        {
+          ROS_WARN("Your executeCallback did not set the goal to a terminal status.\n"
+                   "This is a bug in your ActionServer implementation. Fix your code!\n"
+                   "For now, the ActionServer will set this goal to aborted");
+          setAborted();
+        }
+      }
+      else
+        execute_condition_.timed_wait(lock, boost::posix_time::milliseconds(loop_duration.toSec() * 1000.0f));
+    }
+  }
+
 };
 #endif

Modified: pkg/trunk/stacks/navigation/move_base/include/move_base/move_base.h
===================================================================
--- pkg/trunk/stacks/navigation/move_base/include/move_base/move_base.h	2009-08-12 22:47:31 UTC (rev 21722)
+++ pkg/trunk/stacks/navigation/move_base/include/move_base/move_base.h	2009-08-12 22:52:19 UTC (rev 21723)
@@ -99,11 +99,6 @@
        */
       void executeCycle(geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& global_plan);
 
-      /**
-       * @brief  The main loop of the MoveBase action
-       */
-      void runLoop();
-
     private:
       /**
        * @brief  A service call that can be made when the action is inactive that will return a plan
@@ -160,6 +155,8 @@
 
       void goalCB(const geometry_msgs::PoseStamped::ConstPtr& goal);
 
+      void executeCb(const MoveBaseGoalConstPtr& move_base_goal);
+
       geometry_msgs::PoseStamped goalToGlobalFrame(const geometry_msgs::PoseStamped& goal_pose_msg);
 
       ros::NodeHandle ros_node_;
@@ -186,10 +183,9 @@
       ClearingState clearing_state_;
 
       ros::Time last_valid_plan_, last_valid_control_;
-      boost::thread* run_loop_;
-      pluginlib::PluginLoader<nav_core::BaseGlobalPlanner> bgp_loader_; 
-      pluginlib::PluginLoader<nav_core::BaseLocalPlanner> blp_loader_; 
-      
+      pluginlib::PluginLoader<nav_core::BaseGlobalPlanner> bgp_loader_;
+      pluginlib::PluginLoader<nav_core::BaseLocalPlanner> blp_loader_;
+
   };
 };
 #endif

Modified: pkg/trunk/stacks/navigation/move_base/src/move_base.cpp
===================================================================
--- pkg/trunk/stacks/navigation/move_base/src/move_base.cpp	2009-08-12 22:47:31 UTC (rev 21722)
+++ pkg/trunk/stacks/navigation/move_base/src/move_base.cpp	2009-08-12 22:52:19 UTC (rev 21723)
@@ -42,8 +42,8 @@
 
   MoveBase::MoveBase(std::string name, tf::TransformListener& tf) :
     tf_(tf),
-    as_(ros::NodeHandle(), name),
-    tc_(NULL), planner_costmap_ros_(NULL), controller_costmap_ros_(NULL), 
+    as_(ros::NodeHandle(), name, boost::bind(&MoveBase::executeCb, this, _1)),
+    tc_(NULL), planner_costmap_ros_(NULL), controller_costmap_ros_(NULL),
     planner_(NULL), bgp_loader_("nav_core", "BaseGlobalPlanner"),
     blp_loader_("nav_core", "BaseLocalPlanner"){
 
@@ -125,8 +125,6 @@
     //the initial clearing state will be to conservatively clear the costmaps
     clearing_state_ = CONSERVATIVE_RESET;
 
-    run_loop_ = new boost::thread(boost::bind(&MoveBase::runLoop, this));
-
   }
 
   void MoveBase::goalCB(const geometry_msgs::PoseStamped::ConstPtr& goal){
@@ -216,13 +214,13 @@
     //update the copy of the costmap the planner uses
     clearCostmapWindows(2 * clearing_radius_, 2 * clearing_radius_);
 
-    //if we have a tolerance on the goal point that is greater 
+    //if we have a tolerance on the goal point that is greater
     //than the resolution of the map... compute the full potential function
     double resolution = planner_costmap_ros_->getResolution();
     std::vector<geometry_msgs::PoseStamped> global_plan;
     geometry_msgs::PoseStamped p;
     p = req.goal;
-    p.pose.position.y = req.goal.pose.position.y - req.tolerance; 
+    p.pose.position.y = req.goal.pose.position.y - req.tolerance;
     bool found_legal = false;
     while(!found_legal && p.pose.position.y <= req.goal.pose.position.y + req.tolerance){
       p.pose.position.x = req.goal.pose.position.x - req.tolerance;
@@ -252,10 +250,6 @@
   }
 
   MoveBase::~MoveBase(){
-    if(run_loop_){
-      run_loop_->join();
-      delete run_loop_;
-    }
 
     if(planner_ != NULL)
       delete planner_;
@@ -369,13 +363,13 @@
 
     //just get the latest available transform... for accuracy they should send
     //goals in the frame of the planner
-    goal_pose.stamp_ = ros::Time(); 
+    goal_pose.stamp_ = ros::Time();
 
     try{
       tf_.transformPose(global_frame, goal_pose, global_pose);
     }
     catch(tf::TransformException& ex){
-      ROS_WARN("Failed to transform the goal pose from %s into the %s frame: %s", 
+      ROS_WARN("Failed to transform the goal pose from %s into the %s frame: %s",
           goal_pose.frame_id_.c_str(), global_frame.c_str(), ex.what());
       return goal_pose_msg;
     }
@@ -386,81 +380,67 @@
 
   }
 
-  void MoveBase::runLoop(){
-    ros::Rate r(controller_frequency_);
-    geometry_msgs::PoseStamped goal;
+  void MoveBase::executeCb(const MoveBaseGoalConstPtr& move_base_goal)
+  {
+    geometry_msgs::PoseStamped goal = move_base_goal->target_pose;
     std::vector<geometry_msgs::PoseStamped> global_plan;
-    while(ros_node_.ok()){
-      if(as_.isActive()){
-        if(!as_.isPreemptRequested()){
-          if(as_.isNewGoalAvailable()){
-            //if we're active and a new goal is available, we'll accept it, but we won't shut anything down
-            goal = goalToGlobalFrame((*as_.acceptNewGoal()).target_pose);
 
-            //we'll make sure that we reset our state for the next execution cycle
-            clearing_state_ = CONSERVATIVE_RESET;
-            state_ = PLANNING;
+    ros::Rate r(controller_frequency_);
 
-            //publish the goal point to the visualizer
-            ROS_DEBUG("move_base has received a goal of x: %.2f, y: %.2f", goal.pose.position.x, goal.pose.position.y);
-            publishGoal(goal);
+    while(as_.isActive())
+    {
+      if (!ros_node_.ok())
+      {
+        as_.setAborted();
+        break;
+      }
 
-            //make sure to reset our timeouts
-            last_valid_control_ = ros::Time::now();
-            last_valid_plan_ = ros::Time::now();
-          }
-          //for timing that gives real time even in simulation
-          struct timeval start, end;
-          double start_t, end_t, t_diff;
-          gettimeofday(&start, NULL);
+      if(!as_.isPreemptRequested()){
+        if(as_.isNewGoalAvailable()){
+          //if we're active and a new goal is available, we'll accept it, but we won't shut anything down
+          goal = goalToGlobalFrame((*as_.acceptNewGoal()).target_pose);
 
-          //the real work on pursuing a goal is done here
-          executeCycle(goal, global_plan);
-          
-          gettimeofday(&end, NULL);
-          start_t = start.tv_sec + double(start.tv_usec) / 1e6;
-          end_t = end.tv_sec + double(end.tv_usec) / 1e6;
-          t_diff = end_t - start_t;
-          ROS_DEBUG("Full control cycle time: %.9f\n", t_diff);
-        }
-        else{
-          //if we've been preempted explicitly we need to shut things down
-          resetState();
+          //we'll make sure that we reset our state for the next execution cycle
+          clearing_state_ = CONSERVATIVE_RESET;
+          state_ = PLANNING;
 
-          //notify the ActionServer that we've successfully preemted
-          ROS_DEBUG("Move base preempting the current goal");
-          as_.setPreempted();
-        }
-      }
-      else if(as_.isNewGoalAvailable()){
-        //if we're inactive and a new goal is available, we'll accept it
-        goal = goalToGlobalFrame((*as_.acceptNewGoal()).target_pose);
+          //publish the goal point to the visualizer
+          ROS_DEBUG("move_base has received a goal of x: %.2f, y: %.2f", goal.pose.position.x, goal.pose.position.y);
+          publishGoal(goal);
 
-        //if we shutdown our costmaps when we're deactivated... we need to start them back up now
-        if(shutdown_costmaps_){
-          planner_costmap_ros_->start();
-          controller_costmap_ros_->start();
+          //make sure to reset our timeouts
+          last_valid_control_ = ros::Time::now();
+          last_valid_plan_ = ros::Time::now();
         }
+        //for timing that gives real time even in simulation
+        struct timeval start, end;
+        double start_t, end_t, t_diff;
+        gettimeofday(&start, NULL);
 
-        //on activation... we'll reset our costmaps
-        clearCostmapWindows(2 * clearing_radius_, 2 * clearing_radius_);
+        //the real work on pursuing a goal is done here
+        executeCycle(goal, global_plan);
 
-        //publish the goal point to the visualizer
-        ROS_DEBUG("move_base has received a goal of x: %.2f, y: %.2f", goal.pose.position.x, goal.pose.position.y);
-        publishGoal(goal);
+        gettimeofday(&end, NULL);
+        start_t = start.tv_sec + double(start.tv_usec) / 1e6;
+        end_t = end.tv_sec + double(end.tv_usec) / 1e6;
+        t_diff = end_t - start_t;
+        ROS_DEBUG("Full control cycle time: %.9f\n", t_diff);
+      }
+      else{
+        //if we've been preempted explicitly we need to shut things down
+        resetState();
 
-        //make sure to reset our timeouts
-        last_valid_control_ = ros::Time::now();
-        last_valid_plan_ = ros::Time::now();
+        //notify the ActionServer that we've successfully preemted
+        ROS_DEBUG("Move base preempting the current goal");
+        as_.setPreempted();
       }
 
       //make sure to sleep for the remainder of our cycle time
       if(!r.sleep() && state_ == CONTROLLING)
         ROS_WARN("Control loop missed its desired rate of %.4fHz... the loop actually took %.4f seconds", controller_frequency_, r.cycleTime().toSec());
-
     }
   }
-  
+
   void MoveBase::executeCycle(geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& global_plan){
     //we need to be able to publish velocity commands
     geometry_msgs::Twist cmd_vel;
@@ -474,7 +454,7 @@
     //push the feedback out
     position_pub_.publish(current_position);
 
-    //check that the observation buffers for the costmap are current, we don't want to drive blind 
+    //check that the observation buffers for the costmap are current, we don't want to drive blind
     if(!controller_costmap_ros_->isCurrent()){
       ROS_WARN("Sensor data is out of date, we're not going to allow commanding of the base for safety");
       publishZeroVelocity();
@@ -505,7 +485,7 @@
         else{
           ros::Time attempt_end = last_valid_plan_ + ros::Duration(planner_patience_);
 
-          //check if we've tried to make a plan for over our time limit 
+          //check if we've tried to make a plan for over our time limit
           if(ros::Time::now() > attempt_end){
             //we'll move into our obstacle clearing mode
             state_ = CLEARING;
@@ -526,7 +506,7 @@
           resetState();
           as_.setSucceeded();
           return;
-        } 
+        }
 
         if(tc_->computeVelocityCommands(cmd_vel)){
           last_valid_control_ = ros::Time::now();
@@ -542,7 +522,7 @@
             resetState();
             as_.setAborted();
             return;
-          } 
+          }
 
           //otherwise, if we can't find a valid control, we'll go back to planning
           last_valid_plan_ = ros::Time::now();
@@ -629,7 +609,7 @@
 
 
 
-  } 
+  }
 
   void MoveBase::resetState(){
     state_ = PLANNING;
@@ -653,12 +633,12 @@
 int main(int argc, char** argv){
   ros::init(argc, argv, "move_base");
   tf::TransformListener tf(ros::Duration(10));
-  
+
   move_base::MoveBase move_base(ros::this_node::getName(), tf);
 
   //ros::MultiThreadedSpinner s;
   ros::spin();
-  
+
   return(0);
 
 }


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




Thread: [Personalrobots-commit] SF.net SVN: personalrobots:[21723] pkg/trunk/stacks (Page 3)

personalrobots-commit