Implement mainNode topology

modules/catkin_ws/src/car/CMakeLists.txt

@@ -52,11 +52,15 @@ include_directories(
 
 set(MAIN_NODE_SOURCE_FILES
         include/mainNode/MainNode.h
+        include/mainNode/EgoMotion.h
         include/mainNode/NotifiableThread.h
         include/mainNode/PlatoonController.h
+        include/mainNode/CruiseController.h
         src/mainNode/MainNode.cpp
+        src/mainNode/EgoMotion.cpp
         src/mainNode/NotifiableThread.cpp
-        src/mainNode/PlatoonController.cpp)
+        src/mainNode/PlatoonController.cpp
+        src/mainNode/CruiseController.cpp)
 
 set(LOCAL_INSTALL_DIR ${CMAKE_CURRENT_LIST_DIR}/../../install)
 

modules/catkin_ws/src/car/include/mainNode/CruiseController.h

+#ifndef CRUISECONTROLLER_H
+#define CRUISECONTROLLER_H
+
+#include "PlatoonController.h"
+#include "EgoMotion.h"
+
+namespace car
+{
+class CruiseController 
+{
+  public:
+    CruiseController(EgoMotion& egoMotion, PlatoonController& platoonController);
+    ~CruiseController(){};
+
+    void run();
+    
+  private:
+    EgoMotion& egoMotion;
+    PlatoonController& platoonController;
+};
+}
+#endif

modules/catkin_ws/src/car/include/mainNode/EgoMotion.h

 #ifndef EGOMOTION_H
 #define EGOMOTION_H
 
-#endif
+#include <nodelet/nodelet.h>
+#include <ros/ros.h>
+
+#include <functional>
+
+#include "car/environmentDataMsg.h"
 
 namespace car
 {
 class EgoMotion 
 {
   public:
-    EgoMotion(){};
+    using Callback = std::function<void()>;
+    
+    EgoMotion(ros::NodeHandle nh);
     ~EgoMotion(){};
+
+    Callback platoonControllerNotify;
+    Callback cruiseControllerNotify;
+    
+  private:
+    ros::Subscriber environmentData;
+    void environmentDataCallback(const environmentDataMsg::ConstPtr & inMsg);
+    
 };
 }
+
+#endif

modules/catkin_ws/src/car/include/mainNode/MainNode.h

@@ -4,7 +4,6 @@
 #include <nodelet/nodelet.h>
 #include <ros/ros.h>
 #include "boost/thread.hpp"
-#include "car/environmentDataMsg.h"
 
 #include "NotifiableThread.h"
 #include "NetworkingLib/Networking.h"
@@ -13,7 +12,7 @@
 
 #include "PlatoonController.h"
 #include "EgoMotion.h"
-// #include "CruiseController.h"
+#include "CruiseController.h"
 
 namespace car
 {
@@ -29,29 +28,29 @@ public:
     ~MainNode();
 
 private:
-    networking::Networking c2cNet;
-    networking::Networking pcNet;
+    ros::NodeHandle nh;
+    std::string name;
+    
+    networking::Networking c2cNet; // thread  + event queue
+    networking::Networking pcNet; // thread  + event queue
+    
+    platoonProtocol::Vehicle::Ptr c2c;
+    pc2car::CommandReceiver::Ptr pc;
 
     EgoMotion egoMotion;
     
     PlatoonController platoonController;
     NotifiableThread platoonControllerThread;
      
-     std::function<void()> platoonControllerNotify =
-         [this] { platoonControllerThread.notify(); };
-     
-    // CruiseController cruiseController;
-
-    platoonProtocol::Vehicle::Ptr c2c;
-    pc2car::CommandReceiver::Ptr commandReceiver;
+    CruiseController cruiseController;
+    NotifiableThread cruiseControllerThread;
       
-    ros::NodeHandle nh;
-    std::string name;
+    std::function<void()> platoonControllerNotify =
+        [this] { platoonControllerThread.notify(); };
+    
+    std::function<void()> cruiseControllerNotify =
+        [this] { cruiseControllerThread.notify(); };
     
-    ros::Publisher logEnabled;
-
-    ros::Subscriber environmentData;
-    void environmentDataCallback(const environmentDataMsg::ConstPtr & inMsg);
 };
 }
 #endif

modules/catkin_ws/src/car/include/mainNode/PlatoonController.h

@@ -7,6 +7,10 @@
 
 #include <functional>
 
+#include "PlatoonProtocolLib/Vehicle.h"
+#include "PC2CarLib/CommandReceiver.h"
+#include "EgoMotion.h"
+
 namespace car
 {
 
@@ -15,12 +19,19 @@ class PlatoonController
 public:
     using Callback = std::function<void()>;
     
-    PlatoonController();
+    PlatoonController(platoonProtocol::Vehicle::Ptr c2c,
+                      pc2car::CommandReceiver::Ptr pc,
+                      EgoMotion& egoMotion);
       
     void run();
+    Callback cruiseControllerNotify;
 
 private:
-    Callback ccNotify;
+
+    platoonProtocol::Vehicle::Ptr c2c;
+    pc2car::CommandReceiver::Ptr pc;
+    EgoMotion& egoMotion;
+
 };
 
 }

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

@@ -32,7 +32,7 @@ namespace car
 
   void Environment::ussDataCallback(const ussDataMsg::ConstPtr& inMsg)
   {
-    std::cout << "Environment recived new uss data (" << inMsg->distance << ").\n";
+    // std::cout << "Environment recived new uss data (" << inMsg->distance << ").\n";
     distance = inMsg->distance;
     prevSpeed = 1.0;
     
@@ -45,7 +45,7 @@ namespace car
 
   void Environment::camDataCallback(const camDataMsg::ConstPtr& inMsg)
   {
-    std::cout << "Environment recived new cam data ( ).\n";
+    // std::cout << "Environment recived new cam data ( ).\n";
   }
   
   

modules/catkin_ws/src/car/src/lanekeeping/Lanekeeping.cpp

@@ -29,7 +29,7 @@ namespace car
 
   void Lanekeeping::camDataCallback(const camDataMsg::ConstPtr& inMsg)
   {
-    std::cout << "Lanekeeping recived new cam data ( ).\n";
+    // std::cout << "Lanekeeping recived new cam data ( ).\n";
   }
   
   

modules/catkin_ws/src/car/src/logging/Logging.cpp

@@ -42,47 +42,47 @@ void Logging::onInit()
 
 void Logging::ccDataCallback(const ccDataMsg::ConstPtr & inMsg)
 {
-    std::cout << "Logging received new cc data ( ).\n";
+    // std::cout << "Logging received new cc data ( ).\n";
 }
 
 void Logging::camDataCallback(const camDataMsg::ConstPtr & inMsg)
 {
-    std::cout << "Logging received new cam data ( ).\n";
+    // std::cout << "Logging received new cam data ( ).\n";
 }
 
 void Logging::environmentDataCallback(const environmentDataMsg::ConstPtr & inMsg)
 {
-    std::cout << "Logging received new environment data ( ).\n";
+    // std::cout << "Logging received new environment data ( ).\n";
 }
 
 void Logging::laneDataCallback(const laneDataMsg::ConstPtr & inMsg)
 {
-    std::cout << "Logging received new lane data ( ).\n";
+    // std::cout << "Logging received new lane data ( ).\n";
 }
 
 void Logging::logEnabledCallback(const logEnabledMsg::ConstPtr & inMsg)
 {
-    std::cout << "Logging received new log enabled ( ).\n";
+    // std::cout << "Logging received new log enabled ( ).\n";
 }
 
 void Logging::logStringCallback(const logStringMsg::ConstPtr & inMsg)
 {
-    std::cout << "Logging received new log string ( ).\n";
+    // std::cout << "Logging received new log string ( ).\n";
 }
 
 void Logging::rcEnabledCallback(const rcEnabledMsg::ConstPtr & inMsg)
 {
-    std::cout << "Logging received new rc enabled ( ).\n";
+    // std::cout << "Logging received new rc enabled ( ).\n";
 }
 
 void Logging::stmDataCallback(const stmDataMsg::ConstPtr & inMsg)
 {
-    std::cout << "Logging received new stm data ( ).\n";
+    // std::cout << "Logging received new stm data ( ).\n";
 }
 
 void Logging::ussDataCallback(const ussDataMsg::ConstPtr & inMsg)
 {
-    std::cout << "Logging received new uss data ( ).\n";
+    // std::cout << "Logging received new uss data ( ).\n";
 }
 
-}
+}
\ No newline at end of file

modules/catkin_ws/src/car/src/mainNode/CruiseController.cpp

+#include "../../include/mainNode/CruiseController.h"
+
+#include <iostream>
+
+namespace car {
+
+CruiseController::CruiseController(EgoMotion& egoMotion, PlatoonController& platoonController)
+    : egoMotion(egoMotion)
+      , platoonController(platoonController)
+{
+  std::cout << "WEEEEEEEEEEEEEEEELT" << std::endl;
+}
+
+void CruiseController::run() {
+    std::cout << "CruiseController was run." << std::endl;
+}
+
+}

modules/catkin_ws/src/car/src/mainNode/EgoMotion.cpp

 #include "mainNode/EgoMotion.h"
+
+
+namespace car {
+  
+EgoMotion::EgoMotion(ros::NodeHandle nh)
+    : platoonControllerNotify()
+      , cruiseControllerNotify()
+{
+    environmentData = nh.subscribe("environmentData", 1, &EgoMotion::environmentDataCallback, this);
+}
+
+void EgoMotion::environmentDataCallback(const environmentDataMsg::ConstPtr & inMsg)
+{
+    std::cout << "EgoMotion recived new environmentData data (" << inMsg->distance << ", " << inMsg->prevSpeed << ").\n";
+    cruiseControllerNotify();
+}
+
+}

modules/catkin_ws/src/car/src/mainNode/MainNode.cpp

-#include <pluginlib/class_list_macros.h>
-#include <ros/ros.h>
-
 #include "mainNode/MainNode.h"
 
-#include "car/camDataMsg.h"
-#include "car/ccDataMsg.h"
-#include "car/environmentDataMsg.h"
-#include "car/logEnabledMsg.h"
-#include "car/logStringMsg.h"
-#include "car/rcEnabledMsg.h"
-#include "car/stmDataMsg.h"
-#include "car/ussDataMsg.h"
+#include <pluginlib/class_list_macros.h>
 
 PLUGINLIB_EXPORT_CLASS(car::MainNode, nodelet::Nodelet);
 
 namespace car
 {
 MainNode::MainNode(ros::NodeHandle & nh, std::string & name) :
-    c2cNet()
+    nh(nh)  
+    , name(name)
+    , c2cNet()
     , pcNet()
-    , egoMotion()
-    , platoonController()
-    , platoonControllerThread([this] { platoonController.run(); })
-    // , cruiseController()
     , c2c()
-    , commandReceiver(pc2car::CommandReceiver::create(pcNet))
-    , nh(nh)  
-    , name(name)
+    , pc(pc2car::CommandReceiver::create(pcNet))
+    , egoMotion(nh)
+    , platoonController(c2c, pc, egoMotion)
+    , platoonControllerThread([this] { platoonController.run(); })
+    , cruiseController(egoMotion, platoonController)
+    , cruiseControllerThread([this] { cruiseController.run(); })
 {
-  std::cout << "Called MainNode(ros::NodeHandle & nh, std::string & name)." << std::endl;
+  egoMotion.cruiseControllerNotify = cruiseControllerNotify;
+  egoMotion.platoonControllerNotify = platoonControllerNotify;
+  platoonController.cruiseControllerNotify = cruiseControllerNotify;
+  // std::cout << "Called MainNode(ros::NodeHandle & nh, std::string & name)." << std::endl;
 }
 
 MainNode::MainNode() : 
-    c2cNet()
+    nh()  
+    , name()
+    , c2cNet()
     , pcNet()
-    , egoMotion()
-    , platoonController()
-    , platoonControllerThread([this] { platoonController.run(); })
-    // , cruiseController()
     , c2c()
-    , commandReceiver(pc2car::CommandReceiver::create(pcNet))
+    , pc(pc2car::CommandReceiver::create(pcNet))
+    , egoMotion(nh)
+    , platoonController(c2c, pc, egoMotion)
+    , platoonControllerThread([this] { platoonController.run(); })
+    , cruiseController(egoMotion, platoonController)
+    , cruiseControllerThread([this] { cruiseController.run(); })
 {
-  std::cout << "Called MainNode()." << std::endl;
+  egoMotion.cruiseControllerNotify = cruiseControllerNotify;
+  egoMotion.platoonControllerNotify = platoonControllerNotify;
+  platoonController.cruiseControllerNotify = cruiseControllerNotify;
+  // std::cout << "Called MainNode()." << std::endl;
 }
 
 MainNode::~MainNode()
@@ -50,16 +50,7 @@ MainNode::~MainNode()
 void MainNode::onInit()
 {
     NODELET_INFO("MainNode::onInit -- START");
-    logEnabled = nh.advertise<logEnabledMsg>("logEnabled", 5);
-    environmentData = nh.subscribe("environmentData", 1, &MainNode::environmentDataCallback, this);
-
-    // CODE GOES HERE
-
     NODELET_INFO("MainNode::onInit -- END");
 }
 
-void MainNode::environmentDataCallback(const environmentDataMsg::ConstPtr & inMsg)
-{
-    std::cout << "MainNode recived new environmentData data (" << inMsg->distance << ", " << inMsg->prevSpeed << ").\n";
-}
 }

modules/catkin_ws/src/car/src/mainNode/PlatoonController.cpp

@@ -8,9 +8,12 @@
 namespace car
 {
 
-PlatoonController::PlatoonController()
-    : ccNotify()
-{}
+PlatoonController::PlatoonController(platoonProtocol::Vehicle::Ptr c2c,
+    pc2car::CommandReceiver::Ptr pc, EgoMotion& egoMotion)
+    : c2c(c2c)
+      , pc(pc)
+      , egoMotion(egoMotion)
+      , cruiseControllerNotify() {}
 
 
 void car::PlatoonController::run()