add option to pass in two nodehandles to a controller: one in the roo…

…t of the controller manager namespace, and one in the namespace of the controller itself. This copies the behavior used by nodelets and nodes

controller_interface/include/controller_interface/controller.h

@@ -59,7 +59,7 @@ class Controller: public ControllerBase
    * non-realtime thread with a pointer to the hardware interface, itself,
    * instead of a pointer to a RobotHW.
    *
-   * \param hw The specific hardware interface used by this controller. 
+   * \param hw The specific hardware interface used by this controller.
    *
    * \param n A NodeHandle in the namespace from which the controller
    * should read its configuration, and where it should set up its ROS
@@ -68,7 +68,8 @@ class Controller: public ControllerBase
    * \returns True if initialization was successful and the controller
    * is ready to be started.
    */
-  virtual bool init(T* hw, ros::NodeHandle &n) = 0;
+  virtual bool init(T* hw, ros::NodeHandle &controller_nh) {return true;};
+  virtual bool init(T* hw, ros::NodeHandle& root_nh, ros::NodeHandle &controller_nh) {return true;};
 
 
 protected:
@@ -79,7 +80,7 @@ class Controller: public ControllerBase
    *
    */
   virtual bool initRequest(hardware_interface::RobotHW* robot_hw,
-                           ros::NodeHandle &n,
+                           ros::NodeHandle& root_nh, ros::NodeHandle &controller_nh,
                            std::set<std::string> &claimed_resources)
   {
     // check if construction finished cleanly
@@ -99,7 +100,7 @@ class Controller: public ControllerBase
 
     // return which resources are claimed by this controller
     hw->clearClaims();
-    if (!init(hw, n))
+    if (!init(hw, controller_nh) || !init(hw, root_nh, controller_nh))
     {
       ROS_ERROR("Failed to initialize the controller");
       return false;

controller_interface/include/controller_interface/controller_base.h

@@ -94,10 +94,10 @@ class ControllerBase
    * \returns True if initialization was successful and the controller
    * is ready to be started.
    */
-  virtual bool initRequest(hardware_interface::RobotHW* hw, ros::NodeHandle &n,
-                           std::set<std::string>& claimed_resources)=0;
+  virtual bool initRequest(hardware_interface::RobotHW* hw, ros::NodeHandle& root_nh, ros::NodeHandle &controller_nh,
+                           std::set<std::string>& claimed_resources) = 0;;
 
-  /// Calls \ref update only if this controller is running. 
+  /// Calls \ref update only if this controller is running.
   void updateRequest(const ros::Time& time, const ros::Duration& period)
   {
     if (state_ == RUNNING)

controller_manager/include/controller_manager/controller_manager.h

@@ -101,11 +101,11 @@ class ControllerManager{
    *
    * This dynamically loads a controller called \c name and initializes the
    * newly
-   * loaded controller. 
+   * loaded controller.
    *
    * It determines the controller type by accessing the ROS parameter "type" in
    * the namespace given by \c name relative to the namespace of \ref
-   * controller_node_. It then initializes the controller with the
+   * root_nh_. It then initializes the controller with the
    * hardware_interface::RobotHW pointer \ref robot_hw_, the ros::NodeHandle
    * describing this namespace, and a reference to a std::set to retrieve the
    * resources needed by this controller.
@@ -117,7 +117,7 @@ class ControllerManager{
    * which the controller should be loaded
    *
    * \returns True on success
-   * \returns False on failure 
+   * \returns False on failure
    */
   bool loadController(const std::string& name);
 
@@ -127,7 +127,7 @@ class ControllerManager{
    *
    */
   bool unloadController(const std::string &name);
-  
+
   /** \brief Switch multiple controllers simultaneously.
    *
    * \param start_controllers A vector of controller names to be started
@@ -144,8 +144,8 @@ class ControllerManager{
 
   /** \brief Get a controller by name.
    *
-   * \param name The name of a controller 
-   * \returns An up-casted pointer to the controller identified by \c name 
+   * \param name The name of a controller
+   * \returns An up-casted pointer to the controller identified by \c name
    */
   virtual controller_interface::ControllerBase* getControllerByName(const std::string& name);
 
@@ -160,7 +160,7 @@ class ControllerManager{
    * then it WILL, regardless of which other loaders are registered.
    *
    * \param controller_loader A pointer to the loader to be registered
-   * 
+   *
    */
   void registerControllerLoader(boost::shared_ptr<ControllerLoaderInterface> controller_loader);
   /*\}*/
@@ -172,7 +172,7 @@ class ControllerManager{
 
   hardware_interface::RobotHW* robot_hw_;
 
-  ros::NodeHandle controller_node_, cm_node_;
+  ros::NodeHandle root_nh_, cm_node_;
 
   typedef boost::shared_ptr<ControllerLoaderInterface> LoaderPtr;
   std::list<LoaderPtr> controller_loaders_;

controller_manager/src/controller_manager.cpp

@@ -42,7 +42,7 @@ namespace controller_manager{
 
 ControllerManager::ControllerManager(hardware_interface::RobotHW *robot_hw, const ros::NodeHandle& nh) :
   robot_hw_(robot_hw),
-  controller_node_(nh),
+  root_nh_(nh),
   cm_node_(nh, "controller_manager"),
   start_request_(0),
   stop_request_(0),
@@ -167,10 +167,10 @@ bool ControllerManager::loadController(const std::string& name)
     }
   }
 
-  ros::NodeHandle c_node;
+  ros::NodeHandle c_nh;
   // Constructs the controller
   try{
-    c_node = ros::NodeHandle(controller_node_, name);
+    c_nh = ros::NodeHandle(root_nh_, name);
   }
   catch(std::exception &e) {
     ROS_ERROR("Exception thrown while constructing nodehandle for controller with name '%s':\n%s", name.c_str(), e.what());
@@ -182,7 +182,7 @@ bool ControllerManager::loadController(const std::string& name)
   }
   boost::shared_ptr<controller_interface::ControllerBase> c;
   std::string type;
-  if (c_node.getParam("type", type))
+  if (c_nh.getParam("type", type))
   {
     ROS_DEBUG("Constructing controller '%s' of type '%s'", name.c_str(), type.c_str());
     try
@@ -226,7 +226,7 @@ bool ControllerManager::loadController(const std::string& name)
   bool initialized;
   std::set<std::string> claimed_resources; // Gets populated during initRequest call
   try{
-    initialized = c->initRequest(robot_hw_, c_node, claimed_resources);
+    initialized = c->initRequest(robot_hw_, root_nh_, c_nh, claimed_resources);
   }
   catch(std::exception &e){
     ROS_ERROR("Exception thrown while initializing controller %s.\n%s", name.c_str(), e.what());