CombinedRobotHW implementation

combined_robot_hw/CMakeLists.txt

@@ -0,0 +1,34 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(combined_robot_hw)
+
+find_package(catkin REQUIRED COMPONENTS
+  hardware_interface
+  pluginlib
+  roscpp
+)
+
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+)
+
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES ${PROJECT_NAME}
+  CATKIN_DEPENDS hardware_interface pluginlib roscpp
+)
+
+add_library(${PROJECT_NAME}
+  src/combined_robot_hw.cpp
+)
+add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES})
+
+
+# Install
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})
+
+install(TARGETS ${PROJECT_NAME}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
+

combined_robot_hw/include/combined_robot_hw/combined_robot_hw.h

@@ -0,0 +1,121 @@
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2015, Shadow Robot Company Ltd.
+//
+// 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 Shadow Robot Company Ltd. 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.
+//////////////////////////////////////////////////////////////////////////////
+
+#ifndef COMBINED_ROBOT_HW_COMBINED_ROBOT_HW_H
+#define COMBINED_ROBOT_HW_COMBINED_ROBOT_HW_H
+
+#include <list>
+#include <map>
+#include <typeinfo>
+#include <hardware_interface/internal/demangle_symbol.h>
+#include <hardware_interface/internal/interface_manager.h>
+#include <hardware_interface/hardware_interface.h>
+#include <hardware_interface/robot_hw.h>
+#include <pluginlib/class_loader.h>
+#include <ros/console.h>
+#include <ros/node_handle.h>
+
+namespace combined_robot_hw
+{
+
+/** \brief CombinedRobotHW
+ *
+ * This class provides a way to combine RobotHW objects.
+ *
+ *
+ *
+ */
+class CombinedRobotHW : public hardware_interface::RobotHW
+{
+public:
+  CombinedRobotHW();
+
+  virtual ~CombinedRobotHW(){}
+
+  /** \brief The init function is called to initialize the RobotHW from a
+   * non-realtime thread.
+   *
+   * \param root_nh A NodeHandle in the root of the caller namespace.
+   *
+   * \param robot_hw_nh A NodeHandle in the namespace from which the RobotHW
+   * should read its configuration.
+   *
+   * \returns True if initialization was successful
+   */
+  virtual bool init(ros::NodeHandle& root_nh, ros::NodeHandle &robot_hw_nh);
+
+
+  /**
+   * Check (in non-realtime) if given controllers could be started and stopped from the current state of the RobotHW
+   * with regard to necessary hardware interface switches and prepare the switching. Start and stop list are disjoint.
+   * This handles the check and preparation, the actual switch is commited in doSwitch()
+   */
+  virtual bool prepareSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
+                             const std::list<hardware_interface::ControllerInfo>& stop_list);
+
+  /**
+   * Perform (in realtime) all necessary hardware interface switches in order to start and stop the given controllers.
+   * Start and stop list are disjoint. The feasability was checked in prepareSwitch() beforehand.
+   */
+  virtual void doSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
+                        const std::list<hardware_interface::ControllerInfo>& stop_list);
+
+  /**
+   * Reads data from the robot HW
+   *
+   * \param time The current time
+   * \param period The time passed since the last call to \ref read
+   */
+  virtual void read(const ros::Time& time, const ros::Duration& period);
+
+  /**
+   * Writes data to the robot HW
+   *
+   * \param time The current time
+   * \param period The time passed since the last call to \ref write
+   */
+  virtual void write(const ros::Time& time, const ros::Duration& period);
+
+protected:
+  ros::NodeHandle root_nh_;
+  ros::NodeHandle robot_hw_nh_;
+  pluginlib::ClassLoader<hardware_interface::RobotHW> robot_hw_loader_;
+  std::vector<boost::shared_ptr<hardware_interface::RobotHW> > robot_hw_list_;
+
+  virtual bool loadRobotHW(const std::string& name);
+
+  /** \brief Filters the start and stop lists so that they only contain the controllers and
+   * resources that correspond to the robot_hw interface manager
+   */
+  void filterControllerList(const std::list<hardware_interface::ControllerInfo>& list,
+                            std::list<hardware_interface::ControllerInfo>& filtered_list,
+                            boost::shared_ptr<hardware_interface::RobotHW> robot_hw);
+};
+
+}
+
+#endif

combined_robot_hw/package.xml

@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+<package>
+  <name>combined_robot_hw</name>
+  <version>0.0.0</version>
+  <description>Combined Robot HW class.</description>
+  <maintainer email="[email protected]">Toni Oliver</maintainer>
+
+  <license>BSD</license>
+
+  <url type="website">https://github.com/ros-controls/ros_control/wiki</url>
+  <url type="bugtracker">https://github.com/ros-controls/ros_control/issues</url>
+  <url type="repository">https://github.com/ros-controls/ros_control</url>
+
+  <author>Toni Oliver</author>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>hardware_interface</build_depend>
+  <build_depend>pluginlib</build_depend>
+  <build_depend>roscpp</build_depend>
+  <run_depend>hardware_interface</run_depend>
+  <run_depend>pluginlib</run_depend>
+  <run_depend>roscpp</run_depend>
+
+
+  <export>
+  </export>
+</package>