diff --git a/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/Vehicle.h b/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/Vehicle.h
index 71a2358fa67735f800ca82d3f78f873ad8c8ed43..73cd760375e382f77f73f1d8eee70204e05c0212 100644
--- a/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/Vehicle.h
+++ b/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/Vehicle.h
@@ -25,6 +25,8 @@ public:
using Callback = std::function<void()>;
static constexpr std::uint16_t UDP_PORT = 10000;
+
+ static const Callback DEFAULT_CALLBACK;
enum class Role
{
@@ -55,6 +57,7 @@ public:
void setOnLeavingPlatoonCallback(const Callback & callback)
{ onLeavingPlatoonCallback = callback; }
+
protected:
Vehicle(
@@ -70,7 +73,6 @@ protected:
};
static const PlatoonConfig DEFAULT_PLATOON_CONFIG;
- static const Callback DEFAULT_CALLBACK;
const NetworkInfo myInfo;
PlatoonId platoonId;
diff --git a/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/VehicleFacade.h b/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/VehicleFacade.h
index 48c19389ef3c53cdf6b637b6c1328d8c21f49666..816c297a413c54ebed574b4bac9af14be095161f 100644
--- a/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/VehicleFacade.h
+++ b/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/VehicleFacade.h
@@ -16,7 +16,7 @@ class VehicleFacade
public:
explicit VehicleFacade(Vehicle::Role role, networking::Networking & net, const NetworkInfo & info);
- void reset(Vehicle::Role role, networking::Networking & net, const NetworkInfo & info);
+ void reset(Vehicle::Role role);
Vehicle::Ptr get();
@@ -35,10 +35,8 @@ public:
VehicleId getOwnVehicleId();
Vehicle::Role getRole();
-
- void setOnRunningPlatoonCallback(const Vehicle::Callback & callback);
-
- void setOnLeavingPlatoonCallback(const Vehicle::Callback & callback);
+
+ void setCallback(const Vehicle::Callback & callback);
// LeaderVehicle methods
@@ -53,12 +51,15 @@ public:
// FollowerVehicle methods
PlatoonConfig getPlatoonConfig();
-
- void setOnPlatoonConfigUpdatedCallback(const Vehicle::Callback & callback);
-
+
private:
Vehicle::Ptr vehicle;
std::mutex mutex;
+
+ networking::Networking & net;
+ NetworkInfo info;
+ Vehicle::Callback callback{Vehicle::DEFAULT_CALLBACK};
+
};
}
diff --git a/modules/catkin_ws/src/PlatoonProtocolLib/src/VehicleFacade.cpp b/modules/catkin_ws/src/PlatoonProtocolLib/src/VehicleFacade.cpp
index dc841bf7ca37acae0cc7a8fcc76f1c944505b3e8..db4ea64ec269ec7f5181514153d740aefa12f498 100644
--- a/modules/catkin_ws/src/PlatoonProtocolLib/src/VehicleFacade.cpp
+++ b/modules/catkin_ws/src/PlatoonProtocolLib/src/VehicleFacade.cpp
@@ -9,18 +9,24 @@
namespace platoonProtocol
{
-VehicleFacade::VehicleFacade(Vehicle::Role role, networking::Networking & net, const NetworkInfo & info)
+VehicleFacade::VehicleFacade(Vehicle::Role role, networking::Networking & net, const NetworkInfo & info) :
+ net(net), info(info)
{
- reset(role, net, info);
+ reset(role);
}
-void VehicleFacade::reset(Vehicle::Role role, networking::Networking & net, const NetworkInfo & info)
+void VehicleFacade::reset(Vehicle::Role role)
{
std::lock_guard<std::mutex> lock(mutex);
if (role == Vehicle::Role::LEADER)
vehicle = LeaderVehicle::create(net, info);
else
+ {
vehicle = FollowerVehicle::create(net, info);
+ std::dynamic_pointer_cast<FollowerVehicle>(vehicle)->setOnPlatoonConfigUpdatedCallback(callback);
+ }
+ vehicle->setOnLeavingPlatoonCallback(callback);
+ vehicle->setOnRunningPlatoonCallback(callback);
}
Vehicle::Ptr VehicleFacade::get()
@@ -71,16 +77,14 @@ Vehicle::Role VehicleFacade::getRole()
return vehicle->getRole();
}
-void VehicleFacade::setOnRunningPlatoonCallback(const Vehicle::Callback & callback)
-{
- std::lock_guard<std::mutex> lock(mutex);
- vehicle->setOnRunningPlatoonCallback(callback);
-}
-
-void VehicleFacade::setOnLeavingPlatoonCallback(const Vehicle::Callback & callback)
+void VehicleFacade::setCallback(const Vehicle::Callback & callback)
{
std::lock_guard<std::mutex> lock(mutex);
+ this->callback = callback;
vehicle->setOnLeavingPlatoonCallback(callback);
+ vehicle->setOnRunningPlatoonCallback(callback);
+ if (vehicle->getRole() == Vehicle::Role::FOLLOWER)
+ std::dynamic_pointer_cast<FollowerVehicle>(vehicle)->setOnPlatoonConfigUpdatedCallback(callback);
}
std::size_t VehicleFacade::getNumFollowers()
@@ -113,10 +117,4 @@ PlatoonConfig VehicleFacade::getPlatoonConfig()
return std::dynamic_pointer_cast<FollowerVehicle>(vehicle)->getPlatoonConfig();
}
-void VehicleFacade::setOnPlatoonConfigUpdatedCallback(const Vehicle::Callback & callback)
-{
- std::lock_guard<std::mutex> lock(mutex);
- std::dynamic_pointer_cast<FollowerVehicle>(vehicle)->setOnPlatoonConfigUpdatedCallback(callback);
}
-
-}
\ No newline at end of file
diff --git a/modules/catkin_ws/src/car/include/mainNode/CruiseController.h b/modules/catkin_ws/src/car/include/mainNode/CruiseController.h
index fd8977e215e26a22c4c449e07f58ecdff1964e08..a94dcb7830087835635a5e6a7dbfdd3ec0ad09e6 100644
--- a/modules/catkin_ws/src/car/include/mainNode/CruiseController.h
+++ b/modules/catkin_ws/src/car/include/mainNode/CruiseController.h
@@ -1,6 +1,7 @@
#ifndef CRUISECONTROLLER_H
#define CRUISECONTROLLER_H
+#include "PlatoonProtocolLib/VehicleFacade.h"
#include "PlatoonController.h"
#include "EgoMotion.h"
@@ -9,12 +10,16 @@ namespace car
class CruiseController
{
public:
- CruiseController(EgoMotion& egoMotion, PlatoonController& platoonController);
+ CruiseController(platoonProtocol::VehicleFacade& c2c,
+ EgoMotion& egoMotion,
+ PlatoonController& platoonController);
+
~CruiseController(){};
void run();
private:
+ platoonProtocol::VehicleFacade& c2c;
EgoMotion& egoMotion;
PlatoonController& platoonController;
};
diff --git a/modules/catkin_ws/src/car/src/mainNode/CruiseController.cpp b/modules/catkin_ws/src/car/src/mainNode/CruiseController.cpp
index 231b7973c5de412ae5b538aafa59394f363b34c6..32651413d484ac9deaa2f26417f1a3d84d7b4439 100644
--- a/modules/catkin_ws/src/car/src/mainNode/CruiseController.cpp
+++ b/modules/catkin_ws/src/car/src/mainNode/CruiseController.cpp
@@ -1,15 +1,18 @@
-#include "../../include/mainNode/CruiseController.h"
+#include "mainNode/CruiseController.h"
+
+#include "PlatoonProtocolLib/VehicleFacade.h"
#include <iostream>
namespace car {
-CruiseController::CruiseController(EgoMotion& egoMotion, PlatoonController& platoonController)
- : egoMotion(egoMotion)
+CruiseController::CruiseController(platoonProtocol::VehicleFacade& c2c,
+ EgoMotion& egoMotion,
+ PlatoonController& platoonController)
+ : c2c(c2c)
+ , 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/MainNode.cpp b/modules/catkin_ws/src/car/src/mainNode/MainNode.cpp
index 303ce1e7784e67b6eca1c3982b2c015f56195e9f..7a98560f7c6734fcdc8840fe189b2349aba39a41 100644
--- a/modules/catkin_ws/src/car/src/mainNode/MainNode.cpp
+++ b/modules/catkin_ws/src/car/src/mainNode/MainNode.cpp
@@ -64,38 +64,40 @@ MainNode::MainNode(ros::NodeHandle & nh, std::string & name) :
nh(nh)
, name(name)
, c2cNet()
- , c2c(platoonProtocol::Vehicle::Role::LEADER, c2cNet, readNetworkInfo())
+ , c2c(platoonProtocol::Vehicle::Role::FOLLOWER, c2cNet, readNetworkInfo())
, pcNet()
, pc(pc2car::CommandReceiver::create(pcNet))
, egoMotion(nh)
, platoonController(c2c, pc, egoMotion)
, platoonControllerThread([this] { platoonController.run(); })
- , cruiseController(egoMotion, platoonController)
+ , cruiseController(c2c, egoMotion, platoonController)
, cruiseControllerThread([this] { cruiseController.run(); })
{
egoMotion.cruiseControllerNotify = cruiseControllerNotify;
egoMotion.platoonControllerNotify = platoonControllerNotify;
platoonController.cruiseControllerNotify = cruiseControllerNotify;
- // pc->reciveCommands([this] (auto commandCode) {platoonControllerThread.notify();} )
+ pc->receiveCommands([this] (auto commandCode) {platoonControllerThread.notify();} );
+ c2c.setCallback( [this] {platoonControllerThread.notify();} );
}
MainNode::MainNode() :
nh()
, name()
, c2cNet()
- , c2c(platoonProtocol::Vehicle::Role::LEADER, c2cNet, readNetworkInfo())
+ , c2c(platoonProtocol::Vehicle::Role::FOLLOWER, c2cNet, readNetworkInfo())
, pcNet()
, pc(pc2car::CommandReceiver::create(pcNet))
, egoMotion(nh)
, platoonController(c2c, pc, egoMotion)
, platoonControllerThread([this] { platoonController.run(); })
- , cruiseController(egoMotion, platoonController)
+ , cruiseController(c2c, egoMotion, platoonController)
, cruiseControllerThread([this] { cruiseController.run(); })
{
egoMotion.cruiseControllerNotify = cruiseControllerNotify;
egoMotion.platoonControllerNotify = platoonControllerNotify;
platoonController.cruiseControllerNotify = cruiseControllerNotify;
- // pc->reciveCommands([this] (auto commandCode) {platoonControllerThread.notify();} )
+ pc->receiveCommands([this] (auto commandCode) {platoonControllerThread.notify();} );
+ c2c.setCallback( [this] {platoonControllerThread.notify();} );
}
MainNode::~MainNode()