Add vehicle stream location and fix cancellation handling

CMakeLists.txt

@@ -62,6 +62,7 @@ add_subdirectory(src/gateway_bridge)
 add_subdirectory(src/cluster_bridge)
 add_subdirectory(src/planning_bridge)
 add_subdirectory(src/perception_bridge)
+add_subdirectory(src/location_bridge)
 add_subdirectory(src/trip_bridge)
 add_subdirectory(proto)
 
@@ -79,6 +80,7 @@ target_link_libraries(autoware_agent
         planning_bridge
         perception_bridge
         trip_bridge
+        location_bridge
         proto_lib
         zenohc::lib
         zenohcxx::zenohc

proto/vehicle_frame.proto

@@ -378,4 +378,20 @@ TripState trip_state        = 8;
   double    goal_lon          = 18;
   double    goal_distance_m   = 19;
 
+}
+
+
+//*********************location data*************************************
+
+message VehicleLocation{
+  double longitude = 1;
+  double latitude = 2;
+}
+
+
+
+message LocationFrame{
+  int64  stamp_ns = 1;
+  uint64 seq      = 2;
+  VehicleLocation vehicle_location = 3;
 }

src/AutowareApp.cpp

@@ -19,6 +19,7 @@
 #include "ClusterBridgeProvider.h"
 #include "Config.h"
 #include "cluster_bridge/include/ClusterBridge.h"
+#include "location_bridge/include/LocationBridge.h"
 #include "perception_bridge/include/PerceptionBridge.h"
 #include "planning_bridge/include/PlanningBridge.h"
 #include "trip_bridge/include/TripBridge.h"
@@ -99,6 +100,18 @@ AppHandles startAutowareApp(const std::string& yaml_path, const std::string& ser
     "127.0.0.1:50051", trip_adapter.get(), eta_adapter.get(), loc_adapter.get(), "ORIN_NANO_001",
     h.cluster_bridge_->GetRequestId());
 
+  h.location_bridge_ = std::make_shared<LocationBridge>(
+    node, h.zsession_,
+    [ctrl = h.controller_]() -> std::optional<std::pair<double, double>> {
+      TripStatus st = ctrl->getTripStatusSync();
+      if (st.current_gps_.latitude == 0.0 && st.current_gps_.longitude == 0.0)
+        return std::nullopt;
+      return std::make_pair(st.current_gps_.latitude, st.current_gps_.longitude);
+    },
+    [ta = trip_adapter]() -> int64_t { return ta->GetActiveTripId(); }
+
+  );
+
   stream_client->set_handlers({
     .on_trip_init =
       [ctrl = h.controller_, sc = stream_client](const vehicle_gateway::TripInitRequest& req) {
@@ -160,19 +173,25 @@ AppHandles startAutowareApp(const std::string& yaml_path, const std::string& ser
 
   });
 
-  h.controller_->setTripStateCallback([sc = stream_client](TripState, TripState next) {
-    if (next == TripState::WAITING_FOR_MOVE) {
-      sc->ReportTripInitAck();
-      sc->ReportArrive();
-      sc->ReportStatus("ARRIVED_AT_PICKUP");
-    } else if (next == TripState::RUNNING)
-      sc->ReportStatus("IN_PROGRESS");
-    else if (next == TripState::COMPLETED) {
-      sc->ReportArrive();
-      sc->ReportStatus("COMPLETED");
-    } else if (next == TripState::FAILED)
-      sc->ReportStatus("ERROR");
-  });
+  h.controller_->setTripStateCallback(
+    [sc = stream_client, lb = h.location_bridge_](TripState, TripState next) {
+      if (next == TripState::WAITING_FOR_MOVE) {
+        sc->ReportTripInitAck();
+        sc->ReportArrive();
+        sc->ReportStatus("ARRIVED_AT_PICKUP");
+        lb->setStreamingEnabled(false);
+      } else if (next == TripState::DRIVING_TO_PICKUP) {
+        lb->setStreamingEnabled(true);
+      }
+
+      else if (next == TripState::RUNNING)
+        sc->ReportStatus("IN_PROGRESS");
+      else if (next == TripState::COMPLETED) {
+        sc->ReportArrive();
+        sc->ReportStatus("COMPLETED");
+      } else if (next == TripState::FAILED)
+        sc->ReportStatus("ERROR");
+    });
 
   h.stream_client_ = stream_client;
 
@@ -197,6 +216,11 @@ void stopAutowareApp(AppHandles& h) {
   if (h.cluster_bridge_) {
     h.cluster_bridge_->shutdown();
   }
+
+  if (h.location_bridge_) {
+    h.location_bridge_->shutdown();
+  }
+
   if (h.stream_client_)
     h.stream_client_->shutdown();
 
@@ -210,6 +234,7 @@ void stopAutowareApp(AppHandles& h) {
   h.perception_bridge_.reset();
   h.planning_bridge_.reset();
   h.cluster_bridge_.reset();
+  h.location_bridge_.reset();
   h.controller_.reset();
   h.zsession_.reset();
 }

src/AutowareApp.h

@@ -28,6 +28,7 @@ class ClusterBridge;
 class PlanningBridge;
 class PerceptionBridge;
 class TripBridge;
+class LocationBridge;
 
 namespace vehicle_gateway {
 class VehicleGatewayService;
@@ -49,6 +50,8 @@ struct AppHandles {
   std::shared_ptr<::PlanningBridge> planning_bridge_;
   std::shared_ptr<::PerceptionBridge> perception_bridge_;
   std::shared_ptr<::TripBridge> trip_bridge_;
+  std::shared_ptr<::LocationBridge> location_bridge_;
+
   std::shared_ptr<vehicle_gateway::VehicleGatewayStreamClient> stream_client_;
   std::shared_ptr<zenoh::Session> zsession_;
   std::thread ros_thread_;

src/gateway_bridge/src/TripController.cc

@@ -306,6 +306,13 @@ void TripController::cancel() {
   engaging_for_pickup_ = false;
   current_route_state_.reset();
 
+  // reset the vehicle to the default start position in the map
+  const FixedStartPosition* fixed_start = lanelet_map_.getDefaultStart();
+  if (fixed_start) {
+    doPublishInitialPose(fixed_start->local.x, fixed_start->local.y, fixed_start->local.z,
+                         fixed_start->orientation.z, fixed_start->orientation.w);
+  }
+
   transitionTo(TripState::IDLE);
 }
 
@@ -572,6 +579,59 @@ void TripController::finishQueryLeg() {
                 .distance_m_ = eta_distance_m_,
                 .goal_gps_ = query_goal_gps_};
 
+    /////////////////////////////////////////////////
+    // Raw local coords of what backend sent:
+    LocalCoordinate raw_pickup = lanelet_map_.gpsToLocal(query_start_gps_);
+    LocalCoordinate raw_dest = lanelet_map_.gpsToLocal(query_goal_gps_);
+
+    // Resolved pickup coords (trip leg's start = pickup lane center/projection):
+    double pickup_x = status_.start_x_;
+    double pickup_y = status_.start_y_;
+
+    // Resolved destination coords:
+    double dest_x = status_.goal_x_;
+    double dest_y = status_.goal_y_;
+
+    // Distances (snap distance: how far the resolved point is from the raw GPS)
+    double pickup_snap_dist =
+      std::sqrt(std::pow(pickup_x - raw_pickup.x, 2) + std::pow(pickup_y - raw_pickup.y, 2));
+
+    double dest_snap_dist =
+      std::sqrt(std::pow(dest_x - raw_dest.x, 2) + std::pow(dest_y - raw_dest.y, 2));
+
+    // Convert resolved pickup local coords back to GPS
+    GPSCoordinate resolved_pickup_gps = lanelet_map_.localToGps({pickup_x, pickup_y, 0.0});
+    GPSCoordinate resolved_dest_gps = lanelet_map_.localToGps({dest_x, dest_y, 0.0});
+
+    RCLCPP_INFO(
+      node_->get_logger(),
+      "[RouteResolution] PICKUP:"
+      "\n  Backend GPS:       (%.6f, %.6f)"
+      "\n  Backend local:     (%.2f, %.2f)"
+      "\n  Resolved lane:     %s"
+      "\n  Resolved local:    (%.2f, %.2f)"
+      "\n  Resolved GPS:      (%.6f, %.6f)"  // ADD THIS LINE
+      "\n  Snap distance:     %.2f m",
+      query_start_gps_.latitude, query_start_gps_.longitude, raw_pickup.x, raw_pickup.y,
+      pickup_route_.has_value() ? std::to_string(pickup_route_->goal_lane_id_).c_str() : "?",
+      pickup_x, pickup_y, resolved_pickup_gps.latitude, resolved_pickup_gps.longitude,  // ADD THIS
+      pickup_snap_dist);
+
+    RCLCPP_INFO(node_->get_logger(),
+                "[RouteResolution] DESTINATION:"
+                "\n  Backend GPS:       (%.6f, %.6f)"
+                "\n  Backend local:     (%.2f, %.2f)"
+                "\n  Resolved lane:     %s"
+                "\n  Resolved local:    (%.2f, %.2f)"
+                "\n  Resolved GPS:      (%.6f, %.6f)"  // ADD THIS LINE
+                "\n  Snap distance:     %.2f m",
+                query_goal_gps_.latitude, query_goal_gps_.longitude, raw_dest.x, raw_dest.y,
+                status_.goal_lanelet_id_.c_str(), dest_x, dest_y, resolved_dest_gps.latitude,
+                resolved_dest_gps.longitude,  // ADD THIS
+                dest_snap_dist);
+
+    /////////////////////////////////////
+
     RCLCPP_INFO(node_->get_logger(), "[AutowareAgent] Trip leg done — ETA %.1f s / %.1f m",
                 eta_seconds_, eta_distance_m_);
     spdlog::info("[AutowareAgent] Trip leg done — {:.1f} s / {:.1f} m", eta_seconds_,

src/location_bridge/CMakeLists.txt

@@ -0,0 +1,46 @@
+find_package(rclcpp REQUIRED)
+# find_package(geometry_msgs REQUIRED)
+# find_package(autoware_vehicle_msgs REQUIRED)
+# find_package(tier4_external_api_msgs REQUIRED)
+# find_package(autoware_perception_msgs REQUIRED)
+# find_package(autoware_internal_debug_msgs REQUIRED)
+# find_package(autoware_internal_msgs REQUIRED)
+# find_package(autoware_internal_planning_msgs REQUIRED)
+# find_package(nav_msgs REQUIRED)
+# find_package(tier4_vehicle_msgs REQUIRED)
+# find_package(autoware_adapi_v1_msgs REQUIRED)
+# find_package(autoware_planning_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(fmt REQUIRED)
+find_package(yaml-cpp REQUIRED)
+find_package(Boost REQUIRED COMPONENTS system)
+
+
+add_library(location_bridge
+        src/LocationBridge.cc
+        include/FrameStates.h
+)
+
+target_link_libraries(location_bridge
+        PUBLIC rclcpp::rclcpp
+
+        PUBLIC ${nav_msgs_TARGETS}
+        PUBLIC map_routes_lib
+        PUBLIC config_header
+        PUBLIC fmt::fmt
+        PUBLIC yaml-cpp
+        PUBLIC zenohc::lib
+        PUBLIC zenohcxx::zenohc
+        PUBLIC proto_lib
+        PRIVATE Boost::system
+)
+
+target_include_directories(location_bridge PUBLIC
+        ${CMAKE_CURRENT_SOURCE_DIR}/include
+        ${CMAKE_CURRENT_SOURCE_DIR}/../map_routes
+        ${CMAKE_BINARY_DIR}/generated
+)
+
+install(TARGETS location_bridge
+        DESTINATION lib/${PROJECT_NAME}
+)

src/location_bridge/include/FrameStates.h

@@ -0,0 +1,15 @@
+//
+// Created by alaa on 9/7/26.
+//
+
+#ifndef VEHICLEAUTOWAREAGENT_LOCATION_FRAMESTATES_H
+#define VEHICLEAUTOWAREAGENT_LOCATION_FRAMESTATES_H
+#include <cstdint>
+#include <vector>
+
+#include <vehicle_frame.grpc.pb.h>
+
+struct LocationFrameState {
+  vehicle_frame::VehicleLocation vehicle_location{};
+};
+#endif

src/location_bridge/include/LocationBridge.h

@@ -0,0 +1,81 @@
+/*
+ * Copyright 2026 alaa
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef VEHICLEAUTOWAREAGENT_LOCATIONBRIDGE_H
+#define VEHICLEAUTOWAREAGENT_LOCATIONBRIDGE_H
+
+#include "FrameStates.h"
+#include "vehicle_frame.pb.h"
+#include "zenoh_publisher.h"
+
+#include <nav_msgs/msg/odometry.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <boost/asio/executor_work_guard.hpp>
+#include <boost/asio/io_context.hpp>
+#include <boost/asio/steady_timer.hpp>
+#include <boost/asio/strand.hpp>
+
+#include <zenoh.hxx>
+
+class LocationBridge : public autoware_agent::ZenohPublisher {
+ public:
+  // GPS provider function type, returns optional pair of latitude and longitude
+  using GpsProvider = std::function<std::optional<std::pair<double, double>>()>;
+  using TripIdProvider = std::function<int64_t()>;
+
+  explicit LocationBridge(rclcpp::Node::SharedPtr node,
+                          const std::shared_ptr<zenoh::Session>& zsession, GpsProvider gps_provider,
+                          TripIdProvider trip_id_provider);
+
+  ~LocationBridge();
+
+  void setStreamingEnabled(bool enabled);
+
+  void shutdown();
+
+ private:
+  LocationFrameState state_;
+  uint64_t frame_seq_{0};
+  boost::asio::io_context io_context_;
+  boost::asio::io_context::strand strand_;
+  boost::asio::executor_work_guard<boost::asio::io_context::executor_type> work_guard_;
+  std::thread io_thread_;
+
+  GpsProvider gps_provider_;
+  TripIdProvider trip_id_provider_;
+  double current_lat_{0.0};
+  double current_lon_{0.0};
+  // asio timer 60Hz
+  boost::asio::steady_timer publisher_timer_;
+
+  void scheduleNextTick();
+  void onTick();
+
+  // called on strand for grpc clients
+  vehicle_frame::LocationFrame buildFrame();
+
+  // called on strand for grpc clients
+  void broadcastFrame(const vehicle_frame::LocationFrame& frame);
+
+  // ros
+  rclcpp::Node::SharedPtr node_;
+
+  // handle location streaming
+  bool streaming_enabled_{false};
+};
+
+#endif  // VEHICLEAUTOWAREAGENT_PERCEPTIONBRIDGE_H