Merge pull request #426 from Nova-UTD/chore/obc-changes-1

[Vehicle] Pure Pursuit and Map Manager Changes

- New routes
- Pure pursuit adjustments
- Map manager logic when no nearby lane geometry detected

src/control/pure_pursuit_controller/pure_pursuit_controller/pure_pursuit_controller_node.py

@@ -1,3 +1,9 @@
+"""
+ROS2 Node for the Pure Pursuit Controller. 
+
+NOT WORKING as of 04/30/2024, mainly due to faulty path planners (RTP, Nav2, attempts at Graph/A* path planning).
+"""
+
 import math
 
 import rclpy
@@ -32,6 +38,8 @@ class Constants:
     STOPPING_DIST: float = 10
     # Max break possible
     MAX_BREAK: float = 1.0
+    # Max steering angle in radians.
+    MAX_STEERING_ANGLE = 0.58
 
 
 class VehicleState:
@@ -82,12 +90,30 @@ def calc_steer(self, target: tuple[float, float]) -> float | None:
             return
 
         alpha = math.atan2(target[1], target[0])
-        delta = math.atan2(
-            2.0 * Constants.WHEEL_BASE * math.sin(alpha), self.lookahead_distance()
-        )
 
-        # Normalize the steering angle from [-pi, pi] to [-1, 1]
-        return -delta / (math.pi / 2)
+        # delta = (
+        #     2.0 * Constants.WHEEL_BASE * math.sin(alpha) / math.hypot(target[0], target[1])
+        # )
+
+        # Alpha works with RPT node to a limited extent -- only in parking lot, 
+
+        # Originally, delta was used (and working in Carla when following smooth route),
+        # but simply did not work in real world testing.
+        # Delta is derived following this article: https://thomasfermi.github.io/Algorithms-for-Automated-Driving/Control/PurePursuit.html.
+
+        # We clip and normalize alpha to a max steering angle, which we believe is ~0.58 as specified by the Unified Controller.
+        clipped_steering_angle = self.steering_angle_to_wheel(-alpha)
+        normalized = clipped_steering_angle / Constants.MAX_STEERING_ANGLE
+
+        return normalized
+
+
+    def steering_angle_to_wheel(self, angle: float) -> float:
+        """
+        Converts a steering angle to a steering position
+        """
+        return float(min(max(angle, -Constants.MAX_STEERING_ANGLE), Constants.MAX_STEERING_ANGLE))
+
 
     def lookahead_distance(self) -> float:
         """Calculate the lookahead distance based on the current velocity.
@@ -108,7 +134,6 @@ class PursuitPath:
     def __init__(self, path: list[tuple[float, float]] = []):
         self.path: npt.NDArray[np.float64] = np.asarray(path)
         self.target_index: int | None = None
-
     def distance(self) -> float:
         if len(self.path) == 0:
             return 0.0
@@ -208,7 +233,7 @@ def __init__(self):
         )
         self.lookahead_path_publisher = self.create_publisher(Path, "/ppc/path", 1)
 
-        self.control_timer = self.create_timer(0.1, self.control_callback)
+        self.control_timer = self.create_timer(0.05, self.control_callback)
         self.visualize_path_timer = self.create_timer(0.1, self.visualize_path_callback)
         self.visualize_waypoint_timer = self.create_timer(
             0.1, self.visualize_waypoint_callback
@@ -245,31 +270,37 @@ def control_callback(self):
         """Calculate and publish the vehicle control commands based on the pure pursuit algorithm."""
         # Calculate the waypoint just outside the lookahead distance.
         self.target_waypoint = self.path.calc_target_point(self.vehicle_state)
-        self.get_logger().info(f"Target Waypoint: {self.target_waypoint}")
 
         # If no target waypoint, do nothing.
         if not self.target_waypoint:
             return
 
-        path_dist = self.path.distance()
-        if path_dist < Constants.STOPPING_DIST:
-            # Scale the break value linearly based on the distance to the end of the path.
-            # Break scale is 1.0 at the end of the path and 0.0 at the stopping distance.
-            break_scale = 1.0 - (path_dist / Constants.STOPPING_DIST)
-            break_value = min(
-                Constants.MAX_BREAK,
-                break_scale * Constants.MAX_BREAK,
-            )
-            steer = self.vehicle_state.calc_steer(self.target_waypoint)
-            self.stop_vehicle(steer, break_value)
-            return
+        # ------------------------
+        # Stopping distance and easing stop was not tested
+        # because base pure pursuit functinality was not working to begin with.
+        # Uncomment in future if/when pure pursuit integrates with rest of the stack.
+        #
+        #
+        # path_dist = self.path.distance()
+        # if path_dist < Constants.STOPPING_DIST:
+        #     # Scale the break value linearly based on the distance to the end of the path.
+        #     # Break scale is 1.0 at the end of the path and 0.0 at the stopping distance.
+        #     break_scale = 1.0 - (path_dist / Constants.STOPPING_DIST)
+        #     break_value = min(
+        #         Constants.MAX_BREAK,
+        #         break_scale * Constants.MAX_BREAK,
+        #     )
+        #     steer = self.vehicle_state.calc_steer(self.target_waypoint)
+        #     self.stop_vehicle(steer, break_value)
+        #     return
+        #
+        #
+        # ------------------------
 
         # Calculate throttle, brake, and steer
         throttle_brake = self.vehicle_state.calc_throttle_brake()
         steer = self.vehicle_state.calc_steer(self.target_waypoint)
 
-        self.get_logger().info(f"Steer: {steer} Throttle/Brake: {throttle_brake}")
-
         # If vehicle state is not loaded, do nothing.
         if throttle_brake is None or steer is None:
             return
@@ -280,9 +311,6 @@ def control_callback(self):
         control_msg.throttle = throttle
         control_msg.brake = brake
         control_msg.steer = steer
-        self.get_logger().info(
-            f"Steer: {control_msg.steer} Throttle: {throttle} Brake: {brake}"
-        )
         self.command_publisher.publish(control_msg)
 
     def visualize_waypoint_callback(self):

src/mapping/map_management/src/MapManager.cpp

@@ -606,15 +606,15 @@ void MapManagementNode::publishGrids(int top_dist, int bottom_dist, int side_dis
     std::vector<odr::value> lane_shapes_in_range;
     map_wide_tree_.query(bgi::intersects(search_region), std::back_inserter(lane_shapes_in_range));
 
-    if (lane_shapes_in_range.size() < 1)
+
+    bool has_nearby_lanes = lane_shapes_in_range.size() > 0:
+    // Warn if there are no nearby lanes.
+    // This may simply because we are testing in an area that is not on the campus.xodr map.
+    if (!has_nearby_lanes)
     {
-        // RCLCPP_WARN(get_logger(), "There are no lane shapes nearby.");
-        std::printf("(%f, %f)\n", vehicle_pos.x, vehicle_pos.y);
-        return;
+        RCLCPP_WARN(get_logger(), "There are no lane shapes nearby.");
     }
 
-    // std::printf("There are %i shapes in range.\n", lane_shapes_in_range.size());
-
     int idx = 0;
 
     // This was commented out...
@@ -725,7 +725,15 @@ void MapManagementNode::publishGrids(int top_dist, int bottom_dist, int side_dis
                 }
             }
 
-            drivable_grid_data.push_back(cell_is_drivable ? 0 : 100);
+            // If no lanes are are nearby, then the vehicle is assumed to be testing on a route
+            // area which is not on the campus.xodr map. 
+            // In this case, make the entire drivable grid 0 cost. 
+            if (!has_nearby_lanes) {
+                drivable_grid_data.push_back(0);
+            } else {
+                drivable_grid_data.push_back(cell_is_drivable ? 0 : 100);
+            }
+
             junction_grid_data.push_back(cell_is_in_junction ? 100 : 0);
 
             /*  Taking route distance out of the pipeline

src/planning/costs/costs/grid_summation_node.py

@@ -309,13 +309,11 @@ def createCostMap(self):
                 #    weighted_grid_arr = self.resizeOccupancyGrid(weighted_grid_arr)
 
                 if grid_name == 'drivable':
-                    pass
                     #steering_cost += weighted_grid_arr
-                    # steering_cost = np.maximum( steering_cost , weighted_grid_arr )
+                    steering_cost = np.maximum( steering_cost , weighted_grid_arr )
                 elif grid_name == 'junction':
-                    pass
                     # speed_cost += weighted_grid_arr
-                    # speed_cost = np.maximum( speed_cost , weighted_grid_arr )
+                    speed_cost = np.maximum( speed_cost , weighted_grid_arr )
                 else:
                     # steering_cost += weighted_grid_arr
                     # speed_cost += weighted_grid_arr