track updates

QUT-Motorsport · May 7, 2024 · d8cd42c · d8cd42c
1 parent a4775a0
commit d8cd42c
Show file tree

Hide file tree

Showing 7 changed files with 7,510 additions and 4,222 deletions.
diff --git a/src/control/pure_pursuit_cpp/config/pure_pursuit_cpp.yaml b/src/control/pure_pursuit_cpp/config/pure_pursuit_cpp.yaml
@@ -1,9 +1,9 @@
 pure_pursuit_cpp_node:
   ros__parameters:
-    Kp_ang: -3.5
+    Kp_ang: -4
     lookahead: 2.5
-    vel_max: 4.0
-    vel_min: 3.0
+    vel_max: 6.0
+    vel_min: 4.0
     discovery_lookahead: 3.0
-    discovery_vel_max: 4.0
-    discovery_vel_min: 3.0
+    discovery_vel_max: 6.0
+    discovery_vel_min: 4.0
diff --git a/src/navigation/map_creation/include/map_creation/node_cone_placement.hpp b/src/navigation/map_creation/include/map_creation/node_cone_placement.hpp
@@ -84,11 +84,13 @@ struct TrackedCone {
                 colour = driverless_msgs::msg::Cone::YELLOW;
             } else if (blue_count > yellow_count && blue_count > orange_count) {
                 colour = driverless_msgs::msg::Cone::BLUE;
+            } else if (orange_count > yellow_count && orange_count > blue_count) {
+                colour = driverless_msgs::msg::Cone::ORANGE_SMALL;
             }
 
-            if (orange_count > 10 || orange_count > yellow_count || orange_count > blue_count) {
-                colour = driverless_msgs::msg::Cone::ORANGE_BIG;
-            }
+            // if (orange_count > 10 || orange_count > yellow_count || orange_count > blue_count) {
+            //     colour = driverless_msgs::msg::Cone::ORANGE_BIG;
+            // }
         }
 
         // transform map to car

diff --git a/src/navigation/map_creation/src/node_cone_placement.cpp b/src/navigation/map_creation/src/node_cone_placement.cpp
@@ -37,7 +37,8 @@ ConeAssociation::ConeAssociation() : Node("cone_placement_node") {
 
 void ConeAssociation::state_callback(const driverless_msgs::msg::State::SharedPtr msg) {
     // we haven't started driving yet
-    if (msg->state == driverless_msgs::msg::State::DRIVING && msg->lap_count == 0) {
+    // if (msg->state == driverless_msgs::msg::State::DRIVING && msg->lap_count == 0) {
+    if (msg->lap_count == 0) {
         mapping = true;
     }
 
@@ -50,6 +51,7 @@ void ConeAssociation::state_callback(const driverless_msgs::msg::State::SharedPt
 
 void ConeAssociation::callback(const driverless_msgs::msg::ConeDetectionStamped::SharedPtr msg) {
     if (!mapping) {
+        RCLCPP_INFO_ONCE(get_logger(), "Not ready to map");
         return;
     }
 

diff --git a/src/navigation/nav_bringup/config/localisation_params.yaml b/src/navigation/nav_bringup/config/localisation_params.yaml
@@ -22,12 +22,12 @@ odom_filter_node:
                    false, false, false,  # roll_dot, pitch_dot, yaw_dot
                    false, false, false]  # x_ddot, y_ddot, z_ddot
 
-    twist0: vehicle/wheel_twist
-    twist0_config: [false, false, false,  # x, y, z
-                   false, false, false,  # roll, pitch, yaw
-                   true, false, false,  # x_dot, y_dot, z_dot
-                   false, false, false,  # roll_dot, pitch_dot, yaw_dot
-                   false, false, false]  # x_ddot, y_ddot, z_ddot
+    # twist0: vehicle/wheel_twist
+    # twist0_config: [false, false, false,  # x, y, z
+    #                false, false, false,  # roll, pitch, yaw
+    #                true, false, false,  # x_dot, y_dot, z_dot
+    #                false, false, false,  # roll_dot, pitch_dot, yaw_dot
+    #                false, false, false]  # x_ddot, y_ddot, z_ddot
 
     odom1: odometry/sbg_ekf
     odom1_config: [true, true, false,  # x, y, z

diff --git a/src/navigation/planners/planners/node_ft_planner.py b/src/navigation/planners/planners/node_ft_planner.py
@@ -216,8 +216,8 @@ def get_planner_cfg(self):
         self.declare_parameter("max_dist", 6.5)
         self.declare_parameter("max_dist_to_first", 6.0)
         self.declare_parameter("max_length", 12)
-        self.declare_parameter("threshold_directional_angle", 40)
-        self.declare_parameter("threshold_absolute_angle", 65)
+        self.declare_parameter("threshold_directional_angle", 30)
+        self.declare_parameter("threshold_absolute_angle", 55)
         self.declare_parameter("use_unknown_cones", True)
 
         cone_sorting_kwargs = {
@@ -295,6 +295,22 @@ def planning_callback(self):
             return
 
         # skip if we haven't completed a lap yet
+        if not self.initialised:
+            (
+                path,
+                ordered_blues,
+                ordered_yellows,
+                virt_blues,
+                virt_yellows,
+                _,
+                _,
+            ) = self.path_planner.calculate_path_in_global_frame(
+                pre_track, np.array([0.0,0.0]), 0.0, return_intermediate_results=True
+            )
+            self.initialised = True
+            self.get_logger().info("Initialised planner calcs")
+            return
+
         if self.current_track is None or len(self.current_track.cones) == 0:
             self.get_logger().warn("No track data received", throttle_duration_sec=1)
             return
@@ -338,17 +354,21 @@ def planning_callback(self):
 
         global_cones = [unknown_cones, yellow_cones, blue_cones, orange_small_cones, orange_big_cones]
 
-        (
-            path,
-            ordered_blues,
-            ordered_yellows,
-            virt_blues,
-            virt_yellows,
-            _,
-            _,
-        ) = self.path_planner.calculate_path_in_global_frame(
-            global_cones, car_position, car_direction, return_intermediate_results=True
-        )
+        try:
+            (
+                path,
+                ordered_blues,
+                ordered_yellows,
+                virt_blues,
+                virt_yellows,
+                _,
+                _,
+            ) = self.path_planner.calculate_path_in_global_frame(
+                global_cones, car_position, car_direction, return_intermediate_results=True
+            )
+        except Exception as e:
+            self.get_logger().warn("Cant plan, error" + str(e), throttle_duration_sec=1)
+            return
 
         use_virt = True
         if use_virt: