Merge branch 'master' of gitlab.informatik.hu-berlin.de:badenhop/Hochautomatisiertes-Fahren

* 'master' of gitlab.informatik.hu-berlin.de:badenhop/Hochautomatisiertes-Fahren:
  Added readConfigFile() to Environment

modules/catkin_ws/src/car/include/environment/Environment.h

@@ -26,6 +26,7 @@ public:
 private:
     ros::NodeHandle nh_;
     std::string name_;
+    void readConfigFile();
 
     float distance;
     float relativeSpeed;
@@ -34,17 +35,16 @@ private:
     ros::Subscriber ussData;
     ros::Subscriber camData;
 
-
     void ussDataCallback(const ussDataMsg::ConstPtr & inMsg);
-
     void camDataCallback(const camDataMsg::ConstPtr & inMsg);
 
-    zero_vector<double> x {2};
-    zero_matrix<double> P {2, 2};
-    zero_matrix<double> F {2, 2};
-    zero_matrix<double> Q {2, 2};
-    zero_vector<double> H {2};
-    zero_vector<double> R {1};
+    //Notation like in Kalamanfilter <Wikipedia>
+    vector<double> x {2};          // current state (distance, velocity)^T
+    matrix<double> P {2, 2};       // covariance matrix
+    matrix<double> F {2, 2};       // dynamic model
+    matrix<double> Q {2, 2};       // process error
+    vector<double> H {2};          // mesure vector (refers to distance)
+    vector<double> R {1};          // measurement error
     
     void predict();
     void update(vector<double> mesVec);

modules/catkin_ws/src/car/src/environment/Environment.cpp

@@ -2,25 +2,101 @@
 #include <ros/ros.h>
 
 #include "environment/Environment.h"
+#include "exceptions/Exceptions.h"
 
 #include "car/camDataMsg.h"
 #include "car/environmentDataMsg.h"
 #include "car/ussDataMsg.h"
 
+#include <fstream>
+#include <vector>
+#include <iterator>
+
 PLUGINLIB_EXPORT_CLASS(car::Environment, nodelet::Nodelet);
 
 namespace car
 {
 Environment::Environment(ros::NodeHandle &nh, std::string &name) :
-    nh_(nh), 
-    name_(name),
-    distance(0),
-    relativeSpeed(0) {}
+    nh_(nh)
+    , name_(name)
+    , distance(0)
+    , relativeSpeed(0)
+{
+    readConfigFile();
+}
   
-Environment::Environment() {}
+Environment::Environment() :
+    distance(0)
+    , relativeSpeed(0)
+{
+    readConfigFile();
+}
   
 Environment::~Environment() {}
  
+void Environment::readConfigFile()
+{
+    // open config file
+    std::string userHome = getenv("HOME");
+    std::ifstream configFile;
+    configFile.open(userHome + "/.CarConfig/environment.config", std::ifstream::in);
+    if (!configFile.is_open()) { throw FileNotFound(); }
+
+    // desired parameters
+    
+    // read file
+    std::string contentLine;
+    while (!configFile.eof())
+    {
+        std::getline(configFile, contentLine);
+        
+        // split this line
+        std::istringstream iss(contentLine);
+        std::vector<std::string> words {std::istream_iterator<std::string>(iss), {} };
+
+        if ( words.size() > 1 ) // this line contains parameter[at (0)] and value [at (1)]
+        {
+            if (words.at(0) == "x1:") {
+                x(0) = std::stod(words.at(1));
+            } else if (words.at(0) == "x2:"){
+                x(1) = std::stof(words.at(1));
+            } else if (words.at(0) == "p11:"){
+                P(0, 0) = std::stof(words.at(1));
+            } else if (words.at(0) == "p12:"){
+                P(0,1) = std::stof(words.at(1));
+            } else if (words.at(0) == "p21:"){
+                P(1,0) = std::stof(words.at(1));
+            } else if (words.at(0) == "p22:"){
+                P(1,1) = std::stof(words.at(1));
+            } else if (words.at(0) == "f11:"){
+                F(0,0) = std::stof(words.at(1));
+            } else if (words.at(0) == "f12:"){
+                F(0,1) = std::stof(words.at(1));
+            } else if (words.at(0) == "f21:"){
+                F(1,0) = std::stof(words.at(1));
+            } else if (words.at(0) == "f22:"){
+                F(1,1) = std::stof(words.at(1));
+            } else if (words.at(0) == "q11:"){
+                Q(0,0) = std::stof(words.at(1));
+            } else if (words.at(0) == "q12:"){
+                Q(0,1) = std::stof(words.at(1));
+            } else if (words.at(0) == "q21:"){
+                Q(1,0) = std::stof(words.at(1));
+            } else if (words.at(0) == "q22:"){
+                Q(1,1) = std::stof(words.at(1));
+            } else if (words.at(0) == "h1:"){
+                H(0) = std::stof(words.at(1));
+            } else if (words.at(0) == "h2:"){
+                H(1) = std::stof(words.at(1));
+            } else if (words.at(0) == "r:"){
+                R(0) = std::stof(words.at(1));
+            }       
+        }
+    }
+    configFile.close();
+    return;
+}
+
 void Environment::onInit()
 {
     NODELET_INFO("Environment::onInit -- START");