diff --git a/include/adore_map/road_graph.hpp b/include/adore_map/road_graph.hpp index 5972588..97c071e 100644 --- a/include/adore_map/road_graph.hpp +++ b/include/adore_map/road_graph.hpp @@ -29,6 +29,8 @@ namespace map using LaneID = size_t; +using Tangent = std::pair; + 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()( dl.lane_id ) ^ ( std::hash()( dl.reverse ) << 1 ); + } +}; + struct RoadGraph { RoadGraph() {}; @@ -106,6 +137,14 @@ struct RoadGraph std::deque find_path( LaneID from, LaneID to, bool allow_reverse ) const; + std::deque find_path( LaneID from, LaneID to, bool start_reverse, + const std::function( LaneID, bool )>& get_tangent, + double max_uturn_cos ) const; + + std::deque get_best_path( LaneID from, LaneID to, bool start_reverse, + const std::function( LaneID, bool )>& get_tangent, + double max_uturn_cos = -0.7 ) const; + // Helper function to reconstruct the path from `from` to `to` std::deque reconstruct_path( LaneID from, LaneID to, const std::unordered_map& previous_roads ) const; diff --git a/include/adore_map/route.hpp b/include/adore_map/route.hpp index b18c5bd..170f29f 100644 --- a/include/adore_map/route.hpp +++ b/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::max(); auto nearest_start_point = map->quadtree.get_nearest_point( start, min_start_dist ); double min_end_dist = std::numeric_limits::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( 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_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> + { + 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(); diff --git a/src/road_graph.cpp b/src/road_graph.cpp index c0b8e45..4e5c0d5 100644 --- a/src/road_graph.cpp +++ b/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 +RoadGraph::find_path( LaneID from, LaneID to, bool start_reverse, const std::function( LaneID, bool )>& get_tangent, + double max_uturn_cos ) const +{ + using QueueEntry = std::pair; + std::priority_queue, std::greater<>> pq; + + std::unordered_map shortest_paths; + std::unordered_map previous_roads; + std::unordered_set 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 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 +RoadGraph::get_best_path( LaneID from, LaneID to, bool start_reverse, + const std::function( LaneID, bool )>& get_tangent, double max_uturn_cos ) const +{ + return find_path( from, to, start_reverse, get_tangent, max_uturn_cos ); +} + std::deque RoadGraph::reconstruct_path( LaneID from, LaneID to, const std::unordered_map& previous_roads ) const {