From dc82ef75c006d23b11e6ff56a6aee3533ca016d1 Mon Sep 17 00:00:00 2001
From: Esteve Fernandez <esteve@apache.org>
Date: Mon, 8 Jun 2015 09:52:15 -0700
Subject: [PATCH] Publish parameter events

---
 rclcpp/include/rclcpp/node.hpp             |  3 +++
 rclcpp/include/rclcpp/node_impl.hpp        | 26 +++++++++++++++++-----
 rclcpp/include/rclcpp/parameter_client.hpp | 23 +++++++++++++------
 3 files changed, 40 insertions(+), 12 deletions(-)

diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp
index 7459598..f0bbafd 100644
--- a/rclcpp/include/rclcpp/node.hpp
+++ b/rclcpp/include/rclcpp/node.hpp
@@ -22,6 +22,7 @@
 
 #include <rcl_interfaces/msg/list_parameters_result.hpp>
 #include <rcl_interfaces/msg/parameter_descriptor.hpp>
+#include <rcl_interfaces/msg/parameter_event.hpp>
 #include <rcl_interfaces/msg/set_parameters_result.hpp>
 
 #include <rclcpp/callback_group.hpp>
@@ -201,6 +202,8 @@ private:
 
   std::map<std::string, rclcpp::parameter::ParameterVariant> parameters_;
 
+  publisher::Publisher::SharedPtr events_publisher_;
+
   template<
     typename ServiceT,
     typename FunctorT,
diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp
index 4680ef1..72f9b5b 100644
--- a/rclcpp/include/rclcpp/node_impl.hpp
+++ b/rclcpp/include/rclcpp/node_impl.hpp
@@ -46,6 +46,9 @@ Node::Node(std::string node_name, context::Context::SharedPtr context)
   using rclcpp::callback_group::CallbackGroupType;
   default_callback_group_ = \
     create_callback_group(CallbackGroupType::MutuallyExclusive);
+  // TODO(esteve): remove hardcoded values
+  events_publisher_ =
+    create_publisher<rcl_interfaces::msg::ParameterEvent>("parameter_events", 1000);
 }
 
 rclcpp::callback_group::CallbackGroup::SharedPtr
@@ -217,13 +220,9 @@ std::vector<rcl_interfaces::msg::SetParametersResult>
 Node::set_parameters(
   const std::vector<rclcpp::parameter::ParameterVariant> & parameters)
 {
-  std::lock_guard<std::mutex> lock(mutex_);
   std::vector<rcl_interfaces::msg::SetParametersResult> results;
   for (auto p : parameters) {
-    parameters_[p.get_name()] = p;
-    rcl_interfaces::msg::SetParametersResult result;
-    result.successful = true;
-    // TODO: handle parameter constraints
+    auto result = set_parameters_atomically({{p}});
     results.push_back(result);
   }
   return results;
@@ -235,14 +234,31 @@ Node::set_parameters_atomically(
 {
   std::lock_guard<std::mutex> lock(mutex_);
   std::map<std::string, rclcpp::parameter::ParameterVariant> tmp_map;
+  auto parameter_event = std::make_shared<rcl_interfaces::msg::ParameterEvent>();
+
   for (auto p : parameters) {
+    if (parameters_.find(p.get_name()) == parameters_.end()) {
+      if (p.get_type() != rclcpp::parameter::ParameterType::PARAMETER_NOT_SET) {
+        parameter_event->new_parameters.push_back(p.to_parameter());
+      }
+    } else if (p.get_type() != rclcpp::parameter::ParameterType::PARAMETER_NOT_SET) {
+      parameter_event->changed_parameters.push_back(p.to_parameter());
+    } else {
+      parameter_event->deleted_parameters.push_back(p.to_parameter());
+    }
     tmp_map[p.get_name()] = p;
   }
+  // std::map::insert will not overwrite elements, so we'll keep the new
+  // ones and add only those that already exist in the Node's internal map
   tmp_map.insert(parameters_.begin(), parameters_.end());
   std::swap(tmp_map, parameters_);
+
   // TODO: handle parameter constraints
   rcl_interfaces::msg::SetParametersResult result;
   result.successful = true;
+
+  events_publisher_->publish(parameter_event);
+
   return result;
 }
 
diff --git a/rclcpp/include/rclcpp/parameter_client.hpp b/rclcpp/include/rclcpp/parameter_client.hpp
index bc9c398..60543ea 100644
--- a/rclcpp/include/rclcpp/parameter_client.hpp
+++ b/rclcpp/include/rclcpp/parameter_client.hpp
@@ -25,6 +25,7 @@
 #include <rclcpp/parameter.hpp>
 
 #include <rcl_interfaces/msg/parameter.hpp>
+#include <rcl_interfaces/msg/parameter_event.hpp>
 #include <rcl_interfaces/msg/parameter_value.hpp>
 #include <rcl_interfaces/srv/describe_parameters.hpp>
 #include <rcl_interfaces/srv/get_parameters.hpp>
@@ -136,9 +137,8 @@ public:
   std::shared_future<std::vector<rcl_interfaces::msg::SetParametersResult>>
   set_parameters(
     std::vector<rclcpp::parameter::ParameterVariant> parameters,
-    std::function<
-      void(std::shared_future<std::vector<rcl_interfaces::msg::SetParametersResult>>)
-    > callback = nullptr)
+    std::function<void(std::shared_future<std::vector<rcl_interfaces::msg::SetParametersResult>>)> callback =
+    nullptr)
   {
     std::promise<std::vector<rcl_interfaces::msg::SetParametersResult>> promise_result;
     auto future_result = promise_result.get_future().share();
@@ -166,8 +166,8 @@ public:
   std::shared_future<rcl_interfaces::msg::SetParametersResult>
   set_parameters_atomically(
     std::vector<rclcpp::parameter::ParameterVariant> parameters,
-    std::function<void(
-      std::shared_future<rcl_interfaces::msg::SetParametersResult>)> callback = nullptr)
+    std::function<void(std::shared_future<rcl_interfaces::msg::SetParametersResult>)> callback =
+    nullptr)
   {
     std::promise<rcl_interfaces::msg::SetParametersResult> promise_result;
     auto future_result = promise_result.get_future().share();
@@ -196,8 +196,8 @@ public:
   list_parameters(
     std::vector<std::string> prefixes,
     uint64_t depth,
-    std::function<void(
-      std::shared_future<rcl_interfaces::msg::ListParametersResult>)> callback = nullptr)
+    std::function<void(std::shared_future<rcl_interfaces::msg::ListParametersResult>)> callback =
+    nullptr)
   {
     std::promise<rcl_interfaces::msg::ListParametersResult> promise_result;
     auto future_result = promise_result.get_future().share();
@@ -220,6 +220,15 @@ public:
     return future_result;
   }
 
+  template<typename FunctorT>
+  typename rclcpp::subscription::Subscription<rcl_interfaces::msg::ParameterEvent>::SharedPtr
+  on_parameter_event(FunctorT callback)
+  {
+    // TODO(esteve): remove hardcoded values
+    return node_->create_subscription<rcl_interfaces::msg::ParameterEvent>("parameter_events",
+             1000, callback);
+  }
+
 private:
   const rclcpp::node::Node::SharedPtr node_;
   rclcpp::client::Client<rcl_interfaces::srv::GetParameters>::SharedPtr get_parameters_client_;