added readyForSwitch

ros-controls · Nov 1, 2014 · 24034f0 · 24034f0
1 parent 74f0d03
commit 24034f0
Show file tree

Hide file tree

Showing 2 changed files with 14 additions and 2 deletions.
diff --git a/controller_manager/include/controller_manager/controller_manager.h b/controller_manager/include/controller_manager/controller_manager.h
@@ -142,6 +142,12 @@ class ControllerManager{
                         const std::vector<std::string>& stop_controllers,
                         const int strictness);
 
+  /** \brief Notification for switching HW-Interfaces.
+   *
+   * \param info_list A list containing ControlInfo about the controllers to be started (containing the HW-Interfaces requested by the resources)
+   */
+  virtual bool notifyHardwareInterface(const std::list<hardware_interface::ControllerInfo> &info_list) {return true;}
+
   /** \brief Get a controller by name.
    *
    * \param name The name of a controller

diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp
@@ -439,6 +439,14 @@ bool ControllerManager::switchController(const std::vector<std::string>& start_c
     start_request_.clear();
     return false;
   }
+
+  if(!notifyHardwareInterface(info_list))
+  {
+    ROS_ERROR("Could not switch controllers, because switching the HWInterface failed");
+    stop_request_.clear();
+    start_request_.clear();
+    return false;
+  }
 
   // start the atomic controller switching
   switch_strictness_ = strictness;
@@ -460,8 +468,6 @@ bool ControllerManager::switchController(const std::vector<std::string>& start_c
 
 
 
-
-
 bool ControllerManager::reloadControllerLibrariesSrv(
   controller_manager_msgs::ReloadControllerLibraries::Request &req,
   controller_manager_msgs::ReloadControllerLibraries::Response &resp)