Merge pull request ros-controls#2 from ipa-fxm/support_hwinterface_sw…

…itching

Support hwinterface switching

controller_interface/CHANGELOG.rst

@@ -2,6 +2,14 @@
 Changelog for package controller_interface
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+0.9.1 (2014-11-03)
+------------------
+* Update package maintainers
+* Contributors: Adolfo Rodriguez Tsouroukdissian
+
+0.9.0 (2014-10-31)
+------------------
+
 0.8.2 (2014-06-25)
 ------------------
 

controller_interface/package.xml

@@ -1,8 +1,8 @@
 <package>
   <name>controller_interface</name>
-  <version>0.8.2</version>
+  <version>0.9.1</version>
   <description>Interface base class for controllers</description>
-  <maintainer email="wim@hidof.com">Wim Meeussen</maintainer>
+  <maintainer email="adolfo.rodriguez@pal-robotics.com">Adolfo Rodriguez Tsouroukdissian</maintainer>
 
   <license>BSD</license>
 

controller_manager/CHANGELOG.rst

@@ -2,6 +2,17 @@
 Changelog for package controller_manager
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+0.9.1 (2014-11-03)
+------------------
+* Update package maintainers
+* Contributors: Adolfo Rodriguez Tsouroukdissian
+
+0.9.0 (2014-10-31)
+------------------
+* Spawner script: adding shutdown timeout to prevent deadlocks
+* Documentation fixes
+* Contributors: Jonathan Bohren, shadowmanos
+
 0.8.2 (2014-06-25)
 ------------------
 

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

controller_manager/package.xml

@@ -1,8 +1,8 @@
 <package>
   <name>controller_manager</name>
-  <version>0.8.2</version>
+  <version>0.9.1</version>
   <description>The controller manager.</description>
-  <maintainer email="wim@hidof.com">Wim Meeussen</maintainer>
+  <maintainer email="adolfo.rodriguez@pal-robotics.com">Adolfo Rodriguez Tsouroukdissian</maintainer>
 
   <license>BSD</license>
 

controller_manager/scripts/spawner

@@ -50,24 +50,33 @@ load_controller_service = ""
 switch_controller_service = ""
 unload_controller_service = ""
 
+shutdown_timeout = 30.0
 
 def shutdown():
-    global loaded,unload_controller_service,load_controller_service,switch_controller_service
+    global loaded,unload_controller_service,load_controller_service,switch_controller_service,shutdown_timeout
+
+    rospy.loginfo("Shutting down spawner. Stopping and unloading controllers...")
+    # NOTE: Sleep for one second to avoid race conditions inherent in wait_for_service
+    time.sleep(1.0)
 
     try:
         # unloader
         unload_controller = rospy.ServiceProxy(unload_controller_service, UnloadController)
+        unload_controller.wait_for_service(timeout=shutdown_timeout)
 
         # switcher
         switch_controller = rospy.ServiceProxy(switch_controller_service, SwitchController)
+        switch_controller.wait_for_service(timeout=shutdown_timeout)
 
+        rospy.loginfo("Stopping all controllers...");
         switch_controller([], loaded, SwitchControllerRequest.STRICT)
+        rospy.loginfo("Unloading all loaded controllers...");
         for name in reversed(loaded):
             rospy.logout("Trying to unload %s" % name)
             unload_controller(name)
             rospy.logout("Succeeded in unloading %s" % name)
     except (rospy.ServiceException, rospy.exceptions.ROSException) as exc:
-        rospy.logwarn("Controller Spawner couldn't reach controller_manager to take down controllers.")
+        rospy.logwarn("Controller Spawner couldn't reach controller_manager to take down controllers. Waited for %g second(s)."  % (shutdown_timeout))
 
 def parse_args(args=None):
     parser = argparse.ArgumentParser(description='Controller spawner')
@@ -77,21 +86,24 @@ def parse_args(args=None):
                         'start controllers until it hears "True" on a topic (Bool)')
     parser.add_argument('--namespace', metavar='ns',
                         help='namespace of the controller_manager services')
-    parser.add_argument('--timeout', metavar='T', type=int, default=30,
-                        help='how long to wait for controller_manager services [s] (default: 30)')
+    parser.add_argument('--timeout', metavar='T', type=float, default=30,
+                        help='how long to wait for controller_manager services when starting up [s] (default: 30)')
+    parser.add_argument('--shutdown-timeout', metavar='T', type=float, default=30,
+                        help='how long to wait for controller_manager services when shutting down [s] (default: 30)')
     parser.add_argument('controllers', metavar='controller', nargs='+',
                         help='controllers to load')
     return parser.parse_args(args=args)
 
 def main():
-    global unload_controller_service,load_controller_service,switch_controller_service
+    global unload_controller_service,load_controller_service,switch_controller_service,shutdown_timeout
 
     args = parse_args(rospy.myargv()[1:])
 
     wait_for_topic = args.wait_for
     autostart = 1 if not args.stopped else 0
     robot_namespace = args.namespace or ""
     timeout = args.timeout
+    shutdown_timeout = args.shutdown_timeout
 
     rospy.init_node('spawner', anonymous=True)
 

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)

controller_manager_msgs/CHANGELOG.rst

@@ -2,6 +2,20 @@
 Changelog for package controller_manager_msgs
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+0.9.1 (2014-11-03)
+------------------
+* Update package maintainers
+* Contributors: Adolfo Rodriguez Tsouroukdissian
+
+0.9.0 (2014-10-31)
+------------------
+* Add Python helpers for:
+  - Getting all active controller managers.
+  - Determining if a namespace contains the controller manager ROS API.
+  - Filtering the output of the 'list_controllers' service by
+    type, name, state, hardware_interface and claimed resources.
+* Contributors: Adolfo Rodriguez Tsouroukdissian
+
 0.8.2 (2014-06-25)
 ------------------
 

controller_manager_msgs/package.xml

@@ -1,8 +1,8 @@
 <package>
   <name>controller_manager_msgs</name>
-  <version>0.8.2</version>
+  <version>0.9.1</version>
   <description>Messages and services for the controller manager.</description>
-  <maintainer email="wim@hidof.com">Wim Meeussen</maintainer>
+  <maintainer email="adolfo.rodriguez@pal-robotics.com">Adolfo Rodriguez Tsouroukdissian</maintainer>
 
   <license>BSD</license>
 

controller_manager_tests/CHANGELOG.rst

@@ -2,6 +2,17 @@
 Changelog for package controller_manager_tests
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+0.9.1 (2014-11-03)
+------------------
+* Update package maintainers
+* Contributors: Adolfo Rodriguez Tsouroukdissian
+
+0.9.0 (2014-10-31)
+------------------
+* Tests for Python helpers added to controller_manager_msgs
+* Buildsystem and documentation fixes
+* Contributors: Adolfo Rodriguez Tsouroukdissian, Lukas Bulwahn, shadowmanos
+
 0.8.2 (2014-06-25)
 ------------------
 

controller_manager_tests/package.xml

@@ -1,8 +1,8 @@
 <package>
   <name>controller_manager_tests</name>
-  <version>0.8.2</version>
+  <version>0.9.1</version>
   <description>controller_manager_tests</description>
-  <maintainer email="vijay@hidof.com">Vijay Pradeep</maintainer>
+  <maintainer email="adolfo.rodriguez@pal-robotics.com">Adolfo Rodriguez Tsouroukdissian</maintainer>
 
   <license>BSD</license>
 

hardware_interface/CHANGELOG.rst

@@ -2,6 +2,17 @@
 Changelog for package hardware_interface
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+0.9.1 (2014-11-03)
+------------------
+* Update package maintainers
+* Contributors: Adolfo Rodriguez Tsouroukdissian
+
+0.9.0 (2014-10-31)
+------------------
+* Add PosVel and PosVelAcc command interfaces
+* Documentation fixes
+* Contributors: Igorec, shadowmanos
+
 0.8.2 (2014-06-25)
 ------------------
 

hardware_interface/CMakeLists.txt

@@ -38,6 +38,13 @@ if(CATKIN_ENABLE_TESTING)
 
   catkin_add_gtest(interface_manager_test          test/interface_manager_test.cpp)
   target_link_libraries(interface_manager_test ${catkin_LIBRARIES})
+
+    catkin_add_gtest(posvel_command_interface_test test/posvel_command_interface_test.cpp)
+    target_link_libraries(posvel_command_interface_test ${catkin_LIBRARIES})
+
+    catkin_add_gtest(posvelacc_command_interface_test test/posvelacc_command_interface_test.cpp)
+    target_link_libraries(posvelacc_command_interface_test ${catkin_LIBRARIES})
+
 endif()
 
 # Install

hardware_interface/include/hardware_interface/posvel_command_interface.h

@@ -0,0 +1,94 @@
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2012, hiDOF INC.
+//
+// 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 hiDOF, 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 Igor Kalevatykh
+
+#ifndef HARDWARE_INTERFACE_POSVEL_COMMAND_INTERFACE_H
+#define HARDWARE_INTERFACE_POSVEL_COMMAND_INTERFACE_H
+
+#include <cassert>
+#include <string>
+#include <hardware_interface/internal/hardware_resource_manager.h>
+#include <hardware_interface/joint_state_interface.h>
+
+namespace hardware_interface
+{
+
+/** \brief A handle used to read and command a single joint. */
+class PosVelJointHandle : public JointStateHandle
+{
+public:
+  PosVelJointHandle() : JointStateHandle(), cmd_pos_(0), cmd_vel_(0) {}
+
+  /**
+   * \param js This joint's state handle
+   * \param cmd_pos A pointer to the storage for this joint's output command position
+   * \param cmd_vel A pointer to the storage for this joint's output command velocity
+   */
+  PosVelJointHandle(const JointStateHandle& js, double* cmd_pos, double* cmd_vel)
+    : JointStateHandle(js), cmd_pos_(cmd_pos), cmd_vel_(cmd_vel)
+  {
+    if (!cmd_pos)
+    {
+      throw HardwareInterfaceException("Cannot create handle '" + js.getName() + "'. Command position pointer is null.");
+    }
+    if (!cmd_vel)
+    {
+      throw HardwareInterfaceException("Cannot create handle '" + js.getName() + "'. Command velocity  pointer is null.");
+    }
+  }
+
+  void setCommand(double cmd_pos, double cmd_vel)
+  {
+    setCommandPosition(cmd_pos);
+    setCommandVelocity(cmd_vel);
+  }
+
+  void setCommandPosition(double cmd_pos)     {assert(cmd_pos_); *cmd_pos_ = cmd_pos;}
+  void setCommandVelocity(double cmd_vel)     {assert(cmd_vel_); *cmd_vel_ = cmd_vel;}
+
+  double getCommandPosition()     const {assert(cmd_pos_); return *cmd_pos_;}
+  double getCommandVelocity()     const {assert(cmd_vel_); return *cmd_vel_;}
+
+private:
+  double* cmd_pos_;
+  double* cmd_vel_;
+};
+
+/** \brief Hardware interface to support commanding an array of joints.
+ *
+ * This \ref HardwareInterface supports commanding joints by position, velocity
+ * together in one command.
+ *
+ * \note Getting a joint handle through the getHandle() method \e will claim that resource.
+ *
+ */
+class PosVelJointInterface : public HardwareResourceManager<PosVelJointHandle, ClaimResources> {};
+
+}
+
+#endif /*HARDWARE_INTERFACE_POSVEL_COMMAND_INTERFACE_H*/