From f9c0d6a45f85841109e7823ba4e42c3cdc3f171b Mon Sep 17 00:00:00 2001
From: Franz Bethke <bethke@math.hu-berlin.de>
Date: Sat, 28 Apr 2018 19:58:55 +0200
Subject: [PATCH] Implement CC
---
.../car/include/mainNode/CruiseController.h | 23 +++++-
.../src/car/src/mainNode/CruiseController.cpp | 77 ++++++++++++++++++-
.../src/car/src/mainNode/MainNode.cpp | 4 +-
3 files changed, 98 insertions(+), 6 deletions(-)
diff --git a/modules/catkin_ws/src/car/include/mainNode/CruiseController.h b/modules/catkin_ws/src/car/include/mainNode/CruiseController.h
index a94dcb78..c2f16444 100644
--- a/modules/catkin_ws/src/car/include/mainNode/CruiseController.h
+++ b/modules/catkin_ws/src/car/include/mainNode/CruiseController.h
@@ -1,9 +1,12 @@
#ifndef CRUISECONTROLLER_H
#define CRUISECONTROLLER_H
+#include <nodelet/nodelet.h>
+#include <ros/ros.h>
#include "PlatoonProtocolLib/VehicleFacade.h"
#include "PlatoonController.h"
#include "EgoMotion.h"
+#include "PlatoonState.h"
namespace car
{
@@ -12,7 +15,8 @@ class CruiseController
public:
CruiseController(platoonProtocol::VehicleFacade& c2c,
EgoMotion& egoMotion,
- PlatoonController& platoonController);
+ PlatoonController& platoonController,
+ ros::NodeHandle & nh);
~CruiseController(){};
@@ -22,6 +26,23 @@ class CruiseController
platoonProtocol::VehicleFacade& c2c;
EgoMotion& egoMotion;
PlatoonController& platoonController;
+
+ float min_speed{0};
+ float max_speed{20};
+ float crash_time{5};
+ float stop_dist{20};
+
+ float eps{1e-9};
+ float speed{0};
+ float units_conv_fac{1};
+
+ float dist_pow {1.5};
+ float speed_pow{0.5};
+
+ void run_ACC(float target_speed);
+ void run_CACC();
+
+ ros::Publisher ccData;
};
}
#endif
diff --git a/modules/catkin_ws/src/car/src/mainNode/CruiseController.cpp b/modules/catkin_ws/src/car/src/mainNode/CruiseController.cpp
index 32651413..24c3ec17 100644
--- a/modules/catkin_ws/src/car/src/mainNode/CruiseController.cpp
+++ b/modules/catkin_ws/src/car/src/mainNode/CruiseController.cpp
@@ -1,21 +1,92 @@
#include "mainNode/CruiseController.h"
#include "PlatoonProtocolLib/VehicleFacade.h"
+#include "PlatoonProtocolLib/Protocol.h"
+
+#include "car/ccDataMsg.h"
#include <iostream>
+#include <algorithm>
namespace car {
CruiseController::CruiseController(platoonProtocol::VehicleFacade& c2c,
EgoMotion& egoMotion,
- PlatoonController& platoonController)
+ PlatoonController& platoonController,
+ ros::NodeHandle& nh)
: c2c(c2c)
, egoMotion(egoMotion)
, platoonController(platoonController)
-{ }
+ {
+ ccData = nh.advertise<ccDataMsg>("ccData", 1);
+ }
void CruiseController::run() {
- std::cout << "CruiseController was run." << std::endl;
+ switch (platoonController.curState) {
+ case PlatoonState::ACC: {
+ run_ACC(platoonController.getDesSpeed());
+ break;
+ }
+ case PlatoonState::CACC_LV: {
+ platoonProtocol::PlatoonConfig config = platoonController.platoonConfig;
+ run_ACC(config.platoonSpeed);
+ c2c.setPlatoonSpeed(speed);
+ break;
+ }
+ case PlatoonState::CACC_FV: { run_CACC(); break; }
+ }
+}
+
+void CruiseController::run_ACC(float target_speed) {
+
+ std::cout << "CruiseController::run_ACC("<< target_speed <<")" << std::endl;
+ float v_self = egoMotion.getOwnSpeed();
+ float d = egoMotion.getDistance();
+ float crash_dist = crash_time * v_self;
+
+ if ( d < crash_dist ) {
+ std::cout << "in crash_dist" << std::endl;
+ speed = target_speed*(d - stop_dist)/crash_dist;
+ } else {
+ std::cout << "not in crash_dist" << std::endl;
+ speed = target_speed;
+ }
+
+ speed = std::min(std::max(speed, min_speed), max_speed);
+
+ ccDataMsg outMsg;
+ outMsg.speed = speed;
+ ccData.publish(outMsg);
+
+ return;
+}
+
+void CruiseController::run_CACC() {
+
+ std::cout << "CruiseController::run_CACC" << std::endl;
+
+ platoonProtocol::PlatoonConfig config = platoonController.platoonConfig;
+ float PS = config.platoonSpeed;
+ float IPD = config.innerPlatoonDistance;
+ float vp = egoMotion.getPrvSpeed();
+ float d = egoMotion.getDistance();
+
+ if ( IPD < 1 ) { IPD = 1; }
+ if ( vp < eps && vp > -eps ) {
+ if ( d < IPD ) { speed = 0; return; }
+ speed = units_conv_fac*0.5*(d-IPD)/IPD; // THIS IS DEPENDEND ON THE UNITS OF speed AND d!!!
+ } else {
+ speed = pow(d/IPD,dist_pow) * pow(PS/vp,speed_pow) * vp;
+ }
+
+ speed = std::min(std::max(speed, min_speed), max_speed);
+
+ ccDataMsg outMsg;
+ outMsg.speed = speed;
+ ccData.publish(outMsg);
+
+ return;
}
+
}
diff --git a/modules/catkin_ws/src/car/src/mainNode/MainNode.cpp b/modules/catkin_ws/src/car/src/mainNode/MainNode.cpp
index 11d44623..452ded78 100644
--- a/modules/catkin_ws/src/car/src/mainNode/MainNode.cpp
+++ b/modules/catkin_ws/src/car/src/mainNode/MainNode.cpp
@@ -70,7 +70,7 @@ MainNode::MainNode(ros::NodeHandle & nh, std::string & name) :
, egoMotion(nh)
, platoonController(c2c, pc, egoMotion)
, platoonControllerThread([this] { platoonController.run(); })
- , cruiseController(c2c, egoMotion, platoonController)
+ , cruiseController(c2c, egoMotion, platoonController, nh)
, cruiseControllerThread([this] { cruiseController.run(); })
{
egoMotion.cruiseControllerNotify = cruiseControllerNotify;
@@ -90,7 +90,7 @@ MainNode::MainNode() :
, egoMotion(nh)
, platoonController(c2c, pc, egoMotion)
, platoonControllerThread([this] { platoonController.run(); })
- , cruiseController(c2c, egoMotion, platoonController)
+ , cruiseController(c2c, egoMotion, platoonController, nh)
, cruiseControllerThread([this] { cruiseController.run(); })
{
egoMotion.cruiseControllerNotify = cruiseControllerNotify;
--
GitLab