Move mimic motor management to motormanager

rock-simulation · Aug 28, 2015 · 18a719b · 18a719b
1 parent 46ed0d4
commit 18a719b
Show file tree

Hide file tree

Showing 7 changed files with 56 additions and 48 deletions.
diff --git a/entity_generation/smurf/src/smurf.cpp b/entity_generation/smurf/src/smurf.cpp
@@ -929,9 +929,7 @@ namespace mars {
         if (!loadMotor(motorList[i]))
           return 0;
 
-      for (std::map<unsigned long, std::string>::iterator it = mimicmotors.begin();
-        it != mimicmotors.end(); ++it)
-          loadMimic(it->first, it->second);
+      control->motors->connectMimics();
 
       for (unsigned int i = 0; i < sensorList.size(); ++i)
         if (!loadSensor(sensorList[i]))
@@ -1049,50 +1047,9 @@ namespace mars {
 
       entity->addMotor(motor.index, motor.name);
 
-      sim::SimMotor* newMotor = control->motors->getSimMotor(newId);
-
-      // set motor mimics
-      if (config.find("mimic_motor") != config.end()) {
-        mimicmotors[newId] = (std::string)config["mimic_motor"];
-        newMotor->setMimic(
-          (sReal)config["mimic_multiplier"], (sReal)config["mimic_offset"]);
-      }
-
-      // set approximation functions
-      if (config.find("maxeffort_approximation") != config.end()) {
-        std::vector<sReal>* maxeffort_coefficients = new std::vector<sReal>;
-        ConfigVector::iterator vIt = config["maxeffort_coefficients"].begin();
-        for (; vIt != config["maxeffort_coefficients"].end(); ++vIt) {
-          maxeffort_coefficients->push_back((double)(*vIt));
-          newMotor->setMaxEffortApproximation(
-                                              utils::getApproximationFunctionFromString((std::string)config["maxeffort_approximation"]),
-            maxeffort_coefficients);
-        }
-      }
-      if (config.find("maxspeed_approximation") != config.end()) {
-        fprintf(stderr, "found maxspeed_approximation in %s\n", ((std::string)config["name"]).c_str());
-        std::vector<sReal>* maxspeed_coefficients = new std::vector<sReal>;
-        ConfigVector::iterator vIt = config["maxspeed_coefficients"].begin();
-        for (; vIt != config["maxspeed_coefficients"].end(); ++vIt) {
-          maxspeed_coefficients->push_back((double)(*vIt));
-          newMotor->setMaxSpeedApproximation(
-                                             utils::getApproximationFunctionFromString((std::string)config["maxspeed_approximation"]),
-            maxspeed_coefficients);
-        }
-      }
       return true;
     }
 
-    void SMURF::loadMimic(unsigned long mimicId, std::string parent_name) {
-      sim::SimMotor* parentmotor =
-        control->motors->getSimMotorByName(parent_name);
-      if (parentmotor != NULL)
-        fprintf(stderr, ", %s\n", parentmotor->getName().c_str());
-      else
-        fprintf(stderr, "no parentmotor found\n");
-      parentmotor->addMimic(control->motors->getSimMotor(mimicId));
-    }
-
     BaseSensor* SMURF::loadSensor(ConfigMap config) {
       config["mapIndex"].push_back(ConfigItem(mapIndex));
       BaseSensor *sensor = control->sensors->createAndAddSensor(&config);

diff --git a/entity_generation/smurf/src/smurf.h b/entity_generation/smurf/src/smurf.h
@@ -84,7 +84,6 @@ namespace mars {
       std::vector<configmaps::ConfigMap> controllerList;
       std::vector<configmaps::ConfigMap> graphicList;
       std::vector<configmaps::ConfigMap> lightList;
-      std::map<unsigned long, std::string> mimicmotors;
 
     private:
       int groupID;
@@ -112,8 +111,6 @@ namespace mars {
       boost::shared_ptr<urdf::ModelInterface> model;
       sim::SimEntity* entity;
 
-      void loadMimic(unsigned long mimicId, std::string parent_name);
-
       void handleURI(configmaps::ConfigMap *map, std::string uri);
       void handleURIs(configmaps::ConfigMap *map);
       void getSensorIDList(configmaps::ConfigMap *map);

diff --git a/interfaces/src/MotorData.cpp b/interfaces/src/MotorData.cpp
@@ -136,6 +136,8 @@ namespace mars {
       GET_VALUE("maxPosition", maxValue, Double);
       GET_VALUE("minPosition", minValue, Double);
 
+      this->config = *config;
+
       return 1;
     }
 

diff --git a/interfaces/src/sim/MotorManagerInterface.h b/interfaces/src/sim/MotorManagerInterface.h
@@ -320,6 +320,8 @@ namespace mars {
       virtual void getDataBrokerNames(unsigned long jointId, 
                                       std::string *groupName, 
                                       std::string *dataName) const = 0;
+
+      virtual void connectMimics() = 0;
     }; // class MotorManagerInterface
 
   } // end of namespace interfaces

diff --git a/sim/src/core/MotorManager.cpp b/sim/src/core/MotorManager.cpp
@@ -41,6 +41,7 @@
 #include <mars/interfaces/sim/SimulatorInterface.h>
 #include <mars/interfaces/sim/JointManagerInterface.h>
 #include <mars/utils/MutexLocker.h>
+#include <mars/utils/mathUtils.h>
 
 namespace mars {
   namespace sim {
@@ -96,6 +97,39 @@ namespace mars {
       simMotors[newMotor->getIndex()] = newMotor;
       iMutex.unlock();
       control->sim->sceneHasChanged(false);
+
+      configmaps::ConfigMap &config = motorS->config;
+
+      // set motor mimics
+      if (config.find("mimic_motor") != config.end()) {
+        mimicmotors[motorS->index] = (std::string)config["mimic_motor"];
+        newMotor->setMimic(
+          (sReal)config["mimic_multiplier"], (sReal)config["mimic_offset"]);
+      }
+
+      // set approximation functions
+      if (config.find("maxeffort_approximation") != config.end()) {
+        std::vector<sReal>* maxeffort_coefficients = new std::vector<sReal>;
+        ConfigVector::iterator vIt = config["maxeffort_coefficients"].begin();
+        for (; vIt != config["maxeffort_coefficients"].end(); ++vIt) {
+          maxeffort_coefficients->push_back((double)(*vIt));
+          newMotor->setMaxEffortApproximation(
+            utils::getApproximationFunctionFromString((std::string)config["maxeffort_approximation"]),
+            maxeffort_coefficients);
+        }
+      }
+      if (config.find("maxspeed_approximation") != config.end()) {
+        fprintf(stderr, "found maxspeed_approximation in %s\n", ((std::string)config["name"]).c_str());
+        std::vector<sReal>* maxspeed_coefficients = new std::vector<sReal>;
+        ConfigVector::iterator vIt = config["maxspeed_coefficients"].begin();
+        for (; vIt != config["maxspeed_coefficients"].end(); ++vIt) {
+          maxspeed_coefficients->push_back((double)(*vIt));
+          newMotor->setMaxSpeedApproximation(
+            utils::getApproximationFunctionFromString((std::string)config["maxspeed_approximation"]),
+            maxspeed_coefficients);
+        }
+      }
+
       return motorS->index;
     }
 
@@ -384,9 +418,10 @@ namespace mars {
     void MotorManager::clearAllMotors(bool clear_all) {
       MutexLocker locker(&iMutex);
       map<unsigned long, SimMotor*>::iterator iter;
-      for(iter = simMotors.begin(); iter != simMotors.end(); iter++) 
+      for(iter = simMotors.begin(); iter != simMotors.end(); iter++)
         delete iter->second;
       simMotors.clear();
+      mimicmotors.clear();
       if(clear_all) simMotorsReload.clear();
       next_motor_id = 1;
     }
@@ -407,6 +442,7 @@ namespace mars {
         iMutex.lock();
       }
       iMutex.unlock();
+      connectMimics();
     }
 
     /**
@@ -487,5 +523,14 @@ namespace mars {
         iter->second->getDataBrokerNames(groupName, dataName);
     }
 
+    void MotorManager::connectMimics() {
+      std::map<unsigned long, std::string>::iterator it;
+      for (it = mimicmotors.begin(); it!=mimicmotors.end(); ++it) {
+        SimMotor* parentmotor = getSimMotorByName(it->second);
+        if (parentmotor != NULL)
+          parentmotor->addMimic(simMotors[it->first]);
+      }
+    }
+
   } // end of namespace sim
 } // end of namespace mars
diff --git a/sim/src/core/MotorManager.h b/sim/src/core/MotorManager.h
@@ -313,6 +313,8 @@ namespace mars {
                                       std::string *groupName, 
                                       std::string *dataName) const;
 
+      virtual void connectMimics();
+
     private:
       //! the id of the next motor that is added to the simulation
       unsigned long next_motor_id;
@@ -329,6 +331,8 @@ namespace mars {
       //! a mutex for the motor containters
       mutable utils::Mutex iMutex;
 
+      // map of mimicmotors
+      std::map<unsigned long, std::string> mimicmotors;
     }; // class MotorManager
 
   } // end of namespace sim

diff --git a/sim/src/core/SimMotor.cpp b/sim/src/core/SimMotor.cpp
@@ -118,6 +118,7 @@ namespace mars {
       // delete any coefficient vectors we might have created
       delete maxspeed_coefficients;
       delete maxeffort_coefficients;
+      mimics.clear();
     }
 
     void SimMotor::addMimic(SimMotor* mimic) {