Implemented Egomotion.h

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

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

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";
-  }
-  
-  
+}
 }