Fixed some bugs and compability issues.

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

@@ -3,6 +3,8 @@
 
 #include <nodelet/nodelet.h>
 #include <ros/ros.h>
+
+#include "KalmanFilter.h"
 #include <boost/numeric/ublas/vector.hpp>
 #include <boost/numeric/ublas/matrix.hpp>
 
@@ -27,8 +29,8 @@ private:
     ros::NodeHandle nh_;
     std::string name_;
 
-    float distance;
-    float relativeSpeed;
+    double dt;
+    KalmanFilter kalmanFilter;
 
     ros::Publisher environmentData;
     ros::Subscriber ussData;
@@ -37,6 +39,9 @@ private:
     void ussDataCallback(const ussDataMsg::ConstPtr & inMsg);
     void camDataCallback(const camDataMsg::ConstPtr & inMsg);
 
+    matrix<double> buildDynamicModelMatrix(double *deltaT);
+    std::string buildPath();
+
 
 };
 }

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

@@ -12,16 +12,16 @@ PLUGINLIB_EXPORT_CLASS(car::Environment, nodelet::Nodelet);
 
 namespace car
 {
-Environment::Environment(ros::NodeHandle &nh, std::string &name) :
-    nh_(nh)
+Environment::Environment(ros::NodeHandle &nh, std::string &name)
+    : nh_(nh)
     , name_(name)
-    , distance(0)
-    , relativeSpeed(0)
+    , dt()
+    , kalmanFilter(2, buildDynamicModelMatrix(&dt), &dt, buildPath())
     {}
   
-Environment::Environment() :
-    distance(0)
-    , relativeSpeed(0)
+Environment::Environment() 
+    : dt()
+    , kalmanFilter(2, buildDynamicModelMatrix(&dt), &dt, buildPath())
     {}
   
 Environment::~Environment() {}
@@ -38,18 +38,36 @@ void Environment::onInit()
 void Environment::ussDataCallback(const ussDataMsg::ConstPtr& inMsg)
 {
     // std::cout << "Environment recived new uss data (" << inMsg->distance << ").\n";
-    distance = inMsg->distance;
-    relativeSpeed = 1.0;
+    // distance = inMsg->distance;
+    // relativeSpeed = 1.0;
     
-    environmentDataMsg outMsg;
-    outMsg.distance = distance;
-    outMsg.relativeSpeed = relativeSpeed;
+    // environmentDataMsg outMsg;
+    // outMsg.distance = distance;
+    // outMsg.relativeSpeed = relativeSpeed;
     
-    environmentData.publish(outMsg);
+    // environmentData.publish(outMsg);
 }
 
 void Environment::camDataCallback(const camDataMsg::ConstPtr& inMsg)
 {
     // std::cout << "Environment recived new cam data ( ).\n";
 }
+
+std::string Environment::buildPath()
+{
+    std::string userHome = getenv("HOME");
+    return userHome + "/CarConfig/kalman.config";
+}
+
+matrix<double> Environment::buildDynamicModelMatrix(double *deltaT)
+{
+    /* TAKE CARE ON THE DIMENSION */
+    *deltaT = 1.0;
+    matrix<double> dynamicModel(2,2);
+    dynamicModel(0,0) = 1.0,
+    dynamicModel(0,1) = 1.0/(*deltaT);
+    dynamicModel(1,0) = 0;
+    dynamicModel(1,1) = 1.0;
+    return dynamicModel;
+}
 }

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

@@ -2,6 +2,8 @@
 #include "exceptions/Exceptions.h"
 #include <iterator>
 
+#include <stdio.h>
+
 namespace car
 {
 KalmanFilter::KalmanFilter(vector<double> x, matrix<double> P, matrix<double> F, matrix<double> Q, vector<double> H, vector<double> R, double *deltaT)
@@ -22,7 +24,7 @@ KalmanFilter::KalmanFilter(int dimension, matrix<double> F, double *deltaT, std:
     , H(dimension)
     , R(dimension)
     , dt(deltaT)
-    {
+    {   
         readConfigFile(configFilePath);
     }
 
@@ -43,7 +45,6 @@ std::vector<std::string> KalmanFilter::getNextFileLine(std::ifstream *file)
 void KalmanFilter::readConfigFile(std::string configFilePath)
 {
     // open config file
-    std::string userHome = getenv("HOME");
     std::ifstream configFile;
     configFile.open(configFilePath, std::ifstream::in);
     if (!configFile.is_open()) { throw FileNotFound(); }
@@ -57,43 +58,52 @@ void KalmanFilter::readConfigFile(std::string configFilePath)
             if ( words.at(0) == "//") { continue; }
             int rows = std::stoi(words.at(1));
             int cols = std::stoi(words.at(2));
-            int i = 0;
-            int j = 0;
             if (words.at(0) == "x") {
+                int i = 0;
                 while ( i < rows )
                 {
                     words = getNextFileLine(&configFile);
-                    x(i) = std::stod(words.at(0));
+                    x(i++) = std::stod(words.at(0));
                 }
             } else if (words.at(0) == "P"){
+                int i = 0;
                 while ( i < rows )
                 {
                     words = getNextFileLine(&configFile);
+                    int j = 0;
                     while ( j < cols )
                     {
                         P(i, j) = std::stod(words.at(j));
+                        j++;
                     }
+                    i++;
                 }
             } else if (words.at(0) == "Q"){
+                int i = 0;
                 while ( i < rows )
                 {
                     words = getNextFileLine(&configFile);
+                    int j = 0;
                     while ( j < cols )
                     {
                         Q(i, j) = std::stod(words.at(j));
+                        j++;
                     }
+                    i++;
                 }
             } else if (words.at(0) == "H"){
+                int i = 0;
                 while ( i < rows )
                 {
                     words = getNextFileLine(&configFile);
-                    H(i) = std::stod(words.at(0));
+                    H(i++) = std::stod(words.at(0));
                 }
             } else if (words.at(0) == "R"){
+                int i = 0;
                 while ( i < rows )
                 {
                     words = getNextFileLine(&configFile);
-                    x(i) = std::stod(words.at(j));
+                    x(i++) = std::stod(words.at(0));
                 }
             }       
         }