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

[<rob...@us...>]

Revision: 2863
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2863&view=rev
Author:   rob_wheeler
Date:     2008-08-09 08:06:24 +0000 (Sat, 09 Aug 2008)

Log Message:
-----------
Create a transmission factory and use it when initializing mechanism control

Modified Paths:
--------------
    pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h
    pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
    pkg/trunk/mechanism/mechanism_model/include/mechanism_model/transmission.h
    pkg/trunk/mechanism/mechanism_model/manifest.xml
    pkg/trunk/mechanism/mechanism_model/src/simple_transmission.cpp
    pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp

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-09 06:13:28 UTC (rev 2862)
+++ pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h	2008-08-09 08:06:24 UTC (rev 2863)
@@ -31,7 +31,7 @@
  * mc.registerActuator("AnActuatorName", 0);
  * mc.registerActuator("AnotherActuatorName", 0);
  * ...
- * mc.init(config);
+ * mc.initXml(config);
  *
  * mc.spawnController("JointController", controllerConfig);
  */
@@ -62,7 +62,7 @@
   void update();
 
   // Non real-time functions
-  bool init(TiXmlElement* config);
+  bool initXml(TiXmlElement* config);
   bool registerActuator(const std::string &name, int index);
   bool addController(controller::Controller *c, const std::string &name);
   bool spawnController(const std::string &type, const std::string &name, TiXmlElement *config);

Modified: pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp	2008-08-09 06:13:28 UTC (rev 2862)
+++ pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp	2008-08-09 08:06:24 UTC (rev 2863)
@@ -51,14 +51,14 @@
   return true;
 }
 
-bool MechanismControl::init(TiXmlElement* config)
+bool MechanismControl::initXml(TiXmlElement* config)
 {
   bool successful = true;
+  ros::node *node = ros::node::instance();
 
   TiXmlElement *elt;
 
-  // Constructs the joints
-  std::map<std::string, Joint*> joint_map;
+  // Construct the joints
   for (elt = config->FirstChildElement("joint"); elt; elt = elt->NextSiblingElement("joint"))
   {
     Joint *j = new Joint;
@@ -67,41 +67,16 @@
     j->initXml(elt);
   }
 
-  // Constructs the transmissions
+  // Construct the transmissions
   elt = config->FirstChildElement("transmission");
   for (; elt; elt = elt->NextSiblingElement("transmission"))
   {
-    if (0 == strcmp("SimpleTransmission", elt->Attribute("type")))
-    {
-      // Looks up the joint and the actuator used by the transmission.
-      Robot::IndexMap::iterator joint_it =
-        model_.joints_lookup_.find(elt->FirstChildElement("joint")->Attribute("name"));
-      Robot::IndexMap::iterator actuator_it =
-        model_.actuators_lookup_.find(elt->FirstChildElement("actuator")->Attribute("name"));
-      if (joint_it == model_.joints_lookup_.end())
-      {
-        // TODO: report: The joint was not declared in the XML file
-        printf("Unable to locate joint: %s\n", elt->FirstChildElement("joint")->Attribute("name"));
-        continue;
-      }
-      if (actuator_it == model_.actuators_lookup_.end())
-      {
-        // TODO: report: The actuator was not registered with mechanism control.
-        printf("Unable to locate actuator: %s\n", elt->FirstChildElement("actuator")->Attribute("name"));
-        continue;
-      }
-     Transmission *tr =
-        new SimpleTransmission(model_.joints_[joint_it->second], hw_->actuators_[actuator_it->second],
-          atof(elt->FirstChildElement("mechanicalReduction")->GetText()),
-          atof(elt->FirstChildElement("motorTorqueConstant")->GetText()),
-          atof(elt->FirstChildElement("pulsesPerRevolution")->GetText()));
-     model_.transmissions_.push_back(tr);
-    }
-    else
-    {
-      // TODO: report: Unknown transmission type
-      successful = false;
-    }
+    const char *type = elt->Attribute("type");
+    Transmission *t = TransmissionFactory::instance().create(type);
+    if (t == NULL)
+      node->log(ros::FATAL, "Unknown transmission type: %s\n", type);
+    t->initXml(elt, &model_);
+    model_.transmissions_.push_back(t);
   }
 
   initialized_ = true;

Modified: pkg/trunk/mechanism/mechanism_model/include/mechanism_model/transmission.h
===================================================================
--- pkg/trunk/mechanism/mechanism_model/include/mechanism_model/transmission.h	2008-08-09 06:13:28 UTC (rev 2862)
+++ pkg/trunk/mechanism/mechanism_model/include/mechanism_model/transmission.h	2008-08-09 08:06:24 UTC (rev 2863)
@@ -34,17 +34,35 @@
 #ifndef TRANSMISSION_H
 #define TRANSMISSION_H
 
+#include <tinyxml/tinyxml.h>
+
+#include <misc_utils/factory.h>
+
 #include "mechanism_model/joint.h"
 #include "hardware_interface/hardware_interface.h"
 
 namespace mechanism {
 
+class Robot;
+
+class Transmission;
+typedef Factory<Transmission> TransmissionFactory;
+
+#define ROS_REGISTER_TRANSMISSION(c) \
+  mechanism::Transmission *ROS_New_##c() { return new c(); }             \
+  bool ROS_TRANSMISSION_##c = \
+    mechanism::TransmissionFactory::instance().registerType(#c, ROS_New_##c);
+
+
 class Transmission
 {
 public:
   Transmission() {}
   virtual ~Transmission() {}
 
+  // Initialize transmission from XML data
+  virtual void initXml(TiXmlElement *config, Robot *robot) = 0;
+
   // Uses encoder data to fill out joint position and velocities
   virtual void propagatePosition() = 0;
 
@@ -67,6 +85,8 @@
   SimpleTransmission(Joint *joint, Actuator *actuator, double mechanical_reduction, double motor_torque_constant, double pulses_per_revolution);
   ~SimpleTransmission() {}
 
+  void initXml(TiXmlElement *config, Robot *robot);
+
   Actuator *actuator_;
   Joint *joint_;
 

Modified: pkg/trunk/mechanism/mechanism_model/manifest.xml
===================================================================
--- pkg/trunk/mechanism/mechanism_model/manifest.xml	2008-08-09 06:13:28 UTC (rev 2862)
+++ pkg/trunk/mechanism/mechanism_model/manifest.xml	2008-08-09 08:06:24 UTC (rev 2863)
@@ -3,8 +3,10 @@
 </description>
 <author>Eric Berger be...@wi...</author>
 <license>BSD</license>
+<depend package='roscpp'/>
 <depend package='hardware_interface'/>
 <depend package="stl_utils" />
+<depend package="misc_utils" />
 <depend package="tinyxml" />
 <url>http://pr.willowgarage.com</url>
 <export>

Modified: pkg/trunk/mechanism/mechanism_model/src/simple_transmission.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_model/src/simple_transmission.cpp	2008-08-09 06:13:28 UTC (rev 2862)
+++ pkg/trunk/mechanism/mechanism_model/src/simple_transmission.cpp	2008-08-09 08:06:24 UTC (rev 2863)
@@ -31,13 +31,14 @@
  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  *  POSSIBILITY OF SUCH DAMAGE.
  *********************************************************************/
-#include <mechanism_model/transmission.h>
-#include <mechanism_model/joint.h>
 #include <math.h>
-#include <stdio.h>
+#include <ros/node.h>
+#include <mechanism_model/robot.h>
 
 using namespace mechanism;
 
+ROS_REGISTER_TRANSMISSION(SimpleTransmission)
+
 SimpleTransmission::SimpleTransmission(Joint *joint, Actuator *actuator,
   double mechanical_reduction, double motor_torque_constant,
   double pulses_per_revolution)
@@ -49,6 +50,14 @@
   joint_ = joint;
 }
 
+void SimpleTransmission::initXml(TiXmlElement *elt, Robot *robot)
+{
+  joint_ = robot->getJoint(elt->FirstChildElement("joint")->Attribute("name"));
+  actuator_ = robot->getActuator(elt->FirstChildElement("actuator")->Attribute("name"));
+  mechanical_reduction_ = atof(elt->FirstChildElement("mechanicalReduction")->GetText()),
+  motor_torque_constant_ = atof(elt->FirstChildElement("motorTorqueConstant")->GetText()),
+  pulses_per_revolution_ = atof(elt->FirstChildElement("pulsesPerRevolution")->GetText());
+}
 
 void SimpleTransmission::propagatePosition()
 {

Modified: pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp
===================================================================
--- pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp	2008-08-09 06:13:28 UTC (rev 2862)
+++ pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp	2008-08-09 08:06:24 UTC (rev 2863)
@@ -69,7 +69,7 @@
   ec.initXml(root, mc);
 
   // Initialize mechanism control from robot description
-  mc.init(root);
+  mc.initXml(root);
 
   // Spawn controllers
   // TODO what file does this come from?


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