diff --git a/modules/world_model/relative_map/BUILD b/modules/world_model/relative_map/BUILD index fc0b9d76..17f9caf4 100644 --- a/modules/world_model/relative_map/BUILD +++ b/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", ], ) diff --git a/modules/world_model/relative_map/common/relative_map_gflags.cc b/modules/world_model/relative_map/common/relative_map_gflags.cc index 1892061c..a6de7a77 100644 --- a/modules/world_model/relative_map/common/relative_map_gflags.cc +++ b/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."); diff --git a/modules/world_model/relative_map/common/relative_map_gflags.h b/modules/world_model/relative_map/common/relative_map_gflags.h index d74af0b8..a2ac30d7 100644 --- a/modules/world_model/relative_map/common/relative_map_gflags.h +++ b/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); diff --git a/modules/world_model/relative_map/navigation_lane.cc b/modules/world_model/relative_map/navigation_lane.cc index f873358b..96cbe86a 100644 --- a/modules/world_model/relative_map/navigation_lane.cc +++ b/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(); + 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((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 { diff --git a/modules/world_model/relative_map/navigation_lane.h b/modules/world_model/relative_map/navigation_lane.h index 251165e6..41826c86 100644 --- a/modules/world_model/relative_map/navigation_lane.h +++ b/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 diff --git a/modules/world_model/relative_map/relative_map.cc b/modules/world_model/relative_map/relative_map.cc index 0e4688db..efa3f916 100644 --- a/modules/world_model/relative_map/relative_map.cc +++ b/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 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); diff --git a/modules/world_model/relative_map/relative_map.h b/modules/world_model/relative_map/relative_map.h index b390c7b1..d57c4305 100644 --- a/modules/world_model/relative_map/relative_map.h +++ b/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); diff --git a/modules/world_model/relative_map/relative_map_component.cc b/modules/world_model/relative_map/relative_map_component.cc index 403a682b..aea19a67 100644 --- a/modules/world_model/relative_map/relative_map_component.cc +++ b/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(); @@ -70,6 +71,13 @@ bool RelativeMapComponent::InitReaders() { relative_map_.OnNavigationInfo(*navigation_info.get()); }); + routing_request_reader_ = node_->CreateReader( + FLAGS_routing_request_topic, + [this](const std::shared_ptr& routing_request) { + ADEBUG << "Received routing request: updating destination."; + relative_map_.OnRoutingRequest(*routing_request.get()); + }); + relative_map_writer_ = node_->CreateWriter(FLAGS_relative_map_topic); return true; } diff --git a/modules/world_model/relative_map/relative_map_component.h b/modules/world_model/relative_map/relative_map_component.h index 94ac7327..69be5d94 100644 --- a/modules/world_model/relative_map/relative_map_component.h +++ b/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> localization_reader_ = nullptr; std::shared_ptr> navigation_reader_ = nullptr; + std::shared_ptr> + routing_request_reader_ = nullptr; std::shared_ptr vehicle_state_provider_; RelativeMap relative_map_;