Non-naive ClaimedResource merging.

controller_interface/CMakeLists.txt

@@ -14,3 +14,8 @@ catkin_package(
 install(DIRECTORY include/${PROJECT_NAME}/
   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})
 
+if(CATKIN_ENABLE_TESTING)
+  include_directories(include ${catkin_INCLUDE_DIRS})
+  set_directory_properties(PROPERTIES COMPILE_OPTIONS "-std=c++11")
+  catkin_add_gtest(composite_controller_test test/composite_controller_test.cpp)
+endif()

controller_interface/include/controller_interface/composite_controller.h

@@ -38,6 +38,56 @@
 namespace controller_interface
 {
 
+/**
+ * \brief Merge together two ClaimedResources vectors, detecting whether the
+ * any of the merged-in resources were already present. The merge will be
+ * fully executed even if the function returns false (eg, indicating a conflict).
+ *
+ * \param[out] target ClaimedResources to merge resources into.
+ *
+ * \param source ClaimedResources to draw resources from.
+ *
+ * \return true if no resources were duplicated between the target and source.
+ */
+static bool mergeClaimedResources(ControllerBase::ClaimedResources& target,
+                                  const ControllerBase::ClaimedResources& source)
+{
+  bool conflict = false;
+  for (const auto& claimed : source)
+  {
+    // Check if there's a ClaimedResource of this interface type already in the target.
+    bool resource_in_target = false;
+    for (auto& target_claimed : target)
+    {
+      if (target_claimed.hardware_interface == claimed.hardware_interface)
+      {
+        resource_in_target = true;
+
+        size_t size_before = target_claimed.resources.size();
+        target_claimed.resources.insert(claimed.resources.begin(),
+                                        claimed.resources.end());
+
+        // Since it's a set, if the resulting size isn't big enough, we know that
+        // one or more of the inserted items was already present.
+        if (target_claimed.resources.size() < size_before + claimed.resources.size())
+        {
+          conflict = true;
+        }
+
+        break;
+      }
+    }
+
+    if (!resource_in_target)
+    {
+      // There isn't, so append the whole ClaimedResource to the vector.
+      target.push_back(claimed);
+    }
+  }
+  return !conflict;
+}
+
+
 template <typename... Controllers>
 class CompositeController: public virtual ControllerBase, public Controllers...
 {
@@ -193,8 +243,8 @@ class CompositeController: public virtual ControllerBase, public Controllers...
                         ros::NodeHandle&             controller_nh,
                         ClaimedResources&            all_claimed_resources)
   {
-    ClaimedResources cr;
-    bool ret = Controller::initRequest(robot_hw, root_nh, controller_nh, cr);
+    ClaimedResources claimed_resources;
+    bool ret = Controller::initRequest(robot_hw, root_nh, controller_nh, claimed_resources);
     if (state_ != INITIALIZED)
     {
       return false;
@@ -204,8 +254,15 @@ class CompositeController: public virtual ControllerBase, public Controllers...
     // and can also initialize themselves.
     state_ = CONSTRUCTED;
 
-    // Add the resources claimed by this specific controller to the overall list.
-    all_claimed_resources.insert(std::end(all_claimed_resources), std::begin(cr), std::end(cr));
+    // Add the resources claimed by this specific controller to the overall list, checking
+    // first that they aren't already in there (as that would be a conflict).
+    if (!mergeClaimedResources(all_claimed_resources, claimed_resources))
+    {
+      ROS_ERROR_STREAM("Unable to initialize composed controller; one or more resources "
+                       "claimed by multiple sub-controllers.");
+      return false;
+    }
+
     return ret;
   }
 

controller_interface/test/composite_controller_test.cpp

@@ -0,0 +1,71 @@
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2017, Clearpath Robotics
+//
+// 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 Clearpath Robotics 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 Mike Purvis
+
+#include <gtest/gtest.h>
+#include <controller_interface/controller_base.h>
+#include <controller_interface/composite_controller.h>
+
+TEST(CompositeControllerTests, checkMerge)
+{
+  typedef controller_interface::ControllerBase::ClaimedResources ClaimedResources;
+  using hardware_interface::InterfaceResources;
+
+  ClaimedResources base({
+    InterfaceResources("FooInterface", {"a", "b"}),
+    InterfaceResources("BarInterface", {"c", "d", "e"}),
+    InterfaceResources("BazInterface", {"e", "f", "g", "h"})
+  });
+
+  // Should succeed, none of these overlap with what's in the base claims.
+  ClaimedResources merge_success({
+    InterfaceResources("FzzInterface", {"a"}),
+    InterfaceResources("BarInterface", {"a", "b"}),
+    InterfaceResources("FooInterface", {"c", "f", "g"})
+  });
+  ASSERT_TRUE(controller_interface::mergeClaimedResources(base, merge_success));
+  ASSERT_EQ(std::set<std::string>({"a", "b", "c", "f", "g"}), base[0].resources);
+  ASSERT_EQ(std::set<std::string>({"a", "b", "c", "d", "e"}), base[1].resources);
+  ASSERT_EQ(std::set<std::string>({"e", "f", "g", "h"}), base[2].resources);
+  ASSERT_EQ(std::set<std::string>({"a"}), base[3].resources);
+  ASSERT_EQ(4, base.size());
+
+  // Should fail because "e" is already in BarInterface, above.
+  ClaimedResources merge_fail({
+    InterfaceResources("BarInterface", {"e", "f", "g"})
+  });
+  ASSERT_FALSE(controller_interface::mergeClaimedResources(base, merge_fail));
+  ASSERT_EQ(std::set<std::string>({"a", "b", "c", "d", "e", "f", "g"}), base[1].resources);
+  ASSERT_EQ(4, base.size());
+}
+
+int main(int argc, char** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}