Merge pull request #106 from georgeslabreche/connectors_fix

Connectors Plugin: setting properties from config, interpreting…

app/manifest.xml

@@ -25,6 +25,7 @@
     <depend package="simulation/mars/plugins/entity_view" optional="1" />
     <depend package="simulation/mars/entity_generation/smurf" optional="1" />
     <depend package="simulation/mars/smurf_loader" optional="1" />
+    <depend package="simulation/mars/plugins/connectors" optional="1" />
     <rosdep name="qt4" optional="1" />
     <tags>needs_opt</tags>
 </package>

plugins/connectors/README.md

@@ -0,0 +1,62 @@
+# Connectors Plugin
+
+## What is this?
+This plugin mates male and female connectors that are close to each other. Parameters for proximity thresholds are set for distance and angle in a connectors YAML of a model's smurf export.
+
+## Sample configuration YAML:
+
+```yaml
+    connectors:
+        autoconnect: true
+        types:
+        - name: transterra
+          distance: 0.15
+          angle: 3.1415
+          maxforce: 100
+
+        connectors:
+        - gender: male
+          link: male_connector
+          mating: automatic
+          name: payload_male_connector
+          type: transterra
+        - gender: female
+          link: female_connector
+          mating: automatic
+          name: payload_female_connector
+          type: transterra
+```
+
+## GUI
+
+### Control
+Connector connections can be manually enabled or disabled from the plugin's interface:
+ - Control > Connect available connectors.
+ - Control > Disconnect all connectors.
+
+
+### Properties
+Global autoconnect can be set from the Plugin properties interface.
+
+## Mating requirements
+
+There are 2 cases that will trigger checking for connections:
+  1. When it is forced from the Control GUI: Control > Connect available connectors
+  2. During plugin update(): **If** autoconnect is globally set to true **or if** male **and** female connectors' mating properties are both set to `automatic`.
+
+## Truth table
+
+  | case | autoconnect | male mating | female mating | connected |
+  |------|-------------|-------------|---------------|-----------|
+  | 1 | F | automatic | - | F |
+  | 2 | F | - | automatic | F |
+  | 3 | F | - | - | F |
+  | 4 | F | automatic | automatic | T |
+  | 5 | T | automatic | - | T |
+  | 6 | T | - | automatic | T |
+  | 7 | T | - | - | T |
+  | 8 | T | automatic | automatic | T |
+
+## Known issue
+Female mating is always set as 'automatic' no matter the actual value set in the YAML. This results in a failure of truth table case 1.
+This can be replicated with the sample YAML featured in this README.

plugins/connectors/build.sh

@@ -0,0 +1,16 @@
+#! /bin/bash
+
+echo  -e "\033[32;1m"
+echo "********** build MARS plugin **********"
+echo -e "\033[0m"
+
+rm -rf build
+mkdir build
+cd build
+cmake_debug
+make -j4
+cd ..
+
+echo  -e "\033[32;1m"
+echo "********** done building MARS plugin **********"
+echo -e "\033[0m"

plugins/connectors/src/Connectors.cpp

@@ -44,15 +44,22 @@ namespace mars {
       using namespace mars::utils;
       using namespace mars::interfaces;
 
-      Connectors::Connectors(lib_manager::LibManager *theManager)
-        : MarsPluginTemplateGUI(theManager, "Connectors") {
+      // To manage thread shared resources.
+      std::atomic<bool> isDisconnectionTriggered(false);
+
+      Connectors::Connectors(lib_manager::LibManager *theManager) :
+        MarsPluginTemplateGUI(theManager, "Connectors"),
+        mars::utils::Thread() {
       }
 
       void Connectors::init() {
+
         const std::map<unsigned long, sim::SimEntity*>* entities = control->entities->subscribeToEntityCreation(this);
         for (std::map<unsigned long, sim::SimEntity*>::const_iterator it = entities->begin(); it != entities->end(); ++it) {
+          // Regsiter the entity.
           registerEntity(it->second);
         }
+
         if(control->cfg) {
           cfgautoconnect = control->cfg->getOrCreateProperty("Connectors", "autoconnect", false, this);
           cfgbreakable = control->cfg->getOrCreateProperty("Connectors", "breakable", false, this);
@@ -61,11 +68,50 @@ namespace mars {
           cfgautoconnect.bValue = false;
           cfgbreakable.bValue = false;
         }
+
         gui->addGenericMenuAction("../Control/", 0, NULL, 0, "", 0, -1); // separator
         gui->addGenericMenuAction("../Control/Connect available connectors", 1, this);
         gui->addGenericMenuAction("../Control/Disconnect all connectors", 2, this);
         //maleconnectors.clear();
         //femaleconnectors.clear();
+
+        // Start thread loop.
+        // This tread will diconnect connections when they are triggered by the user from the Control GUI.
+        // At the same time, it prevents the plugin' update() method from invoking checkForPossibleConnections().
+        start();
+
+        LOG_INFO("Connectors Plugin: init was successful.");
+
+      }
+
+      void Connectors::run() {
+        // This thread is an infinite loop that checks if connections need to be disconnected and proceesds in
+        // disconnecting them if they do.
+        // This logic essentially prevents the update() method from invoking checkForPossibleConnections().'
+        while(true){
+
+          // Has disconnection been triggered by the user via the Control GUI?
+          if(isDisconnectionTriggered.load()){
+
+            // If so, then disconnect all connections.
+            for (std::map<std::string, std::string>::iterator it = connections.begin(); it!=connections.end(); ++it) {
+               disconnect(it->first);
+            }
+
+            // Sleep for a bit to allow time for disconnects to physically detach by falling far enough so that a
+            // reconnection is not just retriggered automatically if autoconnect is set to true.
+            msleep(300);
+
+            // Set atomic boolean to indicate that we are done with the disconnection.
+            // The plugin's update() call can continue invoking checkForPossibleConnections().
+            isDisconnectionTriggered = false;
+
+          }else{
+
+            // Sleep for a bit before checking again if a disconnection was triggered by the user.
+            msleep(100);
+          }
+        }
       }
 
       void Connectors::connect(std::string male, std::string female) {
@@ -86,6 +132,10 @@ namespace mars {
         }
       }
 
+      // This ia a shared resources.
+      // When invokating, make sure to wrap with !isDisconnectionTriggered.load().
+      // The method itself is not wrapped because more often than not it is contained in a for loop
+      // so it is best to wrap the for loop rather than every invokation of this method within the for loop.
       void Connectors::disconnect(std::string connector) {
         configmaps::ConfigMap* conmap = NULL;
         unsigned long jointid = 0;
@@ -122,25 +172,30 @@ namespace mars {
           (it->second)["partner"] = "";
         }
       }
+
       void Connectors::registerEntity(sim::SimEntity* entity) {
+
         configmaps::ConfigMap entitymap = entity->getConfig();
         if (entitymap.hasKey("connectors")) {
+
           configmaps::ConfigMap tmpmap;
+
           // gather types
           configmaps::ConfigVector typevec = entitymap["connectors"]["types"];
+
           for (configmaps::ConfigVector::iterator it = typevec.begin(); it!= typevec.end(); ++it) {
             tmpmap = (*it);
             connectortypes[(*it)["name"]] = tmpmap; // the one last read in determines the type
           }
+
           // gather connectors
           configmaps::ConfigVector convec = entitymap["connectors"]["connectors"];
           for (configmaps::ConfigVector::iterator it = convec.begin(); it!= convec.end(); ++it) {
             tmpmap = (*it);
             tmpmap["nodeid"] = control->nodes->getID((*it)["link"]);
             if (((std::string)((*it)["gender"])).compare("male") == 0) { // male
               maleconnectors[(std::string)(tmpmap["name"])] =  tmpmap;
-              fprintf(stderr, "Adding male connector: %s\n", ((std::string)(tmpmap["name"])).c_str(),
-            (unsigned long)(tmpmap["nodeid"]));
+              fprintf(stderr, "Adding male connector: %s\n", ((std::string)(tmpmap["name"])).c_str(), (unsigned long)(tmpmap["nodeid"]));
             } else { // female
               femaleconnectors[(std::string)(tmpmap["name"])] = tmpmap;
               fprintf(stderr, "Adding female connector: %s\n", ((std::string)(tmpmap["name"])).c_str());
@@ -155,7 +210,7 @@ namespace mars {
       Connectors::~Connectors() {
       }
 
-      bool Connectors::mated(std::string malename, std::string femalename) {
+      bool Connectors::closeEnough(std::string malename, std::string femalename) {
         unsigned long maleid = maleconnectors[malename]["nodeid"];
         unsigned long femaleid = femaleconnectors[femalename]["nodeid"];
         utils::Vector malerot = control->nodes->getRotation(maleid)*Vector(1.0, 0.0, 0.0);
@@ -171,36 +226,65 @@ namespace mars {
 
       }
 
-      void Connectors::checkForPossibleConnections() {
-        std::string malename, femalename, maletype, femaletype;
+      void Connectors::checkForPossibleConnections(bool isforced) {
+        // Connector properties.
+        std::string malename, femalename, maletype, femaletype, malemating, femalemating;
+
+        // Merge type to check against.
+        std::string automatic ("automatic");
+
+        // Nested for loop to check for every male-female connector mating combination.
         for (std::map<std::string, configmaps::ConfigMap>::iterator mit= maleconnectors.begin();
           mit!=maleconnectors.end(); ++mit) {
+
+          // Get male connector properties.
           malename = (std::string)mit->second["name"];
           maletype = (std::string)mit->second["type"];
+          malemating = (std::string)mit->second["mating"];
+
           for (std::map<std::string, configmaps::ConfigMap>::iterator fit= femaleconnectors.begin();
             fit!=femaleconnectors.end(); ++fit) {
+
+            // Get female connector properties.
             femalename = (std::string)fit->second["name"];
             femaletype = (std::string)fit->second["type"];
-            if (maletype.compare(femaletype) == 0 && mated(malename, femalename)
-          && ((std::string)mit->second["partner"]).empty() && ((std::string)fit->second["partner"]).empty()) {
-              connect(malename, femalename);
+            femalemating = (std::string)fit->second["mating"];
+
+            // There are 2 cases that will trigger checking for connections:
+            //  1. When it is forced from the Control GUI: Control > Connect available connectors
+            //  2. During plugin update(): IF autoconnect is globally set to true OR IF male and female's mating properties are both set to automatic.
+            if(isforced || (cfgautoconnect.bValue || (malemating.compare(automatic) == 0 && femalemating.compare(automatic) == 0))){
+
+              // Check if connectors meet the mating requirements:
+              //  1. They are of the  same type.
+              //  2. They are close enough to each other (distance and angle) as per the set thresholds in the model's YML config file.
+              //  3. They are not already connected to each other.
+              if (maletype.compare(femaletype) == 0 && closeEnough(malename, femalename)
+                && ((std::string)mit->second["partner"]).empty() && ((std::string)fit->second["partner"]).empty()) {
+
+                  // All mating requirements have been met. Mate the connectors.
+                  connect(malename, femalename);
               }
             }
+          }
         }
       }
 
-
       void Connectors::update(sReal time_ms) {
-        if (cfgautoconnect.bValue) {
-          checkForPossibleConnections();
+
+        if(!isDisconnectionTriggered.load()){
+            checkForPossibleConnections(false);
         }
+
         // the following is experimental and not working yet
         if (cfgbreakable.bValue) {
-          for (std::map<std::string, std::string>::iterator it = connections.begin(); it!=connections.end(); ++it) {
-            utils::Vector forcevec = control->joints->getSimJoint(maleconnectors[it->first]["jointid"])->getJointLoad();
-            //fprintf(stderr, "JointLoad: %g\n", forcevec.norm());
-            if (forcevec.norm() > (double)(connectortypes[maleconnectors[it->first]["type"]]["maxforce"])) {
-              disconnect(it->first);
+          if(!isDisconnectionTriggered.load()){
+            for (std::map<std::string, std::string>::iterator it = connections.begin(); it!=connections.end(); ++it) {
+              utils::Vector forcevec = control->joints->getSimJoint(maleconnectors[it->first]["jointid"])->getJointLoad();
+              //fprintf(stderr, "JointLoad: %g\n", forcevec.norm());
+              if (forcevec.norm() > (double)(connectortypes[maleconnectors[it->first]["type"]]["maxforce"])) {
+                disconnect(it->first);
+              }
             }
           }
         }
@@ -222,11 +306,12 @@ namespace mars {
 
       void Connectors::menuAction(int action, bool checked) {
         if(action == 1) {
-          checkForPossibleConnections();
+          checkForPossibleConnections(true);
+
         } else if (action == 2) {
-          for (std::map<std::string, std::string>::iterator it = connections.begin(); it!=connections.end(); ++it) {
-            disconnect(it->first);
-          }
+          // Use atomic boolean to flag the thread that it should disconnect connections.
+          // Setting this atomic boolean to true will also prevent the plugin's update method from invoking checkForPossibleConnections().
+          isDisconnectionTriggered = true;
         }
       }
 

plugins/connectors/src/Connectors.h

@@ -42,6 +42,10 @@
 #include <mars/cfg_manager/CFGManagerInterface.h>
 #include <configmaps/ConfigData.h>
 
+// Threads.
+#include <mars/utils/Thread.h>
+#include <atomic>
+
 #include <string>
 
 namespace mars {
@@ -53,7 +57,8 @@ namespace mars {
         public mars::data_broker::ReceiverInterface,
         public mars::main_gui::MenuInterface,
         public mars::cfg_manager::CFGClient,
-        public mars::interfaces::EntitySubscriberInterface {
+        public mars::interfaces::EntitySubscriberInterface,
+        public mars::utils::Thread {
 
       public:
         Connectors(lib_manager::LibManager *theManager);
@@ -86,15 +91,27 @@ namespace mars {
         // Connectors methods
         void registerEntity(sim::SimEntity* entity);
 
+        /**
+         * This tread will diconnect connections when they are triggered by the user from the Control GUI.
+         * At the same time, it prevents the plugin' update() method from invoking checkForPossibleConnections().
+         */
+        void run(void);
+
       private:
         cfg_manager::cfgPropertyStruct cfgautoconnect, cfgbreakable;
         std::map<std::string, configmaps::ConfigMap> maleconnectors;
         std::map<std::string, configmaps::ConfigMap> femaleconnectors;
         std::map<std::string, configmaps::ConfigMap> connectortypes;
         std::map<std::string, std::string> connections;
-        bool mated(std::string malename, std::string femalename);
-        void checkForPossibleConnections();
-
+        bool closeEnough(std::string malename, std::string femalename);
+
+        /**
+         * Checks every male-female connector mating combination and mates all
+         * the pairs that meet the mating requirements.
+         *
+         * @param isforced Connection check is triggered from the Control GUI.
+         */
+        void checkForPossibleConnections(bool isforced);
 
       }; // end of class definition Connectors