use unordered map with string pair, add test for different nodes same…

… parameter, address feedback

Signed-off-by: bpwilcox <bpwilcox@eng.ucsd.edu>

rclcpp/include/rclcpp/parameter_events_subscriber.hpp

@@ -15,9 +15,9 @@
 #ifndef RCLCPP__PARAMETER_EVENTS_SUBSCRIBER_HPP_
 #define RCLCPP__PARAMETER_EVENTS_SUBSCRIBER_HPP_
 
-#include <map>
 #include <string>
 #include <utility>
+#include <unordered_map>
 #include <vector>
 
 #include "rclcpp/rclcpp.hpp"
@@ -140,9 +140,33 @@ class ParameterEventsSubscriber
 
   rclcpp::QoS qos_;
 
-  // Map containers for registered parameters
-  std::map<std::string, std::string> parameter_node_map_;
-  std::map<std::string, std::function<void(const rclcpp::Parameter &)>> parameter_callbacks_;
+  // *INDENT-OFF* Uncrustify doesn't handle indented public/private labels
+  // Hash function for string pair required in std::unordered_map
+  class stringPairHash
+  {
+  public:
+    template<typename T>
+    inline void hash_combine(std::size_t & seed, const T & v) const
+    {
+      std::hash<T> hasher;
+      seed ^= hasher(v) + 0x9e3779b9 + (seed << 6) + (seed >> 2);
+    }
+
+    inline size_t operator()(const std::pair<std::string, std::string> & s) const
+    {
+      size_t seed = 0;
+      hash_combine(seed, s.first);
+      hash_combine(seed, s.second);
+      return seed;
+    }
+  };
+  // *INDENT-ON*
+
+  // Map container for registered parameters
+  std::unordered_map<
+    std::pair<std::string, std::string>,
+    std::function<void(const rclcpp::Parameter &)>,
+    stringPairHash> parameter_callbacks_;
 
   // Vector of unique namespaces added
   std::vector<std::string> node_namespaces_;

rclcpp/src/rclcpp/parameter_events_subscriber.cpp

@@ -12,9 +12,9 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include <map>
 #include <memory>
 #include <string>
+#include <unordered_map>
 #include <utility>
 
 #include "rclcpp/parameter_events_subscriber.hpp"
@@ -57,9 +57,9 @@ void ParameterEventsSubscriber::set_event_callback(
   std::string full_namespace;
   if (node_namespace == "") {
     full_namespace = node_base_->get_namespace();
+  } else {
+    full_namespace = resolve_path(full_namespace);
   }
-
-  full_namespace = resolve_path(full_namespace);
   add_namespace_event_subscriber(full_namespace);
   user_callback_ = callback;
 }
@@ -71,8 +71,7 @@ void ParameterEventsSubscriber::register_parameter_callback(
 {
   auto full_node_name = resolve_path(node_name);
   add_namespace_event_subscriber(split_path(full_node_name).first);
-  parameter_node_map_[parameter_name] = full_node_name;
-  parameter_callbacks_[parameter_name] = callback;
+  parameter_callbacks_[{parameter_name, full_node_name}] = callback;
 }
 
 bool ParameterEventsSubscriber::get_parameter_from_event(
@@ -86,7 +85,8 @@ bool ParameterEventsSubscriber::get_parameter_from_event(
       {rclcpp::ParameterEventsFilter::EventType::NEW,
         rclcpp::ParameterEventsFilter::EventType::CHANGED});
     if (!filter.get_events().empty()) {
-      auto param_msg = filter.get_events()[0].second;
+      const auto & events = filter.get_events();
+      auto param_msg = events[events.size() - 1].second;
       parameter = rclcpp::Parameter::from_parameter_msg(*param_msg);
       return true;
     }
@@ -101,12 +101,15 @@ void ParameterEventsSubscriber::event_callback(
   RCLCPP_DEBUG(node_logging_->get_logger(), "Parameter event received for node: %s",
     node_name.c_str());
 
-  for (std::map<std::string, std::string>::iterator it = parameter_node_map_.begin();
-    it != parameter_node_map_.end(); ++it)
-  {
+  std::unordered_map<
+    std::pair<std::string, std::string>,
+    std::function<void(const rclcpp::Parameter &)>,
+    stringPairHash>::iterator it;
+
+  for (it = parameter_callbacks_.begin(); it != parameter_callbacks_.end(); ++it) {
     rclcpp::Parameter p;
-    if (get_parameter_from_event(event, p, it->first, it->second)) {
-      parameter_callbacks_[it->first](p);
+    if (get_parameter_from_event(event, p, it->first.first, it->first.second)) {
+      it->second(p);
     }
   }
 

rclcpp/test/test_parameter_events_subscriber.cpp

@@ -58,18 +58,21 @@ class TestNode : public ::testing::Test
     multiple = std::make_shared<rcl_interfaces::msg::ParameterEvent>();
     remote_node_string = std::make_shared<rcl_interfaces::msg::ParameterEvent>();
     diff_ns_bool = std::make_shared<rcl_interfaces::msg::ParameterEvent>();
+    diff_node_int = std::make_shared<rcl_interfaces::msg::ParameterEvent>();
 
     same_node_int->node = node->get_fully_qualified_name();
     same_node_double->node = node->get_fully_qualified_name();
     multiple->node = node->get_fully_qualified_name();
     remote_node_string->node = remote_node_name;
     diff_ns_bool->node = diff_ns_name;
+    diff_node_int->node = remote_node_name;
 
     rcl_interfaces::msg::Parameter p;
     p.name = "my_int";
     p.value.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER;
     p.value.integer_value = 1;
     same_node_int->changed_parameters.push_back(p);
+    diff_node_int->changed_parameters.push_back(p);
     multiple->changed_parameters.push_back(p);
 
     p.name = "my_double";
@@ -97,6 +100,7 @@ class TestNode : public ::testing::Test
 
   rcl_interfaces::msg::ParameterEvent::SharedPtr same_node_int;
   rcl_interfaces::msg::ParameterEvent::SharedPtr same_node_double;
+  rcl_interfaces::msg::ParameterEvent::SharedPtr diff_node_int;
   rcl_interfaces::msg::ParameterEvent::SharedPtr remote_node_string;
   rcl_interfaces::msg::ParameterEvent::SharedPtr multiple;
   rcl_interfaces::msg::ParameterEvent::SharedPtr diff_ns_bool;
@@ -166,6 +170,28 @@ TEST_F(TestNode, RegisterParameterUpdate)
   EXPECT_EQ(int_param, 1);
 }
 
+TEST_F(TestNode, SameParameterDifferentNode)
+{
+  int int_param_node1;
+  int int_param_node2;
+
+
+  // Set individual parameters
+  ParamSubscriber->register_parameter_update("my_int", int_param_node1);
+  ParamSubscriber->register_parameter_update("my_int", int_param_node2, remote_node_name);
+
+  ParamSubscriber->test_event(same_node_int);
+  EXPECT_EQ(int_param_node1, 1);
+  EXPECT_NE(int_param_node2, 1);
+
+  int_param_node1 = 0;
+  int_param_node2 = 0;
+
+  ParamSubscriber->test_event(diff_node_int);
+  EXPECT_NE(int_param_node1, 1);
+  EXPECT_EQ(int_param_node2, 1);
+}
+
 TEST_F(TestNode, UserCallback)
 {
   double double_param = 0.0;