fixed reverse routes and U turns, route is always in the heading and doesnt allow U turns

include/adore_map/road_graph.hpp

@@ -29,6 +29,8 @@ namespace map
 
 using LaneID = size_t;
 
+using Tangent = std::pair<double, double>;
+
 enum ConnectionType
 {
   END_TO_START,
@@ -91,6 +93,35 @@ struct ConnectionHasher
   }
 };
 
+struct DirectedLane
+{
+  LaneID lane_id;
+  bool   reverse; // true = traversing this lane from end -> start (decreasing s)
+
+  bool
+  operator==( const DirectedLane& other ) const
+  {
+    return lane_id == other.lane_id && reverse == other.reverse;
+  }
+
+  bool
+  operator<( const DirectedLane& other ) const
+  {
+    if( lane_id != other.lane_id )
+      return lane_id < other.lane_id;
+    return reverse < other.reverse;
+  }
+};
+
+struct DirectedLaneHasher
+{
+  std::size_t
+  operator()( const DirectedLane& dl ) const
+  {
+    return std::hash<LaneID>()( dl.lane_id ) ^ ( std::hash<bool>()( dl.reverse ) << 1 );
+  }
+};
+
 struct RoadGraph
 {
   RoadGraph() {};
@@ -106,6 +137,14 @@ struct RoadGraph
 
   std::deque<LaneID> find_path( LaneID from, LaneID to, bool allow_reverse ) const;
 
+  std::deque<DirectedLane> find_path( LaneID from, LaneID to, bool start_reverse,
+                                      const std::function<std::optional<Tangent>( LaneID, bool )>& get_tangent,
+                                      double                                                       max_uturn_cos ) const;
+
+  std::deque<DirectedLane> get_best_path( LaneID from, LaneID to, bool start_reverse,
+                                          const std::function<std::optional<Tangent>( LaneID, bool )>& get_tangent,
+                                          double                                                       max_uturn_cos = -0.7 ) const;
+
   // Helper function to reconstruct the path from `from` to `to`
   std::deque<LaneID> reconstruct_path( LaneID from, LaneID to, const std::unordered_map<LaneID, LaneID>& previous_roads ) const;
 

include/adore_map/route.hpp

@@ -85,27 +85,106 @@ Route::Route( const StartPoint& start_point, const EndPoint& end, const std::sha
   destination.y = end.y;
   map           = reference_map;
 
-  // Find nearest start and end points using the quadtree
   double min_start_dist      = std::numeric_limits<double>::max();
   auto   nearest_start_point = map->quadtree.get_nearest_point( start, min_start_dist );
 
   double min_end_dist      = std::numeric_limits<double>::max();
   auto   nearest_end_point = map->quadtree.get_nearest_point( end, min_end_dist );
 
-
   if( nearest_start_point && nearest_end_point )
   {
     size_t start_lane_id = nearest_start_point->parent_id;
     size_t end_lane_id   = nearest_end_point->parent_id;
 
-    // Find the best path between the start and end lanes
-    auto lane_id_route = map->lane_graph.get_best_path( start_lane_id, end_lane_id );
+    // 1) Determine which direction along the start lane matches the
+    //    vehicle's current heading. This becomes the mandatory starting
+    //    direction for the graph search below.
+    bool heading_wants_reverse = false;
+    {
+      auto        start_lane = map->lanes.at( start_lane_id );
+      const auto& pts        = start_lane->borders.center.interpolated_points;
+
+      if( pts.size() >= 2 )
+      {
+        auto it = std::lower_bound( pts.begin(), pts.end(), nearest_start_point->s,
+                                     []( const auto& pt, double val ) { return pt.s < val; } );
+        size_t idx = static_cast<size_t>( std::distance( pts.begin(), it ) );
+        if( idx == 0 )
+          idx = 1;
+        if( idx >= pts.size() )
+          idx = pts.size() - 1;
+
+        const auto& p1  = pts[idx - 1];
+        const auto& p2  = pts[idx];
+        double      dx  = p2.x - p1.x;
+        double      dy  = p2.y - p1.y;
+        double      len = std::sqrt( dx * dx + dy * dy );
+
+        if( len > 1e-9 )
+        {
+          double tangent_x = dx / len;
+          double tangent_y = dy / len;
+          double heading_x = std::cos( start_point.yaw_angle );
+          double heading_y = std::sin( start_point.yaw_angle );
+          double alignment = heading_x * tangent_x + heading_y * tangent_y;
+
+          heading_wants_reverse = ( alignment < 0.0 );
+        }
+      }
+    }
+
+    // 2) Tangent lookup used by the graph search to detect U-turns at
+    //    lane junctions (needed because ConnectionType alone doesn't
+    //    capture actual junction geometry/angle).
+    std::shared_ptr<Map> map_for_lambda = map; // local copy: avoids capturing the member 'map' via 'this'
+
+    auto get_tangent = [map_for_lambda]( LaneID id, bool at_end ) -> std::optional<std::pair<double, double>>
+    {
+      auto it = map_for_lambda->lanes.find( id );
+      if( it == map_for_lambda->lanes.end() )
+        return std::nullopt;
+
+      const auto& pts = it->second->borders.center.interpolated_points;
+      if( pts.size() < 2 )
+        return std::nullopt;
+
+      size_t i0 = at_end ? pts.size() - 2 : 0;
+      size_t i1 = at_end ? pts.size() - 1 : 1;
+
+      double dx  = pts[i1].x - pts[i0].x;
+      double dy  = pts[i1].y - pts[i0].y;
+      double len = std::sqrt( dx * dx + dy * dy );
+      if( len < 1e-9 )
+        return std::nullopt;
+
+      return std::make_pair( dx / len, dy / len );
+    };
+
+    // 3) Search for a U-turn-free path starting in the vehicle's heading
+    //    direction. If none exists (e.g. destination is only reachable
+    //    via a genuine reversal, such as a cul-de-sac), fall back to an
+    //    unconstrained search rather than leaving the vehicle without a
+    //    route at all.
+    constexpr double kMaxUTurnCos = -0.7; // reject turns sharper than ~135 degrees
+
+    auto directed_route = map->lane_graph.get_best_path( start_lane_id, end_lane_id, heading_wants_reverse,
+                                                           get_tangent, kMaxUTurnCos );
+
+    if( directed_route.empty() )
+    {
+      std::cerr << "Route: no U-turn-free path found, retrying without U-turn constraint" << std::endl;
+      directed_route = map->lane_graph.get_best_path( start_lane_id, end_lane_id, heading_wants_reverse, get_tangent,
+                                                        -1.0 ); // only forbid exact 180s
+    }
 
-    // Iterate over the route and process each lane
-    for( size_t i = 0; i < lane_id_route.size(); ++i )
+    // 4) Build sections directly from the directed path -- each entry
+    //    already carries the correct traversal direction as determined
+    //    by the search, so no separate left_of_reference/override logic
+    //    is needed here anymore.
+    for( const auto& directed_lane : directed_route )
     {
-      auto lane = map->lanes.at( lane_id_route[i] );
-      add_route_section( lane->borders.center, *nearest_start_point, *nearest_end_point, lane->left_of_reference );
+      auto lane = map->lanes.at( directed_lane.lane_id );
+      add_route_section( lane->borders.center, *nearest_start_point, *nearest_end_point, directed_lane.reverse );
     }
 
     initialize_reference_line();

src/road_graph.cpp

@@ -96,6 +96,138 @@ RoadGraph::get_best_path( LaneID from, LaneID to ) const
   return find_path( from, to, false );
 }
 
+std::deque<DirectedLane>
+RoadGraph::find_path( LaneID from, LaneID to, bool start_reverse, const std::function<std::optional<Tangent>( LaneID, bool )>& get_tangent,
+                      double max_uturn_cos ) const
+{
+  using QueueEntry = std::pair<double, DirectedLane>;
+  std::priority_queue<QueueEntry, std::vector<QueueEntry>, std::greater<>> pq;
+
+  std::unordered_map<DirectedLane, double, DirectedLaneHasher>       shortest_paths;
+  std::unordered_map<DirectedLane, DirectedLane, DirectedLaneHasher> previous_roads;
+  std::unordered_set<DirectedLane, DirectedLaneHasher>               visited;
+
+  DirectedLane start_state{ from, start_reverse };
+  pq.push( { 0.0, start_state } );
+  shortest_paths[start_state] = 0.0;
+
+  DirectedLane goal_state{};
+  bool         goal_found = false;
+
+  while( !pq.empty() )
+  {
+    auto [current_cost, current] = pq.top();
+    pq.pop();
+
+    if( visited.count( current ) )
+      continue;
+    visited.insert( current );
+
+    if( current.lane_id == to )
+    {
+      goal_state = current;
+      goal_found = true;
+      break;
+    }
+
+    if( to_successors.count( current.lane_id ) == 0 )
+      continue;
+
+    for( const auto& neighbor : to_successors.at( current.lane_id ) )
+    {
+      auto conn = find_connection( current.lane_id, neighbor );
+      if( !conn )
+        continue;
+
+      bool exit_is_start = ( conn->connection_type == START_TO_START || conn->connection_type == START_TO_END );
+
+      // For non-PARALLEL connections, from_id (current.lane_id) can only be
+      // exited at the end implied by connection_type. If our current
+      // directed state exits at the other end, this edge isn't usable here.
+      if( conn->connection_type != PARALLEL && current.reverse != exit_is_start )
+        continue;
+
+      bool to_reverse;
+      if( conn->connection_type == PARALLEL )
+      {
+        // Lateral lane change: doesn't flip direction of travel.
+        to_reverse = current.reverse;
+      }
+      else
+      {
+        bool entry_is_end = ( conn->connection_type == END_TO_END || conn->connection_type == START_TO_END );
+        to_reverse        = entry_is_end;
+      }
+
+      // Geometric U-turn check using actual tangents at the junction.
+      if( get_tangent )
+      {
+        bool from_at_end = exit_is_start ? false : true;
+        bool to_at_end;
+        if( conn->connection_type == PARALLEL )
+          to_at_end = from_at_end; // adjacent lane change, mirror the exit side
+        else
+          to_at_end = ( conn->connection_type == END_TO_END || conn->connection_type == START_TO_END );
+
+        auto from_tangent_raw = get_tangent( current.lane_id, from_at_end );
+        auto to_tangent_raw   = get_tangent( neighbor, to_at_end );
+
+        if( from_tangent_raw && to_tangent_raw )
+        {
+          double fsign = current.reverse ? -1.0 : 1.0;
+          double tsign = to_reverse ? -1.0 : 1.0;
+
+          double fx = from_tangent_raw->first * fsign;
+          double fy = from_tangent_raw->second * fsign;
+          double tx = to_tangent_raw->first * tsign;
+          double ty = to_tangent_raw->second * tsign;
+
+          double dot = fx * tx + fy * ty; // both are unit vectors, dot = cos(angle)
+
+          if( dot < max_uturn_cos )
+            continue; // reject: this transition reverses heading too sharply
+        }
+      }
+
+      DirectedLane neighbor_state{ neighbor, to_reverse };
+      double       new_cost = current_cost + conn->weight;
+
+      if( shortest_paths.find( neighbor_state ) == shortest_paths.end() || new_cost < shortest_paths[neighbor_state] )
+      {
+        shortest_paths[neighbor_state] = new_cost;
+        previous_roads[neighbor_state] = current;
+        pq.push( { new_cost, neighbor_state } );
+      }
+    }
+  }
+
+  if( !goal_found )
+  {
+    std::cerr << "failed to find u-turn-constrained route from " << from << " to " << to << std::endl;
+    return {};
+  }
+
+  std::deque<DirectedLane> path;
+  DirectedLane             current = goal_state;
+  while( !( current.lane_id == from && current.reverse == start_reverse ) )
+  {
+    path.push_front( current );
+    current = previous_roads.at( current );
+  }
+  path.push_front( DirectedLane{ from, start_reverse } );
+
+  return path;
+}
+
+// --- get_best_path (5-arg overload) ---
+
+std::deque<DirectedLane>
+RoadGraph::get_best_path( LaneID from, LaneID to, bool start_reverse,
+                          const std::function<std::optional<Tangent>( LaneID, bool )>& get_tangent, double max_uturn_cos ) const
+{
+  return find_path( from, to, start_reverse, get_tangent, max_uturn_cos );
+}
+
 std::deque<LaneID>
 RoadGraph::reconstruct_path( LaneID from, LaneID to, const std::unordered_map<LaneID, LaneID>& previous_roads ) const
 {