first functional version with pid

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})

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();

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;

package.xml

@@ -4,7 +4,7 @@
   <version>0.0.0</version>
   <description>The tailored_mpc package</description>
 
-  <maintainer email="[email protected]">fetty</maintainer>
+  <maintainer email="[email protected]">fetty</maintainer>
 
   <license>TODO</license>
 

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,35 +22,35 @@ 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: []
       parent: 0
       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

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