From fb31453e311d45d2673fa3d8958bb32badfd130b Mon Sep 17 00:00:00 2001
From: Franz Bethke <bethke@math.hu-berlin.de>
Date: Thu, 22 Mar 2018 17:01:19 +0100
Subject: [PATCH] Implement mainNode topology
---
modules/catkin_ws/src/car/CMakeLists.txt | 6 +-
.../car/include/mainNode/CruiseController.h | 22 +++++++
.../src/car/include/mainNode/EgoMotion.h | 21 ++++++-
.../src/car/include/mainNode/MainNode.h | 33 +++++-----
.../car/include/mainNode/PlatoonController.h | 15 ++++-
.../src/car/src/environment/Environment.cpp | 4 +-
.../src/car/src/lanekeeping/Lanekeeping.cpp | 2 +-
.../catkin_ws/src/car/src/logging/Logging.cpp | 20 +++---
.../src/car/src/mainNode/CruiseController.cpp | 18 ++++++
.../src/car/src/mainNode/EgoMotion.cpp | 18 ++++++
.../src/car/src/mainNode/MainNode.cpp | 63 ++++++++-----------
.../car/src/mainNode/PlatoonController.cpp | 9 ++-
12 files changed, 157 insertions(+), 74 deletions(-)
create mode 100644 modules/catkin_ws/src/car/include/mainNode/CruiseController.h
create mode 100644 modules/catkin_ws/src/car/src/mainNode/CruiseController.cpp
diff --git a/modules/catkin_ws/src/car/CMakeLists.txt b/modules/catkin_ws/src/car/CMakeLists.txt
index 93bbf7d6..26947910 100644
--- a/modules/catkin_ws/src/car/CMakeLists.txt
+++ b/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)
diff --git a/modules/catkin_ws/src/car/include/mainNode/CruiseController.h b/modules/catkin_ws/src/car/include/mainNode/CruiseController.h
new file mode 100644
index 00000000..fd8977e2
--- /dev/null
+++ b/modules/catkin_ws/src/car/include/mainNode/CruiseController.h
@@ -0,0 +1,22 @@
+#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
diff --git a/modules/catkin_ws/src/car/include/mainNode/EgoMotion.h b/modules/catkin_ws/src/car/include/mainNode/EgoMotion.h
index aac01990..d398bd90 100644
--- a/modules/catkin_ws/src/car/include/mainNode/EgoMotion.h
+++ b/modules/catkin_ws/src/car/include/mainNode/EgoMotion.h
@@ -1,14 +1,31 @@
#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
diff --git a/modules/catkin_ws/src/car/include/mainNode/MainNode.h b/modules/catkin_ws/src/car/include/mainNode/MainNode.h
index 046ba523..60a4c812 100644
--- a/modules/catkin_ws/src/car/include/mainNode/MainNode.h
+++ b/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
diff --git a/modules/catkin_ws/src/car/include/mainNode/PlatoonController.h b/modules/catkin_ws/src/car/include/mainNode/PlatoonController.h
index f1423875..982f099b 100644
--- a/modules/catkin_ws/src/car/include/mainNode/PlatoonController.h
+++ b/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;
+
};
}
diff --git a/modules/catkin_ws/src/car/src/environment/Environment.cpp b/modules/catkin_ws/src/car/src/environment/Environment.cpp
index ed418339..df596bbc 100644
--- a/modules/catkin_ws/src/car/src/environment/Environment.cpp
+++ b/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";
}
diff --git a/modules/catkin_ws/src/car/src/lanekeeping/Lanekeeping.cpp b/modules/catkin_ws/src/car/src/lanekeeping/Lanekeeping.cpp
index b072e46e..6ef02126 100644
--- a/modules/catkin_ws/src/car/src/lanekeeping/Lanekeeping.cpp
+++ b/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";
}
diff --git a/modules/catkin_ws/src/car/src/logging/Logging.cpp b/modules/catkin_ws/src/car/src/logging/Logging.cpp
index 688eacfd..9bfdcf94 100644
--- a/modules/catkin_ws/src/car/src/logging/Logging.cpp
+++ b/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
+}
diff --git a/modules/catkin_ws/src/car/src/mainNode/CruiseController.cpp b/modules/catkin_ws/src/car/src/mainNode/CruiseController.cpp
new file mode 100644
index 00000000..231b7973
--- /dev/null
+++ b/modules/catkin_ws/src/car/src/mainNode/CruiseController.cpp
@@ -0,0 +1,18 @@
+#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;
+}
+
+}
diff --git a/modules/catkin_ws/src/car/src/mainNode/EgoMotion.cpp b/modules/catkin_ws/src/car/src/mainNode/EgoMotion.cpp
index 5a2c6841..8ad351c0 100644
--- a/modules/catkin_ws/src/car/src/mainNode/EgoMotion.cpp
+++ b/modules/catkin_ws/src/car/src/mainNode/EgoMotion.cpp
@@ -1 +1,19 @@
#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();
+}
+
+}
diff --git a/modules/catkin_ws/src/car/src/mainNode/MainNode.cpp b/modules/catkin_ws/src/car/src/mainNode/MainNode.cpp
index 6e37c055..1cad6c10 100644
--- a/modules/catkin_ws/src/car/src/mainNode/MainNode.cpp
+++ b/modules/catkin_ws/src/car/src/mainNode/MainNode.cpp
@@ -1,47 +1,47 @@
-#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";
-}
}
diff --git a/modules/catkin_ws/src/car/src/mainNode/PlatoonController.cpp b/modules/catkin_ws/src/car/src/mainNode/PlatoonController.cpp
index fd1aff2b..3231cc5b 100644
--- a/modules/catkin_ws/src/car/src/mainNode/PlatoonController.cpp
+++ b/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()
--
GitLab