Merge pull request #8 from openads-project/feature/separate-global-route

Handle enriched/global route separation in lanelet2_route_planning

Author: lreiher

.openads-dev-environment

@@ -2,8 +2,8 @@ repositories:
   perception_interfaces:
     type: git
     url: https://github.com/ika-rwth-aachen/perception_interfaces.git
-    version: v1.0.0
+    version: v1.1.2
   planning_interfaces:
     type: git
     url: https://github.com/ika-rwth-aachen/planning_interfaces.git
-    version: v1.0.0
+    version: v1.1.2

.repos

@@ -11,7 +11,7 @@ services:
         route_topic:=/planning/lanelet2_route_planning/route
 
   lanelet2-route-planning:
-    image: ghcr.io/openads-project/lanelet2_route_planning:v1.2.0
+    image: ghcr.io/openads-project/lanelet2_route_planning:v2.0.0
     environment:
       RMW_IMPLEMENTATION: rmw_fastrtps_cpp
     command: |
@@ -20,15 +20,15 @@ services:
         ego_data_topic:=/localization/ego_data
 
   lanelet2-map-server:
-    image: ghcr.io/openads-project/lanelet2_map_server:v1.2.2
+    image: ghcr.io/openads-project/lanelet2_map_server:v1.2.3
     environment:
       RMW_IMPLEMENTATION: rmw_fastrtps_cpp
     command: |
       ros2 launch lanelet2_map_server lanelet2_map_server_launch.py
         namespace:=/localization
 
   ego-data-publisher:
-    image: ghcr.io/openads-project/monitoring:v1.0.0
+    image: ghcr.io/openads-project/monitoring:v26.9.4
     environment:
       RMW_IMPLEMENTATION: rmw_fastrtps_cpp
     command: |
@@ -38,19 +38,19 @@ services:
         "{header: {stamp: 'now', frame_id: 'map'}, state: {model_id: 1, continuous_state: [80.0, 90.0, 0.75, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.5708, 0.0, 0.0, 0.0], discrete_state: [0, 0, 0, 0]}, length: 4.0, width: 1.8, height: 1.5}"
 
   base-link-transform-publisher:
-    image: ghcr.io/openads-project/monitoring:v1.0.0
+    image: ghcr.io/openads-project/monitoring:v26.9.4
     environment:
       RMW_IMPLEMENTATION: rmw_fastrtps_cpp
     command: |
       ros2 run tf2_ros static_transform_publisher
         80.0 88.54 0.0 1.5708 0.0 0.0 map base_link
 
   rviz:
-    image: ghcr.io/openads-project/monitoring:v1.0.0
+    image: ghcr.io/openads-project/monitoring:v26.9.4
     environment:
       RMW_IMPLEMENTATION: rmw_fastrtps_cpp
       DISPLAY: ${DISPLAY}
     command: rviz2 -d /config.rviz
     volumes:
       - ./rviz/config.rviz:/config.rviz
-      - /tmp/.X11-unix:/tmp/.X11-unix:rw
+      - /tmp/.X11-unix:/tmp/.X11-unix:rw

demo/docker-compose.yml

@@ -6,4 +6,4 @@ description: ROS 2 Reference Trajectory Planning for Automated Driving
 dependencies:
   - repository: oci://ghcr.io/openads-project/openads-helm
     name: openadservice
-    version: 1.0.0
+    version: 1.1.0

deployment/helm/Chart.yaml

@@ -351,7 +351,6 @@ void SimplePlannerNode::routeCallback(const route_planning_msgs::msg::Route::Uni
     route_topic_diagnostic_->tick(msg->header.stamp);
   }
   route_ = *msg;
-
   if (!route_init_) {
     RCLCPP_INFO(this->get_logger(), "Received new route message, initialized global variable");
     route_init_ = true;
@@ -647,12 +646,28 @@ SimplePlannerNode::FollowRoutePlan SimplePlannerNode::buildRoutePlan(const std_m
 void SimplePlannerNode::appendRoutePoints(const route_planning_msgs::msg::Route& tf_route,
                                           FollowRoutePlan& route_plan,
                                           std::map<uint64_t, uint64_t>& lane_change_indices_map) {
+  if (tf_route.current_route_element_idx >= tf_route.route_elements.size()) {
+    const std::string msg = "Current route element index is outside the received local route";
+    setHealth(diagnostic_msgs::msg::DiagnosticStatus::WARN, msg, health_.key_value_pairs);
+    RCLCPP_WARN(this->get_logger(), "%s", msg.c_str());
+    return;
+  }
+
+  const bool destination_is_present = tf_route.destination_route_element_idx < tf_route.route_elements.size();
+  const size_t destination_or_route_end_idx =
+      destination_is_present ? tf_route.destination_route_element_idx : tf_route.route_elements.size();
+  if (destination_or_route_end_idx <= tf_route.current_route_element_idx) {
+    const std::string msg = "Received local route contains no remaining route elements";
+    setHealth(diagnostic_msgs::msg::DiagnosticStatus::WARN, msg, health_.key_value_pairs);
+    RCLCPP_WARN(this->get_logger(), "%s", msg.c_str());
+    return;
+  }
+
   double t_total = 0.0;
-  RCLCPP_DEBUG(this->get_logger(), "Number of remaining route elements: %zu",
-               tf_route.destination_route_element_idx - tf_route.current_route_element_idx);
-  health_.key_value_pairs.insert(
-      {"RemainingRouteElements", std::to_string(tf_route.destination_route_element_idx - tf_route.current_route_element_idx)});
-  for (size_t j = tf_route.current_route_element_idx; j < tf_route.destination_route_element_idx; ++j) {
+  const size_t remaining_route_elements = destination_or_route_end_idx - tf_route.current_route_element_idx;
+  RCLCPP_DEBUG(this->get_logger(), "Number of remaining route elements: %zu", remaining_route_elements);
+  health_.key_value_pairs.insert_or_assign("RemainingRouteElements", std::to_string(remaining_route_elements));
+  for (size_t j = tf_route.current_route_element_idx; j < destination_or_route_end_idx; ++j) {
     const auto& route_element = tf_route.route_elements[j];
     if (!route_element.is_enriched) {
       RCLCPP_DEBUG(this->get_logger(), "Route element %zu is not enriched. Skipping.", j);
@@ -664,21 +679,15 @@ void SimplePlannerNode::appendRoutePoints(const route_planning_msgs::msg::Route&
     simple_path_point.position =
         Eigen::Vector2d(suggested_lane.reference_pose.position.x, suggested_lane.reference_pose.position.y);
     simple_path_point.s = route_element.s;
-    simple_path_point.v = v_ref_;
-    if (v_ref_ < 0.0) {
-      simple_path_point.v = suggested_lane.speed_limit / 3.6;
-    }
+    simple_path_point.v = v_ref_ < 0.0 ? suggested_lane.speed_limit / 3.6 : v_ref_;
 
     if (!route_plan.path.points.empty()) {
-      double v_average = (route_plan.path.points.back().v + simple_path_point.v) / 2.0;
-      double dt = 0.0;
-      if (v_average != 0.0) {
-        dt = (simple_path_point.s - route_plan.path.points.back().s) / v_average;
-      }
+      const double v_average = (route_plan.path.points.back().v + simple_path_point.v) / 2.0;
+      const double dt = v_average != 0.0 ? (simple_path_point.s - route_plan.path.points.back().s) / v_average : 0.0;
       if (dt <= 0.0) {
-        std::string msg = "Negative time difference " + std::to_string(dt) +
-                          " between points at s=" + std::to_string(route_plan.path.points.back().s) +
-                          " and s=" + std::to_string(simple_path_point.s) + ". Could lead to unexpected behavior.";
+        const std::string msg = "Negative time difference " + std::to_string(dt) +
+                                " between points at s=" + std::to_string(route_plan.path.points.back().s) +
+                                " and s=" + std::to_string(simple_path_point.s) + ". Could lead to unexpected behavior.";
         setHealth(diagnostic_msgs::msg::DiagnosticStatus::WARN, msg, health_.key_value_pairs);
         RCLCPP_WARN(this->get_logger(), "%s", msg.c_str());
       }
@@ -700,8 +709,7 @@ void SimplePlannerNode::appendRoutePoints(const route_planning_msgs::msg::Route&
     }
 
     route_plan.path.points.push_back(simple_path_point);
-
-    if (j == tf_route.destination_route_element_idx - 1) {
+    if (destination_is_present && j + 1 == destination_or_route_end_idx) {
       SimplePathPoint destination_point;
       destination_point.position = Eigen::Vector2d(tf_route.destination.x, tf_route.destination.y);
       destination_point.s = simple_path_point.s + (destination_point.position - simple_path_point.position).norm();
@@ -714,6 +722,12 @@ void SimplePlannerNode::appendRoutePoints(const route_planning_msgs::msg::Route&
       break;
     }
   }
+
+  if (!destination_is_present && !route_plan.stop_at_end && t_total < 2.0 * trajectory_horizon_) {
+    const std::string msg = "Local enriched route ends before the planner horizon; trajectory will end at its last element";
+    setHealth(diagnostic_msgs::msg::DiagnosticStatus::WARN, msg, health_.key_value_pairs);
+    RCLCPP_WARN(this->get_logger(), "%s", msg.c_str());
+  }
 }
 
 bool SimplePlannerNode::tryRegisterLaneChange(const route_planning_msgs::msg::Route& tf_route,