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

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

Revision: 2841
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2841&view=rev
Author:   stuglaser
Date:     2008-08-08 23:18:51 +0000 (Fri, 08 Aug 2008)

Log Message:
-----------
Changed spawnController in MechanismControl to take a name for the controller.

Modified Paths:
--------------
    pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_actuators.h
    pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_actuators.cpp
    pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h
    pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp

Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_actuators.h
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_actuators.h	2008-08-08 23:01:41 UTC (rev 2840)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_actuators.h	2008-08-08 23:18:51 UTC (rev 2841)
@@ -60,8 +60,8 @@
 
   Model *parent_model_;
   HardwareInterface hw_;
-  MechanismControlNode mc_;
-  ros::node *rosnode_;
+  MechanismControl mc_;
+  MechanismControlNode mcn_;
 
   // Each joint in joints_ corresponds to the joint with the same
   // index in mech_joints_.  The mech_joints_ vector exists so that

Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_actuators.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_actuators.cpp	2008-08-08 23:01:41 UTC (rev 2840)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_actuators.cpp	2008-08-08 23:18:51 UTC (rev 2841)
@@ -48,23 +48,12 @@
 GZ_REGISTER_DYNAMIC_CONTROLLER("gazebo_actuators", GazeboActuators);
 
 GazeboActuators::GazeboActuators(Entity *parent)
-  : Controller(parent), hw_(0), mc_(&hw_)
+  : Controller(parent), hw_(0), mc_(&hw_), mcn_(&mc_)
 {
-  this->parent_model_ = dynamic_cast<Model*>(this->parent);
+   this->parent_model_ = dynamic_cast<Model*>(this->parent);
 
-  if (!this->parent_model_)
-    gzthrow("GazeboActuators controller requires a Model as its parent");
-
-  rosnode_ = ros::g_node;
-  int argc = 0;
-  char** argv = NULL;
-  if (rosnode_ == NULL)
-  {
-    // start a ros node if none exist
-    ros::init(argc,argv);
-    rosnode_ = new ros::node("ros_gazebo",ros::node::DONT_HANDLE_SIGINT);
-    printf("-------------------- starting node in GazeboActuators\n");
-  }
+   if (!this->parent_model_)
+      gzthrow("GazeboActuators controller requires a Model as its parent");
 }
 
 GazeboActuators::~GazeboActuators()

Modified: pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h
===================================================================
--- pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h	2008-08-08 23:01:41 UTC (rev 2840)
+++ pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h	2008-08-08 23:18:51 UTC (rev 2841)
@@ -49,6 +49,7 @@
 #include <generic_controllers/controller.h>
 
 #include "mechanism_control/ListControllerTypes.h"
+#include "mechanism_control/SpawnController.h"
 
 typedef controller::Controller* (*ControllerAllocator)();
 
@@ -63,8 +64,8 @@
   // Non real-time functions
   bool init(TiXmlElement* config);
   bool registerActuator(const std::string &name, int index);
-  bool addController(controller::Controller *c);
-  bool spawnController(const char* type, TiXmlElement* config);
+  bool addController(controller::Controller *c, const std::string &name);
+  bool spawnController(const std::string &type, const std::string &name, TiXmlElement *config);
 
   mechanism::Robot model_;
 
@@ -78,21 +79,25 @@
   const static int MAX_NUM_CONTROLLERS = 100;
   ros::thread::mutex controllers_mutex_;
   controller::Controller* controllers_[MAX_NUM_CONTROLLERS];
+  std::string controller_names_[MAX_NUM_CONTROLLERS];
 };
 
 /*
  * Exposes MechanismControl's interface over ROS
  */
-class MechanismControlNode : public MechanismControl, public ros::node
+class MechanismControlNode : public ros::node
 {
 public:
-  MechanismControlNode(HardwareInterface *hw);
+  MechanismControlNode(MechanismControl *mc);
   virtual ~MechanismControlNode() {}
 
   bool listControllerTypes(mechanism_control::ListControllerTypes::request &req,
                            mechanism_control::ListControllerTypes::response &resp);
+  bool spawnController(mechanism_control::SpawnController::request &req,
+                       mechanism_control::SpawnController::response &resp);
 
 private:
+  MechanismControl *mc_;
 };
 
 #endif /* MECHANISM_CONTROL_H */

Modified: pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp	2008-08-08 23:01:41 UTC (rev 2840)
+++ pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp	2008-08-08 23:18:51 UTC (rev 2841)
@@ -141,7 +141,7 @@
   controller::ControllerFactory::instance().registerType(type, f);
 }
 
-bool MechanismControl::addController(controller::Controller *c)
+bool MechanismControl::addController(controller::Controller *c, const std::string &name)
 {
   //Add controller to list of controllers in realtime-safe manner;
   controllers_mutex_.lock(); //This lock is only to prevent us from other non-realtime threads.  The realtime thread may be spinning through the list of controllers while we are in here, so we need to keep that list always in a valid state.  This is why we fully allocate and set up the controller before adding it into the list of active controllers.
@@ -152,6 +152,7 @@
     {
       spot_found = true;
       controllers_[i] = c;
+      controller_names_[i] = name;
       break;
     }
   }
@@ -166,22 +167,26 @@
   return true;
 }
 
-bool MechanismControl::spawnController(const char *type, TiXmlElement *config)
+bool MechanismControl::spawnController(const std::string &type,
+                                       const std::string &name,
+                                       TiXmlElement *config)
 {
   controller::Controller *c = controller::ControllerFactory::instance().create(type);
   if (c == NULL)
     return false;
   c->initXml(&model_, config);
 
-  return addController(c);
+  return addController(c, name);
 }
 
 
 
-MechanismControlNode::MechanismControlNode(HardwareInterface *hw)
-  : MechanismControl(hw), ros::node("MechanismControl")
+MechanismControlNode::MechanismControlNode(MechanismControl *mc)
+  : ros::node("MechanismControl"), mc_(mc)
 {
+  assert(mc != NULL);
   advertise_service("list_controller_types", &MechanismControlNode::listControllerTypes);
+  advertise_service("spawn_controller", &MechanismControlNode::spawnController);
 }
 
 
@@ -195,3 +200,12 @@
   return true;
 }
 
+bool MechanismControlNode::spawnController(
+  mechanism_control::SpawnController::request &req,
+  mechanism_control::SpawnController::response &resp)
+{
+  TiXmlDocument doc;
+  doc.Parse(req.xml_config.c_str());
+  resp.ok = mc_->spawnController(req.type, req.name, doc.RootElement());
+  return true;
+}


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