From 0fbf1f42e916147c61648ced7b52bd3b67a7c827 Mon Sep 17 00:00:00 2001
From: Steven Lange <langestx@informatik.hu-berlin.de>
Date: Fri, 20 Apr 2018 13:39:33 +0000
Subject: [PATCH] Implemented Egomotion.h
---
modules/catkin_ws/src/car/CMakeLists.txt | 2 +-
.../src/car/include/environment/Environment.h | 13 +++++++
.../src/car/src/environment/Environment.cpp | 38 +++++++++----------
3 files changed, 32 insertions(+), 21 deletions(-)
diff --git a/modules/catkin_ws/src/car/CMakeLists.txt b/modules/catkin_ws/src/car/CMakeLists.txt
index 0b32354a..ea019a94 100644
--- a/modules/catkin_ws/src/car/CMakeLists.txt
+++ b/modules/catkin_ws/src/car/CMakeLists.txt
@@ -7,7 +7,7 @@ set(CMAKE_CXX_STANDARD 14)
set(CMAKE_CXX_FLAGS "-pthread")
# Include Boost
-find_package(Boost REQUIRED COMPONENTS system regex)
+find_package(Boost REQUIRED)
find_package(catkin REQUIRED COMPONENTS
roscpp
diff --git a/modules/catkin_ws/src/car/include/environment/Environment.h b/modules/catkin_ws/src/car/include/environment/Environment.h
index ff28f961..85ec0298 100644
--- a/modules/catkin_ws/src/car/include/environment/Environment.h
+++ b/modules/catkin_ws/src/car/include/environment/Environment.h
@@ -3,10 +3,13 @@
#include <nodelet/nodelet.h>
#include <ros/ros.h>
+#include <boost/numeric/ublas/vector.hpp>
+#include <boost/numeric/ublas/matrix.hpp>
#include "car/ussDataMsg.h"
#include "car/camDataMsg.h"
+using namespace boost::numeric::ublas;
namespace car
{
class Environment : public nodelet::Nodelet
@@ -35,6 +38,16 @@ private:
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};
+
+ void predict();
+ void update(vector<double> mesVec);
};
}
#endif
diff --git a/modules/catkin_ws/src/car/src/environment/Environment.cpp b/modules/catkin_ws/src/car/src/environment/Environment.cpp
index 8718a215..1f5d69fd 100644
--- a/modules/catkin_ws/src/car/src/environment/Environment.cpp
+++ b/modules/catkin_ws/src/car/src/environment/Environment.cpp
@@ -11,27 +11,27 @@ PLUGINLIB_EXPORT_CLASS(car::Environment, nodelet::Nodelet);
namespace car
{
- Environment::Environment(ros::NodeHandle &nh, std::string &name) :
+Environment::Environment(ros::NodeHandle &nh, std::string &name) :
nh_(nh),
name_(name),
distance(0),
relativeSpeed(0) {}
- Environment::Environment() {}
+Environment::Environment() {}
- Environment::~Environment() {}
-
- void Environment::onInit()
- {
- NODELET_INFO("Environment::onInit -- START");
- environmentData = nh_.advertise<environmentDataMsg>("environmentData", 1);
- ussData = nh_.subscribe("ussData", 1, &Environment::ussDataCallback, this);
- camData = nh_.subscribe("camData", 1, &Environment::camDataCallback, this);
- NODELET_INFO("Environment::onInit -- END");
- }
+Environment::~Environment() {}
+
+void Environment::onInit()
+{
+ NODELET_INFO("Environment::onInit -- START");
+ environmentData = nh_.advertise<environmentDataMsg>("environmentData", 1);
+ ussData = nh_.subscribe("ussData", 1, &Environment::ussDataCallback, this);
+ camData = nh_.subscribe("camData", 1, &Environment::camDataCallback, this);
+ NODELET_INFO("Environment::onInit -- END");
+}
- void Environment::ussDataCallback(const ussDataMsg::ConstPtr& inMsg)
- {
+void Environment::ussDataCallback(const ussDataMsg::ConstPtr& inMsg)
+{
// std::cout << "Environment recived new uss data (" << inMsg->distance << ").\n";
distance = inMsg->distance;
relativeSpeed = 1.0;
@@ -41,12 +41,10 @@ namespace car
outMsg.relativeSpeed = relativeSpeed;
environmentData.publish(outMsg);
- }
+}
- void Environment::camDataCallback(const camDataMsg::ConstPtr& inMsg)
- {
+void Environment::camDataCallback(const camDataMsg::ConstPtr& inMsg)
+{
// std::cout << "Environment recived new cam data ( ).\n";
- }
-
-
+}
}
--
GitLab