mpc consistent

utiasASRL · Oct 31, 2024 · bb038a9 · bb038a9
1 parent 2ce04fd
commit bb038a9
Show file tree

Hide file tree

Showing 7 changed files with 4,825 additions and 4,826 deletions.
diff --git a/config/ouster_warthog_default.yaml b/config/ouster_warthog_default.yaml
@@ -56,9 +56,9 @@
       #- lidar.obstacle_detection
       #- lidar.terrain_assessment
 
-    robot_frame: base_link
+    robot_frame: w200_0066_base_link
     env_info_topic: env_info
-    lidar_frame: os_lidar
+    lidar_frame: w200_0066_os_lidar
     lidar_topic: /ouster/points
     queue_size: 1
     graph_map:

diff --git a/launch/online_navtech_warthog.launch.yaml b/launch/online_navtech_warthog.launch.yaml
@@ -4,6 +4,7 @@ session_name: vtr_online_navtech_warthog
 environment:
   # CYCLONEDDS_URI: ${VTRSRC}/config/cyclonedds.default.xml # custom dds configs
   ROS_DOMAIN_ID: "7"  # set this to a unique number when multiple ROS2 dependent system running on the same network
+  ROS_DISCOVERY_SERVER: "192.168.131.1:11811;192.168.131.100:11811;127.0.0.1:11811"
 
 start_directory: ${VTRTEMP}
 suppress_history: false

diff --git a/launch/online_ouster_warthog.launch.yaml b/launch/online_ouster_warthog.launch.yaml
@@ -2,7 +2,8 @@
 session_name: vtr_online_ouster_warthog
 
 environment:
-  ROS_DOMAIN_ID: "131"  # set this to a unique number when multiple ROS2 dependent system running on the same network
+  ROS_DOMAIN_ID: "7"  # set this to a unique number when multiple ROS2 dependent system running on the same network
+  ROS_DISCOVERY_SERVER: "192.168.131.1:11811;192.168.131.100:11811"
 
 start_directory: ${VTRTEMP}
 suppress_history: false

diff --git a/main/src/vtr_path_planning/include/vtr_path_planning/mpc/mpc_path_planner.hpp b/main/src/vtr_path_planning/include/vtr_path_planning/mpc/mpc_path_planner.hpp
@@ -32,7 +32,7 @@ class CasadiUnicycleMPC {
     // TODO add an automatic way to keep the code in sync
     static constexpr int nStates = 3;
     static constexpr int nControl = 2;
-    static constexpr double alpha = 0.2; // lidar default 0.4
+    static constexpr double alpha = 0.4; // lidar default 0.4
     static constexpr int N = 15;
     static constexpr double DT = 0.25;
     DM previous_vel{nControl, 1};

diff --git a/main/src/vtr_path_planning/include/vtr_path_planning/mpc/mpc_solver.hpp b/main/src/vtr_path_planning/include/vtr_path_planning/mpc/mpc_solver.hpp
@@ -159,4 +159,4 @@ extern "C" int nlp_jac_g_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casa
 #define nlp_jac_g_SZ_ARG 2
 #define nlp_jac_g_SZ_RES 2
 #define nlp_jac_g_SZ_IW 0
-#define nlp_jac_g_SZ_W 370
+#define nlp_jac_g_SZ_W 369
diff --git a/main/src/vtr_path_planning/scripts/unicycle_solver.py b/main/src/vtr_path_planning/scripts/unicycle_solver.py
@@ -22,7 +22,7 @@
 N = 15           # number of look ahead steps
 
 # The first order lag weighting for the angular velocity
-alpha = 0.2
+alpha = 0.4
 
 # state symbolic variables
 x = ca.SX.sym('x')