From 7ee86e6a7529a54337cb5e0b74241804eeb3e8bc Mon Sep 17 00:00:00 2001 From: fetty Date: Mon, 5 Dec 2022 17:46:25 +0100 Subject: [PATCH] first functional version with pid --- CMakeLists.txt | 5 - cfg/long.cfg | 15 --- include/longitudinal.hh | 72 -------------- include/mpc.hh | 6 +- include/utils/params.hh | 1 + package.xml | 2 +- params/dyn_trackdrive.yaml | 24 ++--- params/tailored_mpc.yaml | 19 ++-- solver/fx_table.m | 16 --- solver/rtwTargetInfo.m | 12 --- solver/stdout_temp | 4 - src/longitudinal.cpp | 195 ------------------------------------- src/main.cpp | 1 + src/mpc.cpp | 17 +++- src/pid.cpp | 80 --------------- src/utils/params.cpp | 11 ++- 16 files changed, 48 insertions(+), 432 deletions(-) delete mode 100644 cfg/long.cfg delete mode 100644 include/longitudinal.hh delete mode 100644 solver/fx_table.m delete mode 100644 solver/rtwTargetInfo.m delete mode 100644 solver/stdout_temp delete mode 100644 src/longitudinal.cpp delete mode 100644 src/pid.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 793a773..c9669cd 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -28,7 +28,6 @@ find_library(FORCESPRO_LIBRARY libTailoredSolver.a PATHS ${PROJECT_SOURCE_DIR}/s generate_dynamic_reconfigure_options( cfg/dynamic.cfg - cfg/long.cfg ) catkin_package( @@ -42,8 +41,6 @@ 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 @@ -56,11 +53,9 @@ 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 deleted file mode 100644 index bc72bd8..0000000 --- a/cfg/long.cfg +++ /dev/null @@ -1,15 +0,0 @@ -#!/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 deleted file mode 100644 index 8ead166..0000000 --- a/include/longitudinal.hh +++ /dev/null @@ -1,72 +0,0 @@ -#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 45f0697..15e7fe3 100644 --- a/include/mpc.hh +++ b/include/mpc.hh @@ -20,6 +20,7 @@ #include "as_msgs/ObjectiveArrayCurv.h" #include "as_msgs/CarState.h" #include "as_msgs/CarCommands.h" +#include "as_msgs/CarVelocityArray.h" // Utilities for parameters #include "utils/params.hh" @@ -51,7 +52,9 @@ class MPC{ // Internal variables/methods of MPC - bool plannerFlag = false, stateFlag = false; + Eigen::VectorXd pred_velocities; // predicted velocities (vel profile from long_pid pkg) + + bool plannerFlag = false, stateFlag = false, velsFlag = false; bool paramFlag = false; // flag for parameters set up bool dynParamFlag = false; // flag for dynamic parameters set up bool troActive = false, troProfile = false; // whether TRO/GRO are publishing @@ -143,6 +146,7 @@ class MPC{ void stateCallback(const as_msgs::CarState::ConstPtr& msg); void plannerCallback(const as_msgs::ObjectiveArrayCurv::ConstPtr& msg); void troCallback(const as_msgs::ObjectiveArrayCurv::ConstPtr& msg); + void velsCallback(const as_msgs::CarVelocityArray::ConstPtr& msg); // Solve method void solve(); diff --git a/include/utils/params.hh b/include/utils/params.hh index a664681..214c8b6 100644 --- a/include/utils/params.hh +++ b/include/utils/params.hh @@ -34,6 +34,7 @@ struct Params{ string state; // Car State topic string planner; // Planner topic string tro; // Offline planner topic + string velocities; string predictedSteering; // Visualization topics string predictedPath; string predictedHeading; diff --git a/package.xml b/package.xml index 927ddee..f22380f 100644 --- a/package.xml +++ b/package.xml @@ -4,7 +4,7 @@ 0.0.0 The tailored_mpc package - fetty + fetty TODO diff --git a/params/dyn_trackdrive.yaml b/params/dyn_trackdrive.yaml index 489cbff..9d61bc5 100644 --- a/params/dyn_trackdrive.yaml +++ b/params/dyn_trackdrive.yaml @@ -8,10 +8,10 @@ dictitems: Df: 2785.4 Dr: 3152.3 Vy_max: 5.0 - YawRate_max: 50.0 + YawRate_max: 40.0 dMtv: 1.0 - dRd: 0.2 - diff_delta: 100.0 + dRd: 0.1 + diff_delta: 1000.0 groups: !!python/object/new:dynamic_reconfigure.encoding.Config dictitems: Bf: 10.8529 @@ -22,14 +22,14 @@ dictitems: Df: 2785.4 Dr: 3152.3 Vy_max: 5.0 - YawRate_max: 50.0 + YawRate_max: 40.0 dMtv: 1.0 - dRd: 0.2 - diff_delta: 100.0 + dRd: 0.1 + diff_delta: 1000.0 groups: !!python/object/new:dynamic_reconfigure.encoding.Config state: [] id: 0 - latency: 5 + latency: 15 name: Default parameters: !!python/object/new:dynamic_reconfigure.encoding.Config state: [] @@ -37,20 +37,20 @@ dictitems: q_mu: 0.1 q_n: 10.0 q_nN: 15.0 - q_s: 30.0 - q_sN: 30.0 + q_s: 70.0 + q_sN: 70.0 q_slack_track: 100.0 q_slip: 2.0 state: true type: '' u_r: 0.45 state: [] - latency: 5 + latency: 15 q_mu: 0.1 q_n: 10.0 q_nN: 15.0 - q_s: 30.0 - q_sN: 30.0 + q_s: 70.0 + q_sN: 70.0 q_slack_track: 100.0 q_slip: 2.0 u_r: 0.45 diff --git a/params/tailored_mpc.yaml b/params/tailored_mpc.yaml index 52f13e4..f3c29bd 100644 --- a/params/tailored_mpc.yaml +++ b/params/tailored_mpc.yaml @@ -18,6 +18,7 @@ Topics: Commands: /AS/C/commands Planner: /AS/C/trajectory/partial Tro: /AS/C/trajectory/full + Velocities: /AS/C/pid/velocity Debug: Time: /AS/C/mpc/debug/time ExitFlag: /AS/C/mpc/debug/exitflags @@ -32,16 +33,12 @@ Topics: NLOP: N: 20 # Prediction horizon - Nslacks: 1 # Number of slacks variables + Nslacks: 1 # Number of slacks variables -PID: - Hz: 40 # Longitudinal PI frequency - Topics: - Commands: /AS/C/motor +MPC: + Hz: 20 #[s] MPC frequency + rk4_t: 0.020 #[s] Runge Kutta integration time + Nthreads: 10 # Number of threads + TroProfile: false # Set to true to follow TRO velocity profile -Hz: 20 #[s] MPC frequency -rk4_t: 0.020 #[s] Runge Kutta integration time -Nthreads: 10 # Number of threads -TroProfile: false # Set to true to follow TRO velocity profile - -nPlanning: 1900 # n points saved from the planner + nPlanning: 1900 # n points saved from the planner diff --git a/solver/fx_table.m b/solver/fx_table.m deleted file mode 100644 index f6dd778..0000000 --- a/solver/fx_table.m +++ /dev/null @@ -1,16 +0,0 @@ -function hLib = fx_table - - -hLib = RTW.TflTable; -%---------- entry: lookupFx ----------- -hEnt = createCRLEntry(hLib, ... - 'double y1 = lookupFx( double u1, double u2[2 2], double u3[0 0; Inf Inf], int32 u4 )', ... - 'double y1 = crl_LUT( double u1, double* u2, double* u3, int32 u4 )'); -hEnt.setTflCFunctionEntryParameters( ... - 'Priority', 100, ... - 'SideEffects', true); - - - -hLib.addEntry( hEnt ); - diff --git a/solver/rtwTargetInfo.m b/solver/rtwTargetInfo.m deleted file mode 100644 index 3007368..0000000 --- a/solver/rtwTargetInfo.m +++ /dev/null @@ -1,12 +0,0 @@ -function rtwTargetInfo(cm) - -cm.registerTargetInfo(@loc_register_crl); - -function this = loc_register_crl - -this(1) = RTW.TflRegistry; -this(1).Name = 'CRL for LUT-1D'; -this(1).TableList = {'cel_LUT'}; -this(1).BaseTfl = ''; -this(1).TargetHWDeviceType = {'*'}; -this(1).Description = 'Example Fx code replacement library'; diff --git a/solver/stdout_temp b/solver/stdout_temp deleted file mode 100644 index e2ccd07..0000000 --- a/solver/stdout_temp +++ /dev/null @@ -1,4 +0,0 @@ - -OPTIMAL (within EQTOL=1.0e-06, INEQTOL=1.0e-06, STATTOL=1.0e-05, COMPTOL=1.0e-06). -Solve time: 11.667 ms (71 iterations) - diff --git a/src/longitudinal.cpp b/src/longitudinal.cpp deleted file mode 100644 index 3371c64..0000000 --- a/src/longitudinal.cpp +++ /dev/null @@ -1,195 +0,0 @@ -#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/main.cpp b/src/main.cpp index 06d9af4..9a234e2 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -56,6 +56,7 @@ int main(int argc, char **argv) { ros::Subscriber subState = nh.subscribe(params.mpc.topics.state, 1, &MPC::stateCallback, &mpc); ros::Subscriber subPlanner = nh.subscribe(params.mpc.topics.planner, 1, &MPC::plannerCallback, &mpc); ros::Subscriber subTro = nh.subscribe(params.mpc.topics.tro, 1, &MPC::troCallback, &mpc); + ros::Subscriber subVel = nh.subscribe(params.mpc.topics.velocities, 1, &MPC::velsCallback, &mpc); pubCommands = nh.advertise(params.mpc.topics.commands, 1); // DEBUG diff --git a/src/mpc.cpp b/src/mpc.cpp index be3e05f..a2fd3cd 100644 --- a/src/mpc.cpp +++ b/src/mpc.cpp @@ -125,13 +125,23 @@ void MPC::troCallback(const as_msgs::ObjectiveArrayCurv::ConstPtr& msg){ } +void MPC::velsCallback(const as_msgs::CarVelocityArray::ConstPtr& msg){ + + pred_velocities.resize(msg->velocities.size()); + for(int i=0; ivelocities.size(); i++){ + pred_velocities(i) = msg->velocities[i].x; + } + this->velsFlag = true; + +} + /////////////////////////////////////////////////////////////////////////////////////////////// //------------------------Principal functions-------------------------------------------------- void MPC::solve(){ - if(paramFlag && dynParamFlag && plannerFlag && stateFlag){ + if(paramFlag && dynParamFlag && plannerFlag && stateFlag && velsFlag){ auto start_time = chrono::system_clock::now(); @@ -287,7 +297,6 @@ void MPC::set_params_bounds(){ this->forces.params.all_parameters[21 + k*this->Npar] = this->rk4_t; this->forces.params.all_parameters[22 + k*this->Npar] = this->q_slack_track; - this->forces.params.all_parameters[23 + k*this->Npar] = carState(3); // CHANGE!! --> vx prediction int plannerIdx = 0; if(firstIter){ @@ -328,8 +337,10 @@ void MPC::set_params_bounds(){ progress(k) = planner(plannerIdx, 2); + this->forces.params.all_parameters[23 + k*this->Npar] = pred_velocities(plannerIdx); this->forces.params.all_parameters[24 + k*this->Npar] = planner(plannerIdx, 3); // curvature // cout << "Curvature: " << forces.params.all_parameters[24+k*Npar] << endl; + cout << "pred velocity: " << pred_velocities(plannerIdx) << 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 @@ -514,7 +525,7 @@ void MPC::get_solution(){ void MPC::msgCommands(as_msgs::CarCommands *msg){ msg->header.stamp = ros::Time::now(); - msg->motor = 0.2; + msg->motor = 0.0; msg->steering = solCommands(this->latency, 3); msg->Mtv = solCommands(this->latency, 2); diff --git a/src/pid.cpp b/src/pid.cpp deleted file mode 100644 index 77b718f..0000000 --- a/src/pid.cpp +++ /dev/null @@ -1,80 +0,0 @@ -#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 diff --git a/src/utils/params.cpp b/src/utils/params.cpp index 5a9d202..b166b8a 100644 --- a/src/utils/params.cpp +++ b/src/utils/params.cpp @@ -20,6 +20,7 @@ Params::Params(const ros::NodeHandle* nh) { nh->param("Topics/Commands", mpc.topics.commands, "/AS/C/commands"); nh->param("Topics/Planner", mpc.topics.planner, "/AS/C/trajectory/partial"); nh->param("Topics/Tro", mpc.topics.tro, "/AS/C/trajectory/full"); + nh->param("Topics/Velocities", mpc.topics.velocities, "/AS/C/pid/velocities"); // Visualization topics nh->param("Topics/Vis/PredictedSteering", mpc.topics.predictedSteering, "/AS/C/mpc/vis/predicted/steering"); nh->param("Topics/Vis/PredictedPath", mpc.topics.predictedPath, "/AS/C/mpc/vis/predicted/path"); @@ -31,10 +32,10 @@ Params::Params(const ros::NodeHandle* nh) { nh->param("NLOP/Nslacks", mpc.nlop.Nslacks, 2); // MPC period (1/freq) - nh->param(("rk4_t"), mpc.rk4_t, 0.025); - nh->param(("nPlanning"), mpc.nPlanning, 1900); - nh->param(("Hz"), mpc.Hz, 20); - nh->param(("Nthreads"), mpc.Nthreads, 2); - nh->param(("TroProfile"), mpc.TroProfile, false); + nh->param(("MPC/rk4_t"), mpc.rk4_t, 0.025); + nh->param(("MPC/nPlanning"), mpc.nPlanning, 1900); + nh->param(("MPC/Hz"), mpc.Hz, 20); + nh->param(("MPC/Nthreads"), mpc.Nthreads, 2); + nh->param(("MPC/TroProfile"), mpc.TroProfile, false); } \ No newline at end of file