From f17bcbc1219f3939451a7cdfff84fd4cdaa93ad6 Mon Sep 17 00:00:00 2001 From: fetty Date: Mon, 28 Nov 2022 22:02:23 +0100 Subject: [PATCH] PID node added with nice velocity profile --- CMakeLists.txt | 7 +- cfg/long.cfg | 15 +++ include/longitudinal.hh | 72 +++++++++++++++ include/mpc.hh | 1 - params/tailored_mpc.yaml | 5 + solver/plots.m | 30 ++++-- src/longitudinal.cpp | 195 +++++++++++++++++++++++++++++++++++++++ src/mpc.cpp | 44 ++++----- src/pid.cpp | 80 ++++++++++++++++ 9 files changed, 416 insertions(+), 33 deletions(-) create mode 100644 cfg/long.cfg create mode 100644 include/longitudinal.hh create mode 100644 src/longitudinal.cpp create mode 100644 src/pid.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 573713f..793a773 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.0.2) project(tailored_mpc) #add_compile_options(-std=c++0x) -set(CMAKE_CXX_FLAGS "-fpermissive -fopenmp -std=c++0x") +set(CMAKE_CXX_FLAGS "-fpermissive -fopenmp -std=c++11") set(CMAKE_BUILD_TYPE "Release") message(STATUS "TAILORED MPC says hi :)") @@ -28,6 +28,7 @@ find_library(FORCESPRO_LIBRARY libTailoredSolver.a PATHS ${PROJECT_SOURCE_DIR}/s generate_dynamic_reconfigure_options( cfg/dynamic.cfg + cfg/long.cfg ) catkin_package( @@ -41,6 +42,8 @@ include_directories( ) add_executable(${PROJECT_NAME}_exec src/main.cpp) +add_executable(pid_exec src/pid.cpp + src/longitudinal.cpp) add_library(${PROJECT_NAME} src/utils/params.cpp @@ -53,9 +56,11 @@ add_library(${PROJECT_NAME} target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${FORCESPRO_LIBRARY}) target_link_libraries(${PROJECT_NAME}_exec ${catkin_LIBRARIES} ${PROJECT_NAME}) +target_link_libraries(pid_exec ${catkin_LIBRARIES}) add_dependencies(${PROJECT_NAME} as_msgs_generate_messages_cpp) add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencfg) +add_dependencies(pid_exec ${PROJECT_NAME}_gencfg) # Mandatory for parallelism (OpenMP) target_compile_options(${PROJECT_NAME} PRIVATE ${OpenMP_FLAGS}) diff --git a/cfg/long.cfg b/cfg/long.cfg new file mode 100644 index 0000000..bc72bd8 --- /dev/null +++ b/cfg/long.cfg @@ -0,0 +1,15 @@ +#!/usr/bin/env python +PACKAGE = "tailored_mpc" + +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() + +gen.add("Ax_acc_max", double_t, 0, "Maximum longitudinal acceleration (when accelerating)", 8.0, 0.0, 20.0) +gen.add("Ax_dec_max", double_t, 0, "Maximum longitudinal acceleration (when braking)", 10.0, 0.0, 20.0) +gen.add("Ay_max", double_t, 0, "Maximum lateral acceleration", 10.0, 0.0, 20.0) +gen.add("Vx_max", double_t, 0, "Maximum longitudinal velocity", 20.0, 0.0, 30.0) +gen.add("Vx_final", double_t, 0, "Final velocity", 15.0, 0.0, 20.0) + + +exit(gen.generate(PACKAGE, "longitudinal_controller", "long")) \ No newline at end of file diff --git a/include/longitudinal.hh b/include/longitudinal.hh new file mode 100644 index 0000000..8ead166 --- /dev/null +++ b/include/longitudinal.hh @@ -0,0 +1,72 @@ +#ifndef LONG_HH +#define LONG_HH + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// Dynamic reconfigure headers +#include +#include + +// Msgs used +#include "as_msgs/ObjectiveArrayCurv.h" +#include "as_msgs/CarState.h" +#include "as_msgs/CarCommands.h" + +using namespace std; + +class PID{ + + private: + + int minPoints = 10; + + double ax_acc = 8, ax_dec = 10; // Maximum longitudinal acceleration + double ay_max = 10; // Maximum lateral acceleration + double spacing = 0.025; // Planner discretization + double vx_max = 20, vx_final = 12; // velocities restrictions + + bool stateFlag = false, velFlag = false; + bool dynParamFlag = false; + + void velocityProfile(); + double f_accel(int k, double v); + double f_decel(int k, double v); + vector findLocalMax(Eigen::VectorXd curv); + void saveEigen(string filePath, string name, Eigen::MatrixXd data, bool erase); + template void printVec(vector &input, int firstElements); + + public: + + PID(); + void stateCallback(const as_msgs::CarState::ConstPtr& msg); + void plannerCallback(const as_msgs::ObjectiveArrayCurv::ConstPtr& msg); + void reconfigure(tailored_mpc::longConfig& config); + void msgCommands(as_msgs::CarCommands *msg); + + void run(); // run PI controller + + // Planner's trajectory matrix + Eigen::MatrixXd planner; // [x, y, s, k] + + //Actual state of the car + Eigen::VectorXd carState; // [x, y, theta, vx, vy, w, delta(steering), acc, Mtv] + + // Velocity profile vector + Eigen::VectorXd velocities; + + // throttle + double throttle = 0; + +}; + + +#endif \ No newline at end of file diff --git a/include/mpc.hh b/include/mpc.hh index 176f60f..45f0697 100644 --- a/include/mpc.hh +++ b/include/mpc.hh @@ -172,7 +172,6 @@ class MPC{ // Previous solution Eigen::MatrixXd solStates; // [n, mu, vy, w] Eigen::MatrixXd solCommands; // [slack_track, diff_delta, Mtv, delta] - }; diff --git a/params/tailored_mpc.yaml b/params/tailored_mpc.yaml index 40c9f85..52f13e4 100644 --- a/params/tailored_mpc.yaml +++ b/params/tailored_mpc.yaml @@ -34,6 +34,11 @@ NLOP: N: 20 # Prediction horizon Nslacks: 1 # Number of slacks variables +PID: + Hz: 40 # Longitudinal PI frequency + Topics: + Commands: /AS/C/motor + Hz: 20 #[s] MPC frequency rk4_t: 0.020 #[s] Runge Kutta integration time Nthreads: 10 # Number of threads diff --git a/solver/plots.m b/solver/plots.m index ad8eac7..908b2c4 100644 --- a/solver/plots.m +++ b/solver/plots.m @@ -1,15 +1,27 @@ %% PLOTS -time = readtable('/home/fetty/Desktop/control_ws2022/src/control/tailored_mpc/debug/solve_time.txt'); -% exitflags = readtable('/home/fetty/Desktop/control_ws2022/src/control/tailored_mpc/debug/exit_flags.txt'); - -meanTime = mean(time.Var3) -plot(time.Var1.*10,time.Var3) -title("Solving time") - -axis equal +% time = readtable('/home/fetty/Desktop/control_ws2022/src/control/tailored_mpc/debug/solve_time.txt'); +% % exitflags = readtable('/home/fetty/Desktop/control_ws2022/src/control/tailored_mpc/debug/exit_flags.txt'); +% +% meanTime = mean(time.Var3) +% plot(time.Var1.*10,time.Var3) +% title("Solving time") +% +% axis equal % meanExitflag = mean(exitflags.Var3) % figure() % plot(exitflags.Var1,exitflags.Var3) -% title("Exit Flags") \ No newline at end of file +% title("Exit Flags") + +vfinal = readtable('/home/fetty/Desktop/velocity_profile.csv'); +vaccel = readtable('/home/fetty/Desktop/v_accel.csv'); +vdecel = readtable('/home/fetty/Desktop/v_decel.csv'); + +figure() +plot(vfinal.Var1) + +figure() +plot(vaccel.Var1) +hold on +plot(vdecel.Var1) diff --git a/src/longitudinal.cpp b/src/longitudinal.cpp new file mode 100644 index 0000000..3371c64 --- /dev/null +++ b/src/longitudinal.cpp @@ -0,0 +1,195 @@ +#include "longitudinal.hh" + +PID::PID(){ + carState = Eigen::VectorXd::Zero(9); +} + +/////////////////////////////////////////////////////////////////////////////////////////////// +//------------------------Callback functions-------------------------------------------------- + +void PID::stateCallback(const as_msgs::CarState::ConstPtr& msg){ + + // carState << msg->odom.position.x, msg->odom.position.y, msg->odom.heading, msg->odom.velocity.x, msg->odom.velocity.y + msg->odom.velocity.w*d_IMU, msg->odom.velocity.w, msg->steering, msg->odom.acceleration.x, msg->Mtv; + carState << msg->odom.position.x, msg->odom.position.y, msg->odom.heading, msg->odom.velocity.x, msg->odom.velocity.y, msg->odom.velocity.w, msg->steering, msg->odom.acceleration.x, msg->Mtv; + stateFlag = true; +} + +void PID::plannerCallback(const as_msgs::ObjectiveArrayCurv::ConstPtr& msg){ + + if (msg->objectives.size() < minPoints){ + ROS_WARN("PID::Planner is too short!"); + return; + } + + // Fill planner matrix + planner.resize(msg->objectives.size(),4); + for (unsigned int i = 0; i < msg->objectives.size(); i++) + { + planner(i, 0) = msg->objectives[i].x; + planner(i, 1) = msg->objectives[i].y; + planner(i, 2) = msg->objectives[i].s; + planner(i, 3) = msg->objectives[i].k; + } + + if(this->stateFlag /*&& this->dynParamFlag*/) velocityProfile(); +} + +/////////////////////////////////////////////////////////////////////////////////////////////// +//------------------------Principal functions-------------------------------------------------- + +void PID::run(){ + + if(this->velFlag){ + // Proportional + // double error = theorical_slip - slip; + // double Pout = Kp*error; + + // Integral + // double period = 25e-3; + // if(mode == 0) period = 50e-3; + // integral += error*period; + // double Iout = Ki*integral; + + // Apply PI + // double torque = Pout + Iout + Dout; + // cout << "T: " << torque << endl; + // cout << "without control: " << desired_torque << endl; + + // // Save previous slip error + // previous_error = error; + } +} + +void PID::velocityProfile(){ + + vector apexes = findLocalMax(planner.col(3)); + printVec(apexes,0); + velocities.resize(planner.rows()); // final velocity profile vector + + // First step: restrain limit acceleration + Eigen::VectorXd vAccel(planner.rows()); + vAccel(0) = carState(3); + for(unsigned int j=0; j < planner.rows()-1; j++){ + vAccel(j+1) = vAccel(j) + spacing*f_accel(j, vAccel(j)); // Euler integration + if( vAccel(j+1) > sqrt(ay_max/fabs(planner(j,3))) ) vAccel(j+1) = sqrt(ay_max/fabs(planner(j,3))); + } + + // Second step: restrain limit deceleration + Eigen::VectorXd vDecel(apexes[apexes.size()-1]+1); + vDecel(0) = vAccel(apexes[apexes.size()-1]); + int idx; // global index in planner matrix + for(unsigned int j=0; j < apexes[apexes.size()-1]; j++){ + idx = apexes[apexes.size()-1]-j; // transform index to global (we are going backwards) + vDecel(j+1) = vDecel(j) + spacing*f_decel(idx, vDecel(j)); // Euler integration + if( vDecel(j+1) > sqrt(ay_max/fabs(planner(idx,3))) ) vDecel(j+1) = sqrt(ay_max/fabs(planner(idx,3))); + } + Eigen::VectorXd vDecelGlobal = vDecel.reverse(); // reverse vDecel vector + + // Third step: pick min values from both profiles + for(unsigned int k=0; k < velocities.size(); k++){ + if(k <= apexes[apexes.size()-1]) velocities(k) = min(vAccel(k),vDecelGlobal(k)); + else velocities(k) = vAccel(k); + } + + velFlag = true; + saveEigen("/home/fetty/Desktop/", "velocity_profile.csv", velocities, true); + saveEigen("/home/fetty/Desktop/", "v_accel.csv", vAccel, true); + saveEigen("/home/fetty/Desktop/", "v_decel.csv", vDecelGlobal, true); + saveEigen("/home/fetty/Desktop/", "curvature.csv", planner.col(3), true); +} + + +/////////////////////////////////////////////////////////////////////////////////////////////// +//------------------------Auxiliar functions-------------------------------------------------- + +void PID::msgCommands(as_msgs::CarCommands *msg){ + + msg->header.stamp = ros::Time::now(); + msg->motor = throttle; + + return; +} + +// EDO models of the acceleration physics +double PID::f_accel(int k, double v){ + if(v < sqrt(ay_max/fabs(planner(k,3))) ){ + return ax_acc/v * sqrt( 1 - pow(pow(v,2)*fabs(planner(k,3))/ay_max, 2) ); // EDO Model + } else { + return 0; // Can't accelerate more + } +} +double PID::f_decel(int k, double v){ + if(v < sqrt(ay_max/fabs(planner(k,3))) ){ + return ax_dec/v * sqrt( 1 - pow(pow(v,2)*fabs(planner(k,3))/ay_max, 2) ); // EDO Model + } else { + return 0; // Can't decelerate more + } +} + +template +void PID::printVec(vector &input, int firstElements){ + if(firstElements!=0){ + for (auto it = input.begin(); it != input.end()-input.size()+firstElements; it++) { + cout << *it << "\n"; + } + }else{ + for (auto it = input.begin(); it != input.end(); it++) { + cout << *it << "\n"; + } + } +} + +vector PID::findLocalMax(Eigen::VectorXd curv){ // Find curvature apexes + vector result; + + if(curv(0) > curv(1)) result.push_back(0); // Check whether the first point is local max + + for(int i=1; i < curv.size()-1; i++){ + if(curv(i-1) < curv(i) && curv(i) > curv(i+1)) result.push_back(i); + } + + if(curv(curv.size()-1) > curv(curv.size()-2)) result.push_back(curv.size()-1); + + return result; +} + +void PID::saveEigen(string filePath, string name, Eigen::MatrixXd data, bool erase){ + + try{ + + // Write csv + ofstream file; + if(erase) file.open(filePath + name, ios::out | ios::trunc); + else file.open(filePath + name, ios::app); + + //https://eigen.tuxfamily.org/dox/structEigen_1_1IOFormat.html + const static Eigen::IOFormat CSVFormat(Eigen::FullPrecision, Eigen::DontAlignCols, ", ", "\n"); + + if (file.is_open()){ + file << data.format(CSVFormat); + file.close(); // Remeber to close the file! + } + + // ROS_INFO_STREAM("MPC: matrix data SAVED AT: " << this->savePath+name); + } + catch (const std::exception& e) + { + ROS_ERROR_STREAM( "PID::saveEigen Exception was thrown: " << e.what() ); + } +} + +void PID::reconfigure(tailored_mpc::longConfig& config){ + + try{ + this->ax_acc = config.Ax_acc_max; + this->ax_dec = config.Ax_dec_max; + this->ay_max = config.Ay_max; + this->vx_final = config.Vx_final; + this->vx_max = config.Vx_max; + + this->dynParamFlag = true; + + } catch (exception& e){ + ROS_ERROR_STREAM("PID::RECONFIGURE DIED" << e.what()); + } +} diff --git a/src/mpc.cpp b/src/mpc.cpp index ebc738f..be3e05f 100644 --- a/src/mpc.cpp +++ b/src/mpc.cpp @@ -70,7 +70,6 @@ void MPC::stateCallback(const as_msgs::CarState::ConstPtr& msg){ // carState << msg->odom.position.x, msg->odom.position.y, msg->odom.heading, msg->odom.velocity.x, msg->odom.velocity.y + msg->odom.velocity.w*d_IMU, msg->odom.velocity.w, 0.0, msg->odom.acceleration.x; stateFlag = true; - } @@ -97,6 +96,7 @@ void MPC::plannerCallback(const as_msgs::ObjectiveArrayCurv::ConstPtr& msg){ smax = 2000; plannerFlag = true; } + } void MPC::troCallback(const as_msgs::ObjectiveArrayCurv::ConstPtr& msg){ @@ -119,7 +119,7 @@ void MPC::troCallback(const as_msgs::ObjectiveArrayCurv::ConstPtr& msg){ } smax = msg->smax; - cout << "SMAX: " << smax << endl; + // cout << "SMAX: " << smax << endl; plannerFlag = troActive = true; ROS_WARN_ONCE("MPC: FOLLOWING TRO! :)"); @@ -149,9 +149,9 @@ void MPC::solve(){ // Solve forces.exit_flag = TailoredSolver_solve(&forces.params, &forces.solution, &forces.info, forces.mem_handle, NULL, forces.ext_func); - ROS_ERROR_STREAM("MPC exit flag: " << forces.exit_flag); - ROS_ERROR_STREAM("MPC solve time: " << forces.info.solvetime*1000 << " ms"); - ROS_ERROR_STREAM("MPC iterations: " << forces.info.it); + // ROS_ERROR_STREAM("MPC exit flag: " << forces.exit_flag); + // ROS_ERROR_STREAM("MPC solve time: " << forces.info.solvetime*1000 << " ms"); + // ROS_ERROR_STREAM("MPC iterations: " << forces.info.it); if(forces.exit_flag == 1) this->firstIter = false; else this->firstIter = true; @@ -163,7 +163,7 @@ void MPC::solve(){ elapsed_time = finish_time - start_time; - ROS_WARN("TAILORED MPC elapsed time: %f ms", elapsed_time.count()*1000); + // ROS_WARN("TAILORED MPC elapsed time: %f ms", elapsed_time.count()*1000); // Save data // save("/home/fetty/Desktop/control_ws2022/src/control/tailored_mpc/debug/", "solve_time.txt", elapsed_time.count()*1000, true); @@ -208,8 +208,8 @@ void MPC::initial_conditions(){ double t_angle = atan2(tangent(1), tangent(0)); // heading of the trajectory double mu0 = -(t_angle - continuous(carState(2), t_angle)); - ROS_WARN_STREAM("mu0: " << mu0); - ROS_WARN_STREAM("n0: " << n0); + // ROS_WARN_STREAM("mu0: " << mu0); + // ROS_WARN_STREAM("n0: " << n0); // xinit = [diff_delta, Mtv, delta, n, mu, Vy, w] forces.params.xinit[0] = 0.0; @@ -220,12 +220,12 @@ void MPC::initial_conditions(){ forces.params.xinit[5] = carState(4); forces.params.xinit[6] = carState(5); - cout << "Xinit:\n"; - for(int i=0;i<7;i++){ - cout << forces.params.xinit[i] << endl; - } + // cout << "Xinit:\n"; + // for(int i=0;i<7;i++){ + // cout << forces.params.xinit[i] << endl; + // } - cout << "Steering feedback: " << carState(6) << endl; + // cout << "Steering feedback: " << carState(6) << endl; } @@ -239,8 +239,8 @@ void MPC::set_params_bounds(){ int nh = int(size/this->N); int Nvar = this->n_states + this->n_controls; - cout << "nh: " << nh << endl; - cout << "Nvar: " << Nvar << endl; + // cout << "nh: " << nh << endl; + // cout << "Nvar: " << Nvar << endl; if(!firstIter){ @@ -329,7 +329,7 @@ void MPC::set_params_bounds(){ progress(k) = planner(plannerIdx, 2); this->forces.params.all_parameters[24 + k*this->Npar] = planner(plannerIdx, 3); // curvature - cout << "Curvature: " << forces.params.all_parameters[30+k*Npar] << endl; + // cout << "Curvature: " << forces.params.all_parameters[24+k*Npar] << endl; // Inequality constraints bounds: this->forces.params.hu[k*nh] = fabs(planner(plannerIdx, 5)); // L(s) ortogonal left dist from the path to the track limits @@ -429,7 +429,7 @@ void MPC::set_params_bounds(){ void MPC::s_prediction(){ predicted_s.setZero(); - cout << "S_PRED inicial: " << progress(0) << endl; + // cout << "S_PRED inicial: " << progress(0) << endl; predicted_s(0) = fmod(progress(0), smax); lastState(0,0) = carState(0); @@ -480,7 +480,7 @@ void MPC::s_prediction(){ // If predicted s is too small set initial s again double totalLength = predicted_s(this->N-1) - predicted_s(0); if(totalLength < 1){ - ROS_ERROR("Predicted s smaller than threshold!"); + // ROS_ERROR("Predicted s smaller than threshold!"); firstIter = true; } @@ -506,8 +506,8 @@ void MPC::get_solution(){ // cout << "solStates: " << endl; // cout << solStates << endl; - cout << "solCommands:\n"; - cout << solCommands << endl; + // cout << "solCommands:\n"; + // cout << solCommands << endl; } @@ -518,8 +518,8 @@ void MPC::msgCommands(as_msgs::CarCommands *msg){ msg->steering = solCommands(this->latency, 3); msg->Mtv = solCommands(this->latency, 2); - cout << "steering: " << solCommands(this->latency, 3) << endl; - cout << "Mtv: " << solCommands(this->latency, 2) << endl; + // cout << "steering: " << solCommands(this->latency, 3) << endl; + // cout << "Mtv: " << solCommands(this->latency, 2) << endl; return; } diff --git a/src/pid.cpp b/src/pid.cpp new file mode 100644 index 0000000..77b718f --- /dev/null +++ b/src/pid.cpp @@ -0,0 +1,80 @@ +#include +#include +#include "longitudinal.hh" + +ros::Publisher pubCommands; // Commands publisher + +// void dynamicCallback(tailored_mpc::longConfig &config, uint32_t level, PID* pid){ + +// ROS_WARN("PID: Setting new dynamic parameters.."); +// pid->reconfigure(config); + +// } + +void my_SIGhandler(int sig){ + + as_msgs::CarCommands msgCommands; + msgCommands.motor = -1.0; + msgCommands.steering = 0.0; + msgCommands.Mtv = 0.0; + for(int i=0; i<5; i++){ + pubCommands.publish(msgCommands); + ros::Duration(0.05).sleep(); + } + ROS_ERROR("PID says Goodbye :)"); + ros::shutdown(); +} + +int main(int argc, char **argv) { + + // Init Node: + ros::init(argc, argv, "pid"); + + // Handle Connections: + ros::NodeHandle nh; + + // Signal handler for publishing 0s when dying + signal(SIGINT, my_SIGhandler); // override default ros sigint signal + + // Get params + string stateTopic, plannerTopic, troTopic, commandsTopic; + nh.param("/tailored_mpc/Topics/State", stateTopic, "/AS/C/state"); + nh.param("/tailored_mpc/Topics/Planner", plannerTopic, "/AS/C/trajectory/partial"); + nh.param("/tailored_mpc/Topics/Tro", troTopic, "/AS/C/trajectory/full"); + nh.param("/tailored_mpc/PID/Topics/Commands", commandsTopic, "/AS/C/motor"); + + int freq; + nh.param("/tailored_mpc/PID/Hz", freq, 40); + + // Longitudinal controller object + PID pid; + + // Publishers & Subscribers + ros::Subscriber subState = nh.subscribe(stateTopic, 1, &PID::stateCallback, &pid); + ros::Subscriber subPlanner = nh.subscribe(plannerTopic, 1, &PID::plannerCallback, &pid); + ros::Subscriber subTro = nh.subscribe(troTopic, 1, &PID::plannerCallback, &pid); + pubCommands = nh.advertise(commandsTopic, 1); + + // Dynamic reconfigure + // dynamic_reconfigure::Server server; + // dynamic_reconfigure::Server::CallbackType f; + // f = boost::bind(&dynamicCallback, _1, _2, &pid); + // server.setCallback(f); + + // Msg declaration + as_msgs::CarCommands msg; + + ros::Rate r(freq); + while(ros::ok()){ + + pid.run(); // Solve the NLOP + + pid.msgCommands(&msg); + pubCommands.publish(msg); // publish car commands + + ros::spinOnce(); + r.sleep(); + } + + return 0; +} \ No newline at end of file