[Personalrobots-commit] SF.net SVN: personalrobots:[11699] pkg/trunk

[<stu...@us...>]

Revision: 11699
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=11699&view=rev
Author:   stuglaser
Date:     2009-02-24 23:56:09 +0000 (Tue, 24 Feb 2009)

Log Message:
-----------
Added service for atomic replacement of controllers.

Modified Paths:
--------------
    pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h
    pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp

Added Paths:
-----------
    pkg/trunk/prcore/robot_srvs/srv/KillAndSpawnControllers.srv

Modified: pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h
===================================================================
--- pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h	2009-02-24 23:54:11 UTC (rev 11698)
+++ pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h	2009-02-24 23:56:09 UTC (rev 11699)
@@ -56,6 +56,7 @@
 #include <robot_srvs/ListControllers.h>
 #include <robot_srvs/SpawnController.h>
 #include <robot_srvs/KillController.h>
+#include <robot_srvs/KillAndSpawnControllers.h>
 #include <robot_srvs/SwitchController.h>
 #include <robot_msgs/MechanismState.h>
 #include <robot_msgs/DiagnosticMessage.h>
@@ -161,6 +162,8 @@
                        robot_srvs::ListControllers::Response &resp);
   bool spawnController(robot_srvs::SpawnController::Request &req,
                        robot_srvs::SpawnController::Response &resp);
+  bool killAndSpawnControllers(robot_srvs::KillAndSpawnControllers::Request &req,
+                               robot_srvs::KillAndSpawnControllers::Response &resp);
   bool switchController(robot_srvs::SwitchController::Request &req,
                         robot_srvs::SwitchController::Response &resp);
 
@@ -184,7 +187,8 @@
   realtime_tools::RealtimePublisher<tf::tfMessage> transform_publisher_;
 
   AdvertisedServiceGuard list_controllers_guard_, list_controller_types_guard_,
-    spawn_controller_guard_, kill_controller_guard_, switch_controller_guard_;
+    spawn_controller_guard_, kill_controller_guard_, switch_controller_guard_,
+    kill_and_spawn_controllers_guard_;
 };
 
 #endif /* MECHANISM_CONTROL_H */

Modified: pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp	2009-02-24 23:54:11 UTC (rev 11698)
+++ pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp	2009-02-24 23:56:09 UTC (rev 11699)
@@ -465,6 +465,8 @@
   kill_controller_guard_.set("kill_controller");
   node_->advertiseService("switch_controller", &MechanismControlNode::switchController, this);
   switch_controller_guard_.set("switch_controller");
+  node_->advertiseService("kill_and_spawn_controllers", &MechanismControlNode::killAndSpawnControllers, this);
+  kill_and_spawn_controllers_guard_.set("kill_and_spawn_controllers");
 
   return true;
 }
@@ -653,6 +655,80 @@
   return true;
 }
 
+bool MechanismControlNode::killAndSpawnControllers(
+  robot_srvs::KillAndSpawnControllers::Request &req,
+  robot_srvs::KillAndSpawnControllers::Response &resp)
+{
+  std::vector<MechanismControl::AddReq> add_reqs;
+  std::vector<MechanismControl::RemoveReq> remove_reqs;
+
+  TiXmlDocument doc;
+  doc.Parse(req.add_config.c_str());
+
+  TiXmlElement *config = doc.RootElement();
+  if (!config)
+  {
+    ROS_ERROR("The XML given to SpawnController could not be parsed");
+    return false;
+  }
+  if (config->ValueStr() != "controllers" &&
+      config->ValueStr() != "controller")
+  {
+    ROS_ERROR("The XML given to SpawnController must have either \"controller\" or \
+\"controllers\" as the root tag");
+    return false;
+  }
+
+  if (config->ValueStr() == "controllers")
+  {
+    config = config->FirstChildElement("controller");
+  }
+
+  for (; config; config = config->NextSiblingElement("controller"))
+  {
+    bool ok = true;
+
+    if (!config->Attribute("type"))
+    {
+      ROS_ERROR("Could not spawn a controller because no type was given");
+      ok = false;
+    }
+    else if (!config->Attribute("name"))
+    {
+      ROS_ERROR("Could not spawn a controller because no name was given");
+      ok = false;
+    }
+    else
+    {
+      int last = add_reqs.size();
+      add_reqs.resize(last + 1);
+      add_reqs[last].type = config->Attribute("type");
+      add_reqs[last].name = config->Attribute("name");
+      add_reqs[last].config = config;
+    }
+  }
+
+  remove_reqs.resize(req.kill_name.size());
+  for (size_t i = 0; i < req.kill_name.size(); ++i)
+    remove_reqs[i].name = req.kill_name[i];
+
+  mc_->changeControllers(remove_reqs, add_reqs);
+
+  resp.add_ok.resize(add_reqs.size());
+  resp.add_name.resize(add_reqs.size());
+  for (size_t i = 0; i < add_reqs.size(); ++i)
+  {
+    resp.add_name[i] = add_reqs[i].name;
+    resp.add_ok[i] = add_reqs[i].success;
+  }
+
+  resp.kill_ok.resize(remove_reqs.size());
+  for (size_t i = 0; i < remove_reqs.size(); ++i)
+    resp.kill_ok[i] = remove_reqs[i].success;
+
+  return true;
+}
+
 bool MechanismControlNode::switchController(
   robot_srvs::SwitchController::Request &req,
   robot_srvs::SwitchController::Response &resp)

Added: pkg/trunk/prcore/robot_srvs/srv/KillAndSpawnControllers.srv
===================================================================
--- pkg/trunk/prcore/robot_srvs/srv/KillAndSpawnControllers.srv	                        (rev 0)
+++ pkg/trunk/prcore/robot_srvs/srv/KillAndSpawnControllers.srv	2009-02-24 23:56:09 UTC (rev 11699)
@@ -0,0 +1,6 @@
+string add_config
+string[] kill_name
+---
+string[] add_name
+uint8[] add_ok
+uint8[] kill_ok


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.