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

[<ei...@us...>]

Revision: 17018
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=17018&view=rev
Author:   eitanme
Date:     2009-06-12 02:22:59 +0000 (Fri, 12 Jun 2009)

Log Message:
-----------
Added support for BaseGlobalPlanner and BaseLocalPlanner interfaces. Includes a factory for each that makes use of different global/local planners with MoveBase easy.

Modified Paths:
--------------
    pkg/trunk/highlevel/move_base/include/move_base/move_base.h
    pkg/trunk/highlevel/move_base/src/move_base.cpp
    pkg/trunk/highlevel/move_base/src/move_base_local.cpp
    pkg/trunk/motion_planning/navfn/include/navfn/navfn_ros.h
    pkg/trunk/motion_planning/navfn/manifest.xml
    pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp
    pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner_ros.h
    pkg/trunk/nav/base_local_planner/manifest.xml
    pkg/trunk/nav/base_local_planner/src/trajectory_planner.cpp
    pkg/trunk/nav/base_local_planner/src/trajectory_planner_ros.cpp
    pkg/trunk/nav/nav_robot_actions/manifest.xml

Added Paths:
-----------
    pkg/trunk/nav/nav_robot_actions/include/
    pkg/trunk/nav/nav_robot_actions/include/nav_robot_actions/
    pkg/trunk/nav/nav_robot_actions/include/nav_robot_actions/base_global_planner.h
    pkg/trunk/nav/nav_robot_actions/include/nav_robot_actions/base_local_planner.h

Modified: pkg/trunk/highlevel/move_base/include/move_base/move_base.h
===================================================================
--- pkg/trunk/highlevel/move_base/include/move_base/move_base.h	2009-06-12 01:47:29 UTC (rev 17017)
+++ pkg/trunk/highlevel/move_base/include/move_base/move_base.h	2009-06-12 02:22:59 UTC (rev 17018)
@@ -39,17 +39,18 @@
 #include <robot_actions/action.h>
 #include <robot_actions/action_runner.h>
 #include <nav_robot_actions/MoveBaseState.h>
+#include <nav_robot_actions/base_local_planner.h>
+#include <nav_robot_actions/base_global_planner.h>
 #include <robot_msgs/PoseStamped.h>
 #include <ros/ros.h>
 #include <costmap_2d/costmap_2d_ros.h>
 #include <costmap_2d/costmap_2d.h>
 #include <costmap_2d/rate.h>
-#include <navfn/navfn_ros.h>
-#include <base_local_planner/trajectory_planner_ros.h>
 #include <vector>
 #include <string>
 #include <nav_srvs/Plan.h>
 #include <visualization_msgs/Marker.h>
+#include <robot_msgs/PoseDot.h>
 
 namespace move_base {
   /**
@@ -62,9 +63,10 @@
        * @brief  Constructor for the actions
        * @param ros_node A reference to the ros node used 
        * @param tf A reference to a TransformListener
-       * @return 
+       * @param global_planner The name of the global planner to use
+       * @param local_planner The name of the local planner to use
        */
-      MoveBase(ros::Node& ros_node, tf::TransformListener& tf);
+      MoveBase(ros::Node& ros_node, tf::TransformListener& tf, std::string global_planner, std::string local_planner);
 
       /**
        * @brief  Destructor - Cleans up
@@ -120,11 +122,10 @@
 
       ros::Node& ros_node_;
       tf::TransformListener& tf_;
-      bool run_planner_;
-      base_local_planner::TrajectoryPlannerROS* tc_;
+      nav_robot_actions::BaseLocalPlanner* tc_;
       costmap_2d::Costmap2DROS* planner_costmap_ros_, *controller_costmap_ros_;
 
-      navfn::NavfnROS* planner_;
+      nav_robot_actions::BaseGlobalPlanner* planner_;
       std::vector<robot_msgs::PoseStamped> global_plan_;
       std::vector<robot_msgs::Point> footprint_;
       std::string robot_base_frame_, global_frame_;

Modified: pkg/trunk/highlevel/move_base/src/move_base.cpp
===================================================================
--- pkg/trunk/highlevel/move_base/src/move_base.cpp	2009-06-12 01:47:29 UTC (rev 17017)
+++ pkg/trunk/highlevel/move_base/src/move_base.cpp	2009-06-12 02:22:59 UTC (rev 17018)
@@ -38,9 +38,7 @@
 #include <cstdlib>
 #include <ctime>
 
-using namespace base_local_planner;
 using namespace costmap_2d;
-using namespace navfn;
 using namespace robot_actions;
 
 namespace move_base {
@@ -49,9 +47,9 @@
     return x < 0.0 ? -1.0 : 1.0;
   }
 
-  MoveBase::MoveBase(ros::Node& ros_node, tf::TransformListener& tf) : 
+  MoveBase::MoveBase(ros::Node& ros_node, tf::TransformListener& tf, std::string global_planner, std::string local_planner) :
     Action<robot_msgs::PoseStamped, robot_msgs::PoseStamped>(ros_node.getName()), ros_node_(ros_node), tf_(tf),
-    run_planner_(true), tc_(NULL), planner_costmap_ros_(NULL), controller_costmap_ros_(NULL), 
+    tc_(NULL), planner_costmap_ros_(NULL), controller_costmap_ros_(NULL), 
     planner_(NULL), valid_plan_(false), new_plan_(false), attempted_rotation_(false), attempted_costmap_reset_(false),
     done_half_rotation_(false), done_full_rotation_(false), escaping_(false) {
 
@@ -125,15 +123,27 @@
     //create the ros wrapper for the planner's costmap... and initializer a pointer we'll use with the underlying map
     planner_costmap_ros_ = new Costmap2DROS(ros_node_, tf_, std::string("navfn"), footprint_);
 
-    //initialize the NavFn planner
-    planner_ = new NavfnROS(ros_node_, tf_, *planner_costmap_ros_);
+    //initialize the global planner
+    try {
+      planner_ = nav_robot_actions::BGPFactory::Instance().CreateObject(global_planner, ros_node_, tf_, *planner_costmap_ros_);
+    } catch (Loki::DefaultFactoryError<std::string, nav_robot_actions::BaseGlobalPlanner>::Exception)
+    {
+      ROS_ASSERT_MSG(false, "Cannot create the desired global planner because it is not registered with the factory");
+    }
+
     ROS_INFO("MAP SIZE: %d, %d", planner_costmap_ros_->cellSizeX(), planner_costmap_ros_->cellSizeY());
 
     //create the ros wrapper for the controller's costmap... and initializer a pointer we'll use with the underlying map
     controller_costmap_ros_ = new Costmap2DROS(ros_node_, tf_, std::string("base_local_planner"), footprint_);
 
-    //create a trajectory controller
-    tc_ = new TrajectoryPlannerROS(ros_node_, tf_, *controller_costmap_ros_, footprint_);
+    //create a local planner
+    try {
+      tc_ = nav_robot_actions::BLPFactory::Instance().CreateObject(local_planner, 
+          ros_node_, tf_, *controller_costmap_ros_, footprint_);
+    } catch (Loki::DefaultFactoryError<std::string, nav_robot_actions::BaseLocalPlanner>::Exception)
+    {
+      ROS_ASSERT_MSG(false, "Cannot create the desired local planner because it is not registered with the factory");
+    }
 
     //advertise a service for getting a plan
     ros_node_.advertiseService("~make_plan", &MoveBase::planService, this);
@@ -579,7 +589,7 @@
   ros::Node ros_node("move_base");
   tf::TransformListener tf(ros_node, true, ros::Duration(10));
   
-  move_base::MoveBase move_base(ros_node, tf);
+  move_base::MoveBase move_base(ros_node, tf, "NavfnROS", "TrajectoryPlannerROS");
   robot_actions::ActionRunner runner(20.0);
   runner.connect<robot_msgs::PoseStamped, nav_robot_actions::MoveBaseState, robot_msgs::PoseStamped>(move_base);
   runner.run();

Modified: pkg/trunk/highlevel/move_base/src/move_base_local.cpp
===================================================================
--- pkg/trunk/highlevel/move_base/src/move_base_local.cpp	2009-06-12 01:47:29 UTC (rev 17017)
+++ pkg/trunk/highlevel/move_base/src/move_base_local.cpp	2009-06-12 02:22:59 UTC (rev 17018)
@@ -194,8 +194,8 @@
 
       bool valid_control = false;
 
-      //compute veloctiy commands to send to the base... don't prune the path
-      valid_control = tc_->computeVelocityCommands(cmd_vel, false);
+      //compute veloctiy commands to send to the base
+      valid_control = tc_->computeVelocityCommands(cmd_vel);
 
       //give the base the velocity command
       ros_node_.publish("cmd_vel", cmd_vel);

Modified: pkg/trunk/motion_planning/navfn/include/navfn/navfn_ros.h
===================================================================
--- pkg/trunk/motion_planning/navfn/include/navfn/navfn_ros.h	2009-06-12 01:47:29 UTC (rev 17017)
+++ pkg/trunk/motion_planning/navfn/include/navfn/navfn_ros.h	2009-06-12 02:22:59 UTC (rev 17018)
@@ -46,9 +46,10 @@
 #include <tf/transform_listener.h>
 #include <vector>
 #include <robot_msgs/Point.h>
+#include <nav_robot_actions/base_global_planner.h>
 
 namespace navfn {
-  class NavfnROS {
+  class NavfnROS : public nav_robot_actions::BaseGlobalPlanner {
     public:
       /**
        * @brief  Constructor for the NavFnROS object

Modified: pkg/trunk/motion_planning/navfn/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/navfn/manifest.xml	2009-06-12 01:47:29 UTC (rev 17017)
+++ pkg/trunk/motion_planning/navfn/manifest.xml	2009-06-12 02:22:59 UTC (rev 17018)
@@ -9,6 +9,7 @@
  <depend package="roscpp"/>
  <depend package="robot_msgs"/>
  <depend package="costmap_2d"/>
+ <depend package="nav_robot_actions"/>
  <depend package="tf"/>
  <depend package="visualization_msgs"/>
  <sysdepend os="ubuntu" version="7.04-feisty" package="libfltk1.1-dev"/> <!-- actually optional -->

Modified: pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp
===================================================================
--- pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp	2009-06-12 01:47:29 UTC (rev 17017)
+++ pkg/trunk/motion_planning/navfn/src/navfn_ros.cpp	2009-06-12 02:22:59 UTC (rev 17018)
@@ -37,7 +37,11 @@
 #include <navfn/navfn_ros.h>
 
 namespace navfn {
-  NavfnROS::NavfnROS(ros::Node& ros_node, tf::TransformListener& tf, costmap_2d::Costmap2DROS& costmap_ros) : ros_node_(ros_node), tf_(tf), 
+  //register this factory with the ros BaseGlobalPlanner factory
+  ROS_REGISTER_BGP(NavfnROS);
+
+  NavfnROS::NavfnROS(ros::Node& ros_node, tf::TransformListener& tf, costmap_2d::Costmap2DROS& costmap_ros) 
+    : ros_node_(ros_node), tf_(tf), 
   costmap_ros_(costmap_ros), planner_(costmap_ros.cellSizeX(), costmap_ros.cellSizeY()) {
     //get an initial copy of the costmap
     costmap_ros_.getCostmapCopy(costmap_);

Modified: pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner_ros.h
===================================================================
--- pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner_ros.h	2009-06-12 01:47:29 UTC (rev 17017)
+++ pkg/trunk/nav/base_local_planner/include/base_local_planner/trajectory_planner_ros.h	2009-06-12 02:22:59 UTC (rev 17018)
@@ -67,12 +67,14 @@
 
 #include <angles/angles.h>
 
+#include <nav_robot_actions/base_local_planner.h>
+
 namespace base_local_planner {
   /**
    * @class TrajectoryPlannerROS
    * @brief A ROS wrapper for the trajectory controller that queries the param server to construct a controller
    */
-  class TrajectoryPlannerROS{
+  class TrajectoryPlannerROS : public nav_robot_actions::BaseLocalPlanner {
     public:
       /**
        * @brief  Constructs the ros wrapper
@@ -101,11 +103,9 @@
       /**
        * @brief  Given the current position, orientation, and velocity of the robot, compute velocity commands to send to the base
        * @param cmd_vel Will be filled with the velocity command to be passed to the robot base
-       * @param observations A vector of updates from the robot's sensors in world space, is sometimes unused depending on the model
-       * @param prune_plan Set to true if you would like the plan to be pruned as the robot drives, false to leave the plan as is
        * @return True if a valid trajectory was found, false otherwise
        */
-      bool computeVelocityCommands(robot_msgs::PoseDot& cmd_vel, bool prune_plan = true);
+      bool computeVelocityCommands(robot_msgs::PoseDot& cmd_vel);
 
       /**
        * @brief  Update the plan that the controller is following
@@ -215,6 +215,7 @@
       costmap_2d::Costmap2DPublisher* costmap_publisher_;
       std::vector<robot_msgs::PoseStamped> global_plan_;
       double transform_tolerance_, update_plan_tolerance_;
+      bool prune_plan_;
   };
 
 };

Modified: pkg/trunk/nav/base_local_planner/manifest.xml
===================================================================
--- pkg/trunk/nav/base_local_planner/manifest.xml	2009-06-12 01:47:29 UTC (rev 17017)
+++ pkg/trunk/nav/base_local_planner/manifest.xml	2009-06-12 02:22:59 UTC (rev 17018)
@@ -18,6 +18,7 @@
   <depend package="voxel_grid" />
   <depend package="angles" />
   <depend package="visualization_msgs"/>
+  <depend package="nav_robot_actions"/>
   <url>http://pr.willowgarage.com</url>
   <repository>http://pr.willowgarage.com/repos</repository>
   <export>

Modified: pkg/trunk/nav/base_local_planner/src/trajectory_planner.cpp
===================================================================
--- pkg/trunk/nav/base_local_planner/src/trajectory_planner.cpp	2009-06-12 01:47:29 UTC (rev 17017)
+++ pkg/trunk/nav/base_local_planner/src/trajectory_planner.cpp	2009-06-12 02:22:59 UTC (rev 17018)
@@ -336,7 +336,7 @@
     if(!heading_scoring_)
       cost = pdist_scale_ * path_dist + goal_dist * gdist_scale_ + occdist_scale_ * occ_cost;
     else
-      cost = occdist_scale_ * occ_cost + heading_diff + goal_dist * gdist_scale_;
+      cost = occdist_scale_ * occ_cost + pdist_scale_ * path_dist + 0.3 * heading_diff + goal_dist * gdist_scale_;
 
     traj.cost_ = cost;
   }

Modified: pkg/trunk/nav/base_local_planner/src/trajectory_planner_ros.cpp
===================================================================
--- pkg/trunk/nav/base_local_planner/src/trajectory_planner_ros.cpp	2009-06-12 01:47:29 UTC (rev 17017)
+++ pkg/trunk/nav/base_local_planner/src/trajectory_planner_ros.cpp	2009-06-12 02:22:59 UTC (rev 17018)
@@ -47,6 +47,9 @@
 using namespace laser_scan;
 
 namespace base_local_planner {
+  //register this base local planner with the BaseLocalPlanner factory
+  ROS_REGISTER_BLP(TrajectoryPlannerROS);
+
   TrajectoryPlannerROS::TrajectoryPlannerROS(ros::Node& ros_node, tf::TransformListener& tf, 
       Costmap2DROS& costmap_ros, std::vector<Point> footprint_spec) 
     : world_model_(NULL), tc_(NULL), costmap_ros_(costmap_ros), tf_(tf), ros_node_(ros_node), 
@@ -71,6 +74,7 @@
     ros_node.param("~base_local_planner/costmap/robot_base_frame", robot_base_frame_, string("base_link"));
     ros_node.param("~base_local_planner/transform_tolerance", transform_tolerance_, 0.2);
     ros_node.param("~base_local_planner/update_plan_tolerance", update_plan_tolerance_, 1.0);
+    ros_node.param("~base_local_planner/prune_plan", prune_plan_, true);
 
     double map_publish_frequency;
     ros_node.param("~base_local_planner/costmap_visualization_rate", map_publish_frequency, 2.0);
@@ -294,7 +298,7 @@
     return true;
   }
 
-  bool TrajectoryPlannerROS::computeVelocityCommands(robot_msgs::PoseDot& cmd_vel, bool prune_plan){
+  bool TrajectoryPlannerROS::computeVelocityCommands(robot_msgs::PoseDot& cmd_vel){
     //assume at the beginning of our control cycle that we could have a new goal
     goal_reached_ = false;
 
@@ -336,7 +340,7 @@
     }
 
     //now we'll prune the plan based on the position of the robot
-    if(prune_plan)
+    if(prune_plan_)
       prunePlan(global_pose, transformed_plan, global_plan_);
 
 

Added: pkg/trunk/nav/nav_robot_actions/include/nav_robot_actions/base_global_planner.h
===================================================================
--- pkg/trunk/nav/nav_robot_actions/include/nav_robot_actions/base_global_planner.h	                        (rev 0)
+++ pkg/trunk/nav/nav_robot_actions/include/nav_robot_actions/base_global_planner.h	2009-06-12 02:22:59 UTC (rev 17018)
@@ -0,0 +1,84 @@
+/*********************************************************************
+*
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2008, Willow Garage, Inc.
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+*
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of Willow Garage, Inc. nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*
+* Author: Eitan Marder-Eppstein
+*********************************************************************/
+#ifndef NAV_ROBOT_ACTIONS_BASE_GLOBAL_PLANNER_
+#define NAV_ROBOT_ACTIONS_BASE_GLOBAL_PLANNER_
+
+#include <robot_msgs/PoseStamped.h>
+#include <costmap_2d/costmap_2d_ros.h>
+#include <loki/Factory.h>
+#include <loki/Sequence.h>
+
+namespace nav_robot_actions {
+  class BaseGlobalPlanner{
+    public:
+      virtual bool makePlan(const robot_msgs::PoseStamped& goal, std::vector<robot_msgs::PoseStamped>& plan) = 0;
+
+    protected:
+      BaseGlobalPlanner(){}
+  };
+
+  //create an associated factory
+  typedef Loki::SingletonHolder
+  <
+    Loki::Factory< BaseGlobalPlanner, std::string, 
+      Loki::Seq< ros::Node&, 
+      tf::TransformListener&, 
+      costmap_2d::Costmap2DROS&  > >,
+    Loki::CreateUsingNew,
+    Loki::LongevityLifetime::DieAsSmallObjectParent
+  > BGPFactory;
+
+#define ROS_REGISTER_BGP(c) \
+  nav_robot_actions::BaseGlobalPlanner* ROS_New_##c(ros::Node& ros_node, tf::TransformListener& tf, \
+      costmap_2d::Costmap2DROS& costmap_ros){ \
+    return new c(ros_node, tf, costmap_ros); \
+  }  \
+  class RosBGP##c { \
+    public: \
+    RosBGP##c() \
+    { \
+      nav_robot_actions::BGPFactory::Instance().Register(#c, ROS_New_##c); \
+    } \
+    ~RosBGP##c() \
+    { \
+      nav_robot_actions::BGPFactory::Instance().Unregister(#c); \
+    } \
+  }; \
+  static RosBGP##c ROS_BGP_##c;
+};
+
+#endif

Added: pkg/trunk/nav/nav_robot_actions/include/nav_robot_actions/base_local_planner.h
===================================================================
--- pkg/trunk/nav/nav_robot_actions/include/nav_robot_actions/base_local_planner.h	                        (rev 0)
+++ pkg/trunk/nav/nav_robot_actions/include/nav_robot_actions/base_local_planner.h	2009-06-12 02:22:59 UTC (rev 17018)
@@ -0,0 +1,88 @@
+/*********************************************************************
+*
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2008, Willow Garage, Inc.
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+*
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of Willow Garage, Inc. nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*
+* Author: Eitan Marder-Eppstein
+*********************************************************************/
+#ifndef NAV_ROBOT_ACTIONS_BASE_LOCAL_PLANNER_
+#define NAV_ROBOT_ACTIONS_BASE_LOCAL_PLANNER_
+
+#include <robot_msgs/PoseStamped.h>
+#include <robot_msgs/PoseDot.h>
+#include <costmap_2d/costmap_2d_ros.h>
+#include <loki/Factory.h>
+#include <loki/Sequence.h>
+
+namespace nav_robot_actions {
+  class BaseLocalPlanner{
+    public:
+      virtual bool computeVelocityCommands(robot_msgs::PoseDot& cmd_vel) = 0;
+      virtual bool goalReached() = 0;
+      virtual bool updatePlan(const std::vector<robot_msgs::PoseStamped>& plan) = 0;
+
+    protected:
+      BaseLocalPlanner(){}
+  };
+
+  //create an associated factory
+  typedef Loki::SingletonHolder
+  <
+    Loki::Factory< BaseLocalPlanner, std::string, 
+      Loki::Seq< ros::Node&, 
+      tf::TransformListener&, 
+      costmap_2d::Costmap2DROS&, 
+      std::vector<robot_msgs::Point>  > >,
+    Loki::CreateUsingNew,
+    Loki::LongevityLifetime::DieAsSmallObjectParent
+  > BLPFactory;
+
+#define ROS_REGISTER_BLP(c) \
+  nav_robot_actions::BaseLocalPlanner* ROS_New_##c(ros::Node& ros_node, tf::TransformListener& tf, \
+      costmap_2d::Costmap2DROS& costmap_ros, std::vector<robot_msgs::Point> footprint){ \
+    return new c(ros_node, tf, costmap_ros, footprint); \
+  }  \
+  class RosBLP##c { \
+    public: \
+    RosBLP##c() \
+    { \
+      nav_robot_actions::BLPFactory::Instance().Register(#c, ROS_New_##c); \
+    } \
+    ~RosBLP##c() \
+    { \
+      nav_robot_actions::BLPFactory::Instance().Unregister(#c); \
+    } \
+  }; \
+  static RosBLP##c ROS_BLP_##c;
+};
+
+#endif

Modified: pkg/trunk/nav/nav_robot_actions/manifest.xml
===================================================================
--- pkg/trunk/nav/nav_robot_actions/manifest.xml	2009-06-12 01:47:29 UTC (rev 17017)
+++ pkg/trunk/nav/nav_robot_actions/manifest.xml	2009-06-12 02:22:59 UTC (rev 17018)
@@ -1,6 +1,6 @@
 <package>
-  <description brief="This package contains core interfaces for robot actions"/>
- <author>Conor McGann</author>
+  <description brief="This package contains core interfaces for navigation robot actions"/>
+ <author>Eitan Marder-Eppstein</author>
  <license>BSD</license>
  <review status="unreviewed" notes=""/>
  <url>http://pr.willowgarage.com</url>
@@ -8,10 +8,11 @@
  <depend package="robot_actions"/>
  <depend package="std_msgs"/>
  <depend package="robot_msgs"/>
+ <depend package="loki"/>
 
 
  <export>
-    <cpp cflags="-I${prefix}/msg/cpp -I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib  -lrobot_actions"/>
+    <cpp cflags="-I${prefix}/msg/cpp -I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib"/>
  </export>
 
 </package>


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