RobotHWGroup Implementation

hardware_interface/CMakeLists.txt

@@ -17,6 +17,7 @@ if(USE_ROSBUILD)
   rosbuild_add_gtest(force_torque_sensor_interface_test test/force_torque_sensor_interface_test.cpp)
   rosbuild_add_gtest(imu_sensor_interface_test          test/imu_sensor_interface_test.cpp)
   rosbuild_add_gtest(robot_hw_test                      test/robot_hw_test.cpp)
+  rosbuild_add_gtest(robot_hw_group_test                test/robot_hw_group_test.cpp)
   rosbuild_add_gtest(interface_manager_test             test/interface_manager_test.cpp)
 
   # TODO: why is it explicitly needed???, without it the linker fails.
@@ -28,6 +29,7 @@ if(USE_ROSBUILD)
   target_link_libraries(force_torque_sensor_interface_test pthread)
   target_link_libraries(imu_sensor_interface_test          pthread)
   target_link_libraries(robot_hw_test                      pthread)
+  target_link_libraries(robot_hw_group_test                pthread)
 
 else()
 
@@ -66,6 +68,9 @@ else()
     catkin_add_gtest(robot_hw_test                   test/robot_hw_test.cpp)
     target_link_libraries(robot_hw_test ${catkin_LIBRARIES})
 
+    catkin_add_gtest(robot_hw_group_test             test/robot_hw_group_test.cpp)
+    target_link_libraries(robot_hw_group_test ${catkin_LIBRARIES})
+
     catkin_add_gtest(interface_manager_test          test/interface_manager_test.cpp)
     target_link_libraries(interface_manager_test ${catkin_LIBRARIES})
   endif()

hardware_interface/include/hardware_interface/internal/interface_manager.h

@@ -77,11 +77,11 @@ class InterfaceManager
   template<class T>
   T* get()
   {
-    InterfaceMap::iterator it = interfaces_.find(internal::demangledTypeName<T>());
-    if (it == interfaces_.end())
+    void* iface_data = findInterfaceData(internal::demangledTypeName<T>());
+    if(!iface_data)
       return NULL;
 
-    T* iface = static_cast<T*>(it->second);
+    T* iface = static_cast<T*>(iface_data);
     if (!iface)
     {
       ROS_ERROR_STREAM("Failed reconstructing type T = '" << internal::demangledTypeName<T>().c_str() <<
@@ -91,6 +91,23 @@ class InterfaceManager
     return iface;
   }
 
+  /**
+   * \brief Get generic pointer to interface with type_name.
+   *
+   * This is used as a polymorphic lookup for the templated
+   * get() call, which can't be virtual.
+   *
+   * \param type_name The name of the interface type stored.
+   * \return Generic pointer to the interface.
+   */
+  virtual void* findInterfaceData(std::string type_name)
+  {
+    InterfaceMap::iterator it = interfaces_.find(type_name);
+    if (it == interfaces_.end())
+      return NULL;
+    return it->second;
+  }
+
 protected:
   typedef std::map<std::string, void*> InterfaceMap;
   InterfaceMap interfaces_;

hardware_interface/include/hardware_interface/robot_hw_group.h

@@ -0,0 +1,96 @@
+///////////////////////////////////////////////////////////////////////////////
+//
+// 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: Kelsey Hawkins
+ */
+
+#ifndef HARDWARE_INTERFACE_ROBOT_HW_GROUP_H
+#define HARDWARE_INTERFACE_ROBOT_HW_GROUP_H
+
+#include <vector>
+#include <hardware_interface/robot_hw.h>
+#include <ros/console.h>
+
+namespace hardware_interface
+{
+
+/** \brief Robot Hardware Collection Manager
+ *
+ * This class maintains a group of RobotHW, along with its
+ * own registered interfaces, and exposes an interface
+ * identical to RobotHW.
+ *
+ */
+class RobotHWGroup : public RobotHW
+{
+public:
+  RobotHWGroup()
+  {
+
+  }
+
+  void registerHardware(RobotHW* hw)
+  {
+    hardware_.push_back(hw);
+  }
+
+  /**
+   * \brief Get generic pointer to interface with type_name.
+   *
+   * This overloads the base implementation to look across both
+   * local interfaces and interfaces in all registered RobotHW.
+   * This is used by the get() call in the base class.
+   *
+   * \param type_name The name of the interface type stored.
+   * \return Generic pointer to the interface.
+   */
+  virtual void* findInterfaceData(std::string type_name)
+  {
+    // look for interfaces registered here
+    void* iface_data = InterfaceManager::findInterfaceData(type_name);
+    if(iface_data != NULL)
+      return iface_data;
+
+    // look for interfaces registered in the registered hardware
+    for(HardwareVector::iterator it = hardware_.begin(); it != hardware_.end(); ++it) {
+      iface_data = (*it)->findInterfaceData(type_name);
+      if(iface_data != NULL)
+        return iface_data;
+    }
+    return NULL;
+  }
+
+protected:
+  typedef std::vector<RobotHW*> HardwareVector;
+  HardwareVector hardware_;
+
+};
+
+}
+
+#endif
+

hardware_interface/test/robot_hw_group_test.cpp

@@ -0,0 +1,212 @@
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2013, PAL Robotics S.L.
+//
+// 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 Adolfo Rodriguez Tsouroukdissian
+/// \author Kelsey Hawkins
+
+#include <list>
+#include <set>
+#include <string>
+#include <gtest/gtest.h>
+#include <hardware_interface/joint_command_interface.h>
+#include <hardware_interface/robot_hw.h>
+#include <hardware_interface/robot_hw_group.h>
+
+using std::list;
+using std::string;
+using namespace hardware_interface;
+
+class RobotHWGroupTest : public ::testing::Test
+{
+public:
+  RobotHWGroupTest()
+    : pos1(1.0), vel1(2.0), eff1(3.0), cmd1(0.0),
+      pos2(4.0), vel2(5.0), eff2(6.0), cmd2(0.0),
+      name1("name_1"),
+      name2("name_2"),
+      hs1(name1, &pos1, &vel1, &eff1),
+      hs2(name2, &pos2, &vel2, &eff2),
+      hc1(hs1, &cmd1),
+      hc2(hs2, &cmd2)
+  {}
+
+protected:
+  double pos1, vel1, eff1, cmd1;
+  double pos2, vel2, eff2, cmd2;
+  string name1;
+  string name2;
+  JointStateHandle hs1, hs2;
+  JointHandle hc1, hc2;
+};
+
+TEST_F(RobotHWGroupTest, GroupTest)
+{
+  // Populate hardware interfaces
+  JointStateInterface state_iface;
+  state_iface.registerHandle(hs1);
+  state_iface.registerHandle(hs2);
+
+  EffortJointInterface eff_cmd_iface;
+  eff_cmd_iface.registerHandle(hc1);
+  eff_cmd_iface.registerHandle(hc2);
+
+  PositionJointInterface pos_cmd_iface;
+  pos_cmd_iface.registerHandle(hc1);
+  pos_cmd_iface.registerHandle(hc2);
+
+  // Register them to different RobotHW instances
+  RobotHW hw1, hw2;
+  RobotHWGroup hw_grp;
+  hw1.registerInterface(&state_iface);
+  hw2.registerInterface(&eff_cmd_iface);
+  hw_grp.registerInterface(&pos_cmd_iface);
+
+  hw_grp.registerHardware(&hw1);
+  hw_grp.registerHardware(&hw2);
+
+  // Get interfaces
+  EXPECT_TRUE(&state_iface   == hw_grp.get<JointStateInterface>());
+  EXPECT_TRUE(&eff_cmd_iface == hw_grp.get<EffortJointInterface>());
+  EXPECT_TRUE(&pos_cmd_iface == hw_grp.get<PositionJointInterface>());
+  EXPECT_FALSE(hw_grp.get<VelocityJointInterface>());
+}
+
+TEST_F(RobotHWGroupTest, InterfaceRegistration)
+{
+  // Populate hardware interfaces
+  JointStateInterface state_iface;
+  state_iface.registerHandle(hs1);
+  state_iface.registerHandle(hs2);
+
+  EffortJointInterface eff_cmd_iface;
+  eff_cmd_iface.registerHandle(hc1);
+  eff_cmd_iface.registerHandle(hc2);
+
+  PositionJointInterface pos_cmd_iface;
+  pos_cmd_iface.registerHandle(hc1);
+  pos_cmd_iface.registerHandle(hc2);
+
+  // Register them to a RobotHW instance
+  RobotHW hw;
+  hw.registerInterface(&state_iface);
+  hw.registerInterface(&eff_cmd_iface);
+  hw.registerInterface(&pos_cmd_iface);
+
+  RobotHWGroup hw_grp;
+  hw_grp.registerHardware(&hw);
+
+  // Get interfaces
+  EXPECT_TRUE(&state_iface   == hw_grp.get<JointStateInterface>());
+  EXPECT_TRUE(&eff_cmd_iface == hw_grp.get<EffortJointInterface>());
+  EXPECT_TRUE(&pos_cmd_iface == hw_grp.get<PositionJointInterface>());
+  EXPECT_FALSE(hw_grp.get<VelocityJointInterface>());
+}
+
+TEST_F(RobotHWGroupTest, InterfaceRewriting)
+{
+  // Two hardware interfaces of the same type, with different joints
+  JointStateInterface state1_iface;
+  state1_iface.registerHandle(hs1);
+
+  JointStateInterface state2_iface;
+  state2_iface.registerHandle(hs1);
+  state2_iface.registerHandle(hs2);
+
+  // Register first interface and validate it
+  RobotHW hw;
+  hw.registerInterface(&state1_iface);
+
+  RobotHWGroup hw_grp;
+  hw_grp.registerHardware(&hw);
+
+  JointStateInterface* state_iface_ptr = hw_grp.get<JointStateInterface>();
+  EXPECT_EQ(1, state_iface_ptr->getNames().size());
+
+  // Register second interface and verify that it has taken the place of the previously inserted one
+  hw_grp.registerInterface(&state2_iface);
+  state_iface_ptr = hw_grp.get<JointStateInterface>();
+  EXPECT_EQ(2, state_iface_ptr->getNames().size());
+}
+
+TEST_F(RobotHWGroupTest, ConflictChecking)
+{
+  ControllerInfo info1;
+  info1.name = "controller_1";
+  info1.type = "type_1";
+  info1.hardware_interface = "interface_1";
+  info1.resources.insert("resource_1");
+
+  ControllerInfo info2;
+  info2.name = "controller_2";
+  info2.type = "type_2";
+  info2.hardware_interface = "interface_2";
+  info2.resources.insert("resource_2");
+
+  ControllerInfo info12;
+  info12.name = "controller_12";
+  info12.type = "type_12";
+  info12.hardware_interface = "interface_12";
+  info12.resources.insert("resource_1");
+  info12.resources.insert("resource_2");
+
+  // No conflict
+  {
+    list<ControllerInfo> info_list;
+    info_list.push_back(info1);
+    info_list.push_back(info2);
+
+    RobotHWGroup hw_grp;
+    EXPECT_FALSE(hw_grp.checkForConflict(info_list));
+  }
+
+  // Conflict
+  {
+    list<ControllerInfo> info_list;
+    info_list.push_back(info1);
+    info_list.push_back(info12);
+
+    RobotHWGroup hw_grp;
+    EXPECT_TRUE(hw_grp.checkForConflict(info_list));
+  }
+
+  // Conflict
+  {
+    list<ControllerInfo> info_list;
+    info_list.push_back(info2);
+    info_list.push_back(info12);
+
+    RobotHWGroup hw_grp;
+    EXPECT_TRUE(hw_grp.checkForConflict(info_list));
+  }
+}
+
+int main(int argc, char** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
+