From 0f777a3fc8d3e4d8d58d8b0313778da58d978cc1 Mon Sep 17 00:00:00 2001
From: Steven Lange <langestx@informatik.hu-berlin.de>
Date: Fri, 4 May 2018 11:13:56 +0000
Subject: [PATCH] Fixed some bugs and compability issues.
---
.../src/car/include/environment/Environment.h | 9 +++-
.../src/car/src/environment/Environment.cpp | 44 +++++++++++++------
.../src/car/src/environment/KalmanFilter.cpp | 24 +++++++---
3 files changed, 55 insertions(+), 22 deletions(-)
diff --git a/modules/catkin_ws/src/car/include/environment/Environment.h b/modules/catkin_ws/src/car/include/environment/Environment.h
index 555eecbd..97ac2c16 100644
--- a/modules/catkin_ws/src/car/include/environment/Environment.h
+++ b/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();
+
};
}
diff --git a/modules/catkin_ws/src/car/src/environment/Environment.cpp b/modules/catkin_ws/src/car/src/environment/Environment.cpp
index 3fe8de92..698edf4a 100644
--- a/modules/catkin_ws/src/car/src/environment/Environment.cpp
+++ b/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;
+}
}
diff --git a/modules/catkin_ws/src/car/src/environment/KalmanFilter.cpp b/modules/catkin_ws/src/car/src/environment/KalmanFilter.cpp
index 86c51897..e5f6d55f 100644
--- a/modules/catkin_ws/src/car/src/environment/KalmanFilter.cpp
+++ b/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));
}
}
}
--
GitLab