feat(relative_map): routing-based local planning without HD maps

modules/world_model/relative_map/BUILD

@@ -46,6 +46,7 @@ cc_library(
         "//modules/common/util",
         "//modules/world_model/relative_map/common:relative_map_gflags",
         "//modules/common_msgs/planning_msgs:navigation_cc_proto",
+        "//modules/common_msgs/routing_msgs:routing_cc_proto",
         "//modules/world_model/relative_map/proto:relative_map_config_cc_proto",
         "//modules/common_msgs/perception_msgs:perception_obstacle_cc_proto",
     ],
@@ -81,6 +82,7 @@ cc_library(
         ":relative_map_lib",
         "//cyber",
         "//modules/common/adapters:adapter_gflags",
+        "//modules/common_msgs/routing_msgs:routing_cc_proto",
         "@com_github_gflags_gflags//:gflags",
     ],
 )

modules/world_model/relative_map/common/relative_map_gflags.cc

@@ -32,3 +32,7 @@ DEFINE_bool(enable_cyclic_rerouting, false,
 
 DEFINE_bool(relative_map_generate_left_boundray, true,
             "Generate left boundary for detected lanes.");
+
+DEFINE_double(destination_reached_threshold, 5.0,
+              "Distance threshold in meters within which the routing "
+              "destination is considered reached.");

modules/world_model/relative_map/common/relative_map_gflags.h

@@ -23,3 +23,4 @@ DECLARE_string(navigator_config_filename);
 DECLARE_int32(relative_map_loop_rate);
 DECLARE_bool(enable_cyclic_rerouting);
 DECLARE_bool(relative_map_generate_left_boundray);
+DECLARE_double(destination_reached_threshold);

modules/world_model/relative_map/navigation_lane.cc

@@ -184,6 +184,31 @@ bool NavigationLane::GeneratePath() {
         std::make_tuple(0, left_width, right_width, current_navi_path);
   };
 
+  // When a routing destination is set but no pre-recorded navigation lines are
+  // available, generate a path toward the destination using the bearing angle.
+  // Lane markers (if available) are used for lane-keeping (lateral offset).
+  auto generate_path_to_destination_func = [this, &lane_marker]() {
+    auto current_navi_path = std::make_shared<NavigationPath>();
+    auto *path = current_navi_path->mutable_path();
+    const auto &left_lane = lane_marker.left_lane_marker();
+    const auto &right_lane = lane_marker.right_lane_marker();
+    if (left_lane.view_range() > config_.min_view_range_to_use_lane_marker() ||
+        right_lane.view_range() > config_.min_view_range_to_use_lane_marker()) {
+      // Perception lane markers are available: use them to stay in lane.
+      ConvertLaneMarkerToPath(lane_marker, path);
+    } else {
+      // No lane markers: fall back to pure bearing toward the destination.
+      GenerateDestinationBearingPath(path);
+    }
+    current_navi_path->set_path_priority(0);
+    double left_width = perceived_left_width_ > 0.0 ? perceived_left_width_
+                                                    : default_left_width_;
+    double right_width = perceived_right_width_ > 0.0 ? perceived_right_width_
+                                                      : default_right_width_;
+    current_navi_path_tuple_ =
+        std::make_tuple(0, left_width, right_width, current_navi_path);
+  };
+
   ADEBUG << "Beginning of NavigationLane::GeneratePath().";
   ADEBUG << "navigation_line_num: " << navigation_line_num;
 
@@ -205,9 +230,14 @@ bool NavigationLane::GeneratePath() {
     }
 
     // If no navigation path is generated based on navigation lines, we generate
-    // one where the vehicle is located based on perceived lane markers.
+    // one where the vehicle is located based on perceived lane markers or
+    // toward the routing destination if one has been set.
     if (navigation_path_list_.empty()) {
-      generate_path_on_perception_func();
+      if (has_destination_) {
+        generate_path_to_destination_func();
+      } else {
+        generate_path_on_perception_func();
+      }
       return true;
     }
 
@@ -341,12 +371,73 @@ bool NavigationLane::GeneratePath() {
     return true;
   }
 
-  // Generate a navigation path where the vehicle is located based on perceived
-  // lane markers.
-  generate_path_on_perception_func();
+  // No navigation lines: fall back to destination-bearing path or perception.
+  if (has_destination_) {
+    generate_path_to_destination_func();
+  } else {
+    generate_path_on_perception_func();
+  }
   return true;
 }
 
+bool NavigationLane::IsDestinationReached(double threshold) const {
+  if (!has_destination_) {
+    return false;
+  }
+  const double dx =
+      destination_enu_x_ - original_pose_.position().x();
+  const double dy =
+      destination_enu_y_ - original_pose_.position().y();
+  return std::hypot(dx, dy) <= threshold;
+}
+
+void NavigationLane::GenerateDestinationBearingPath(common::Path *const path) {
+  CHECK_NOTNULL(path);
+  path->set_name("Path toward routing destination.");
+
+  const double dx = destination_enu_x_ - original_pose_.position().x();
+  const double dy = destination_enu_y_ - original_pose_.position().y();
+  const double dist_to_dest = std::hypot(dx, dy);
+
+  // If the vehicle is already at or past the destination, there is nothing to
+  // generate; the destination-reached check in RelativeMap will clear it.
+  if (dist_to_dest < 0.01) {
+    return;
+  }
+
+  // Convert the ENU direction vector to FLU by rotating by -heading.
+  // This is the same convention used in ConvertNavigationLineToPath.
+  auto flu_dir = Vec2d{dx, dy}.Rotate(-original_pose_.heading());
+
+  // Bearing angle in FLU and unit direction vector.
+  const double bearing_angle = std::atan2(flu_dir.y(), flu_dir.x());
+  const double flu_ux = flu_dir.x() / dist_to_dest;
+  const double flu_uy = flu_dir.y() / dist_to_dest;
+
+  const double path_range =
+      std::min(dist_to_dest, config_.max_len_for_navigation_lane());
+
+  // start_s = -2.0: begin the path 2 m behind the vehicle (matching the
+  // convention in ConvertLaneMarkerToPath) so that the planner always has a
+  // reference segment behind the current position.
+  const double start_s = -2.0;
+  // unit_z: longitudinal step size in meters (matches ConvertLaneMarkerToPath).
+  const double unit_z = 1.0;
+  const int num_points =
+      static_cast<int>((path_range - start_s) / unit_z) + 1;
+  for (int i = 0; i < num_points; ++i) {
+    const double s = start_s + i * unit_z;
+    auto *point = path->add_path_point();
+    point->set_x(flu_ux * s);
+    point->set_y(flu_uy * s);
+    // Accumulated arc length: straight line so each step is exactly unit_z.
+    point->set_s(start_s + i * unit_z);
+    point->set_theta(bearing_angle);
+    point->set_kappa(0.0);
+    point->set_dkappa(0.0);
+  }
+}
+
 double NavigationLane::EvaluateCubicPolynomial(const double c0, const double c1,
                                                const double c2, const double c3,
                                                const double x) const {

modules/world_model/relative_map/navigation_lane.h

@@ -136,6 +136,37 @@ class NavigationLane {
    */
   bool GeneratePath();
 
+  /**
+   * @brief Set the routing destination in ENU coordinates.
+   * @param enu_x East coordinate of the destination.
+   * @param enu_y North coordinate of the destination.
+   * @return None.
+   */
+  void SetDestination(double enu_x, double enu_y) {
+    destination_enu_x_ = enu_x;
+    destination_enu_y_ = enu_y;
+    has_destination_ = true;
+  }
+
+  /**
+   * @brief Clear the routing destination.
+   * @return None.
+   */
+  void ClearDestination() { has_destination_ = false; }
+
+  /**
+   * @brief Check whether the vehicle has reached the destination.
+   * @param threshold Distance threshold in meters.
+   * @return True if vehicle is within threshold of the destination.
+   */
+  bool IsDestinationReached(double threshold) const;
+
+  /**
+   * @brief Check whether a routing destination is set.
+   * @return True if a destination has been set.
+   */
+  bool HasDestination() const { return has_destination_; }
+
   /**
    * @brief Update perceived lane line information.
    * @param perception_obstacles Perceived lane line information to be updated.
@@ -265,6 +296,15 @@ class NavigationLane {
    */
   void UpdateStitchIndexInfo();
 
+  /**
+   * @brief Generate a straight path in FLU coordinates pointing toward the
+   * routing destination. Used when no pre-recorded navigation lines are
+   * available and a destination has been set via routing.
+   * @param path The output path in FLU coordinates.
+   * @return None.
+   */
+  void GenerateDestinationBearingPath(common::Path* path);
+
  private:
   // the configuration information required by the `NavigationLane`
   NavigationLaneConfig config_;
@@ -304,6 +344,11 @@ class NavigationLane {
   // in world coordination: ENU
   localization::Pose original_pose_;
   common::VehicleStateProvider* vehicle_state_provider_ = nullptr;
+
+  // routing destination in ENU coordinates
+  double destination_enu_x_ = 0.0;
+  double destination_enu_y_ = 0.0;
+  bool has_destination_ = false;
 };
 
 }  // namespace relative_map

modules/world_model/relative_map/relative_map.cc

@@ -32,6 +32,7 @@ using apollo::common::VehicleStateProvider;
 using apollo::common::monitor::MonitorMessageItem;
 using apollo::localization::LocalizationEstimate;
 using apollo::perception::PerceptionObstacles;
+using apollo::routing::RoutingRequest;
 
 RelativeMap::RelativeMap()
     : monitor_logger_buffer_(MonitorMessageItem::RELATIVE_MAP),
@@ -88,6 +89,27 @@ void RelativeMap::OnNavigationInfo(const NavigationInfo& navigation_info) {
   }
 }
 
+void RelativeMap::OnRoutingRequest(const RoutingRequest& routing_request) {
+  if (routing_request.waypoint_size() < 1) {
+    AWARN << "RoutingRequest has no waypoints, ignoring.";
+    return;
+  }
+  // The destination is the last waypoint in the routing request.
+  const auto& dest = routing_request.waypoint(routing_request.waypoint_size() - 1);
+  if (!dest.has_pose()) {
+    AWARN << "Destination waypoint has no pose, ignoring RoutingRequest.";
+    return;
+  }
+  const double dest_x = dest.pose().x();
+  const double dest_y = dest.pose().y();
+  AINFO << "RelativeMap received routing destination: (" << dest_x << ", "
+        << dest_y << ")";
+  {
+    std::lock_guard<std::mutex> lock(navigation_lane_mutex_);
+    navigation_lane_.SetDestination(dest_x, dest_y);
+  }
+}
+
 void RelativeMap::OnPerception(
     const PerceptionObstacles& perception_obstacles) {
   {
@@ -120,6 +142,15 @@ bool RelativeMap::CreateMapFromNavigationLane(MapMsg* map_msg) {
   vehicle_state_provider_->Update(localization, chassis);
   map_msg->mutable_localization()->CopyFrom(localization_);
 
+  // Check whether the routing destination has been reached.
+  if (navigation_lane_.HasDestination() &&
+      navigation_lane_.IsDestinationReached(
+          FLAGS_destination_reached_threshold)) {
+    AINFO << "RelativeMap: routing destination reached. Clearing destination.";
+    monitor_logger_buffer_.INFO("RelativeMap: routing destination reached.");
+    navigation_lane_.ClearDestination();
+  }
+
   // update navigation_lane from perception_obstacles (lane marker)
   PerceptionObstacles const& perception = perception_obstacles_;
   navigation_lane_.UpdatePerception(perception);

modules/world_model/relative_map/relative_map.h

@@ -20,6 +20,7 @@
 
 #include "modules/common_msgs/perception_msgs/perception_obstacle.pb.h"
 #include "modules/common_msgs/planning_msgs/navigation.pb.h"
+#include "modules/common_msgs/routing_msgs/routing.pb.h"
 #include "modules/world_model/relative_map/proto/relative_map_config.pb.h"
 
 #include "modules/common/monitor_log/monitor_log_buffer.h"
@@ -73,6 +74,7 @@ class RelativeMap {
   void OnChassis(const canbus::Chassis& chassis);
   void OnLocalization(const localization::LocalizationEstimate& localization);
   void OnNavigationInfo(const NavigationInfo& navigation_info);
+  void OnRoutingRequest(const routing::RoutingRequest& routing_request);
 
  private:
   bool CreateMapFromNavigationLane(MapMsg* map_msg);

modules/world_model/relative_map/relative_map_component.cc

@@ -24,6 +24,7 @@ namespace relative_map {
 using apollo::canbus::Chassis;
 using apollo::localization::LocalizationEstimate;
 using apollo::perception::PerceptionObstacles;
+using apollo::routing::RoutingRequest;
 
 bool RelativeMapComponent::Init() {
   vehicle_state_provider_ = std::make_shared<common::VehicleStateProvider>();
@@ -70,6 +71,13 @@ bool RelativeMapComponent::InitReaders() {
         relative_map_.OnNavigationInfo(*navigation_info.get());
       });
 
+  routing_request_reader_ = node_->CreateReader<RoutingRequest>(
+      FLAGS_routing_request_topic,
+      [this](const std::shared_ptr<RoutingRequest>& routing_request) {
+        ADEBUG << "Received routing request: updating destination.";
+        relative_map_.OnRoutingRequest(*routing_request.get());
+      });
+
   relative_map_writer_ = node_->CreateWriter<MapMsg>(FLAGS_relative_map_topic);
   return true;
 }

modules/world_model/relative_map/relative_map_component.h

@@ -21,6 +21,7 @@
 #include "cyber/component/timer_component.h"
 #include "cyber/cyber.h"
 #include "modules/common/vehicle_state/vehicle_state_provider.h"
+#include "modules/common_msgs/routing_msgs/routing.pb.h"
 #include "modules/world_model/relative_map/relative_map.h"
 
 namespace apollo {
@@ -42,6 +43,8 @@ class RelativeMapComponent final : public ::apollo::cyber::TimerComponent {
   std::shared_ptr<cyber::Reader<localization::LocalizationEstimate>>
       localization_reader_ = nullptr;
   std::shared_ptr<cyber::Reader<NavigationInfo>> navigation_reader_ = nullptr;
+  std::shared_ptr<cyber::Reader<routing::RoutingRequest>>
+      routing_request_reader_ = nullptr;
 
   std::shared_ptr<common::VehicleStateProvider> vehicle_state_provider_;
   RelativeMap relative_map_;