From 2f3c7feb4d078846ffc81e3378a02e5c32c19142 Mon Sep 17 00:00:00 2001
From: Steven Lange <langestx@informatik.hu-berlin.de>
Date: Fri, 20 Apr 2018 16:33:09 +0000
Subject: [PATCH] Added readConfigFile() to Environment
---
.../src/car/include/environment/Environment.h | 16 ++--
.../src/car/src/environment/Environment.cpp | 86 +++++++++++++++++--
2 files changed, 89 insertions(+), 13 deletions(-)
diff --git a/modules/catkin_ws/src/car/include/environment/Environment.h b/modules/catkin_ws/src/car/include/environment/Environment.h
index 85ec0298..c869343c 100644
--- a/modules/catkin_ws/src/car/include/environment/Environment.h
+++ b/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);
diff --git a/modules/catkin_ws/src/car/src/environment/Environment.cpp b/modules/catkin_ws/src/car/src/environment/Environment.cpp
index 1f5d69fd..f021c34b 100644
--- a/modules/catkin_ws/src/car/src/environment/Environment.cpp
+++ b/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");
--
GitLab