cached parameter should be unsubscribed (#2068)

* add unsubscribeCachedParam.

Signed-off-by: Tomoya.Fujita <Tomoya.Fujita@sony.com>

* unsubscribe all the cached parameters.

Signed-off-by: Tomoya.Fujita <Tomoya.Fujita@sony.com>

* add const S_string::iterator.

Signed-off-by: Tomoya.Fujita <Tomoya.Fujita@sony.com>

* delete unnecessary if statement.

Signed-off-by: Tomoya.Fujita <Tomoya.Fujita@sony.com>

* unsubscribeCachedParam should be called when parameter is deleted.

Signed-off-by: Tomoya.Fujita <Tomoya.Fujita@sony.com>

* fix parenthesis location.

Signed-off-by: Tomoya.Fujita <Tomoya.Fujita@sony.com>

clients/roscpp/include/ros/param.h

@@ -591,6 +591,17 @@ ROSCPP_DECL bool search(const std::string& key, std::string& result);
  */
 ROSCPP_DECL bool getParamNames(std::vector<std::string>& keys);
 
+/**
+ * \brief Unsubscribe cached parameter from the master
+ * \param key the cached parameter to be unsubscribed
+ */
+ROSCPP_DECL void unsubscribeCachedParam(const std::string& key);
+
+/**
+ * \brief Unsubscribe all cached parameter from the master
+ */
+ROSCPP_DECL void unsubscribeCachedParam(void);
+
 /** \brief Assign value from parameter server, with default.
  *
  * This method tries to retrieve the indicated parameter value from the

clients/roscpp/src/libros/param.cpp

@@ -232,7 +232,11 @@ bool del(const std::string& key)
   {
     boost::mutex::scoped_lock lock(g_params_mutex);
 
-    g_subscribed_params.erase(mapped_key);
+    if (g_subscribed_params.find(mapped_key) != g_subscribed_params.end())
+    {
+      g_subscribed_params.erase(mapped_key);
+      unsubscribeCachedParam(mapped_key);
+    }
     g_params.erase(mapped_key);
   }
 
@@ -797,6 +801,28 @@ void paramUpdateCallback(XmlRpc::XmlRpcValue& params, XmlRpc::XmlRpcValue& resul
   ros::param::update((std::string)params[1], params[2]);
 }
 
+void unsubscribeCachedParam(const std::string& key)
+{
+  XmlRpc::XmlRpcValue params, result, payload;
+  params[0] = this_node::getName();
+  params[1] = XMLRPCManager::instance()->getServerURI();
+  params[2] = key;
+  master::execute("unsubscribeParam", params, result, payload, false);
+}
+
+void unsubscribeCachedParam(void)
+{
+  // lock required, all of the cached parameter will be unsubscribed.
+  boost::mutex::scoped_lock lock(g_params_mutex);
+
+  for(S_string::iterator itr = g_subscribed_params.begin();
+    itr != g_subscribed_params.end(); ++itr)
+  {
+    const std::string mapped_key(*itr);
+    unsubscribeCachedParam(mapped_key);
+  }
+}
+
 void init(const M_string& remappings)
 {
   M_string::const_iterator it = remappings.begin();

clients/roscpp/src/libros/xmlrpc_manager.cpp

@@ -139,6 +139,9 @@ void XMLRPCManager::shutdown()
     return;
   }
 
+  // before shutting down, unsubscribe all the cached parameter
+  ros::param::unsubscribeCachedParam();
+
   shutting_down_ = true;
   server_thread_.join();
 

tools/rosmaster/src/rosmaster/master_api.py

@@ -462,6 +462,7 @@ def subscribeParam(self, caller_id, caller_api, key):
             val = self.param_server.subscribe_param(key, (caller_id, caller_api))
         finally:
             self.ps_lock.release()
+        mloginfo("+CACHEDPARAM [%s] by %s",key, caller_id)
         return 1, "Subscribed to parameter [%s]"%key, val
 
     @apivalidate(0, (is_api('caller_api'), non_empty_str('key'),))
@@ -486,6 +487,7 @@ def unsubscribeParam(self, caller_id, caller_api, key):
             retval = self.param_server.unsubscribe_param(key, (caller_id, caller_api))
         finally:
             self.ps_lock.release()
+        mloginfo("-CACHEDPARAM [%s] by %s",key, caller_id)
         return 1, "Unsubscribe to parameter [%s]"%key, 1