Feature/middleline track offline

config/mainConfig.yaml

@@ -14,4 +14,7 @@ pacsim:
   cog_frame_id_pipeline: "cog" # name of the cog frame in the autnomous pipeline
   broadcast_sensors_tf2: true
 
-  pre_transform_track: true # transform track such that starting pose also seems to be the origin of the track
+  pre_transform_track: true # transform track such that starting pose also seems to be the origin of the track
+  centerline:
+    lookahead_distance: 20.0
+    max_front_points: 300

include/track/centerlinePublisher.hpp

@@ -0,0 +1,45 @@
+#ifndef CENTERLINE_PUBLISHER_HPP
+#define CENTERLINE_PUBLISHER_HPP
+
+#include "rclcpp/rclcpp.hpp"
+#include "types.hpp"
+#include "visualization_msgs/msg/marker_array.hpp"
+
+class CenterlinePublisher
+{
+public:
+    void initialize(std::shared_ptr<rclcpp::Node> node);
+    void setTrack(const Track& track, const std::string& frameId, double time);
+    void publishRaw(double time);
+    void publishFront(double time, const Eigen::Vector3d& trans, const Eigen::Vector3d& rot);
+    void setParameters(double lookaheadDist, std::size_t maxPoints) {
+        lookaheadDistanceM_ = lookaheadDist;
+        maxFrontPoints_ = maxPoints;
+    }
+
+private:
+    LandmarkList centerlineRawMapFrame;
+    bool hasCenterlineRaw = false;
+    bool hasLastClosestIdx = false;
+    std::size_t lastClosestIdx = 0;
+
+    LandmarkList centerlineSmoothedMapFrame;
+    bool hasCenterlineSmoothed = false;
+    bool hasLastClosestIdxSmoothed = false;
+    std::size_t lastClosestIdxSmoothed = 0;
+
+    double lookaheadDistanceM_ = 20.0;
+    std::size_t maxFrontPoints_ = 300;
+
+    std::string mapFrame = "map";
+    std::size_t rawPublishCallCounter = 0;
+    std::size_t frontPublishCallCounter = 0;
+    std::size_t frontPublishCallCounterSmoothed = 0;
+
+    rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr centerlineRawVizPub;
+    rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr centerlineRawFrontVizPub;
+    rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr centerlineSmoothedVizPub;
+    rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr centerlineSmoothedFrontVizPub;
+};
+
+#endif /* CENTERLINE_PUBLISHER_HPP */

include/types.hpp

@@ -41,6 +41,8 @@ struct Track
     bool lanesFirstWithLastConnected = true;
     std::vector<Landmark> left_lane;
     std::vector<Landmark> right_lane;
+    std::vector<Landmark> centerline_raw;
+    std::vector<Landmark> centerline_smoothed;
     std::vector<Landmark> unknown;
     std::vector<std::pair<Landmark, Landmark>> time_keeping_gates;
     Eigen::Vector3d gnssOrigin;
@@ -192,6 +194,8 @@ struct MainConfig
     std::string cog_frame_id_pipeline;
     bool broadcast_sensors_tf2;
     bool pre_transform_track;
+    double centerline_lookahead_distance = 20.0;
+    int centerline_max_front_points = 300;
 };
 
 Discipline stringToDiscipline(const std::string& disciplineStr);

launch/example.launch.py

@@ -18,7 +18,7 @@ def getFullFilePath(name, dir):
 
 
 def generate_launch_description():
-  track_name = "circular.yaml"
+  track_name = "FSE32_with_path.yaml"
   track_frame = "map"
   realtime_ratio = 1.0
   discipline = "autocross"

src/pacsim_main.cpp

@@ -22,6 +22,7 @@
 #include "visualization_msgs/msg/marker_array.hpp"
 
 #include "track/trackLoader.hpp"
+#include "track/centerlinePublisher.hpp"
 
 #include "logger.hpp"
 #include "sensorModels/imuSensor.hpp"
@@ -123,6 +124,7 @@ std::shared_ptr<WheelsSensor> torquesSensor;
 std::shared_ptr<ScalarValueSensor> currentSensorTS;
 std::shared_ptr<ScalarValueSensor> voltageSensorTS;
 std::shared_ptr<lidarModel> lidarSensor;
+std::shared_ptr<CenterlinePublisher> centerlinePublisher;
 
 std::shared_ptr<Logger> logger;
 
@@ -168,6 +170,7 @@ int threadMainLoopFunc(std::shared_ptr<rclcpp::Node> node)
     visualization_msgs::msg::MarkerArray mapMarkerMsg = mapMarkersWrapper.markerFromLMs(lms, trackFrame, 0.0);
     mapVizPub->publish(mapMarkerMsg);
     trackPub->publish(createRosTrackMessage(lms, "map", 0.0));
+    centerlinePublisher->setTrack(lms, "map", 0.0);
 
     deadTimeSteeringFront = DeadTime<double>(0.05);
     deadTimeSteeringRear = DeadTime<double>(0.05);
@@ -197,6 +200,8 @@ int threadMainLoopFunc(std::shared_ptr<rclcpp::Node> node)
         auto t = model->getPosition();
         auto rEulerAngles = model->getOrientation();
         auto alpha = model->getAngularAcceleration();
+        centerlinePublisher->publishFront(simTime, t, rEulerAngles);
+
         finish = cl->performAllChecks(lms, simTime, t, rEulerAngles);
         // geometry_msgs::msg::TransformStamped static_transform = createStaticTransform("map", "center", simTime);
         geometry_msgs::msg::TransformStamped transformStamped
@@ -584,6 +589,13 @@ MainConfig fillMainConfig(std::string path)
 
     config.getElement<bool>(&ret.pre_transform_track, "pre_transform_track");
 
+    if (config.hasElement("centerline"))
+    {
+        auto centerline_cfg = config.getElement("centerline");
+        centerline_cfg.getElement<double>(&ret.centerline_lookahead_distance, "lookahead_distance");
+        centerline_cfg.getElement<int>(&ret.centerline_max_front_points, "max_front_points");
+    }
+
     ret.discipline = stringToDiscipline(discipline);
     return ret;
 }
@@ -644,6 +656,8 @@ int main(int argc, char** argv)
     mapVizPub = node->create_publisher<visualization_msgs::msg::MarkerArray>("/pacsim/track/visualization", 1);
 
     trackPub = node->create_publisher<pacsim::msg::Track>("/pacsim/track/landmarks", 1);
+    centerlinePublisher = std::make_shared<CenterlinePublisher>();
+    centerlinePublisher->initialize(node);
 
 
     auto finishSignalServer = node->create_service<std_srvs::srv::Empty>("/pacsim/finish_signal", cbFinishSignal);
@@ -654,6 +668,8 @@ int main(int argc, char** argv)
 
     getRos2Params(node);
     mainConfig = fillMainConfig(main_config_path);
+    centerlinePublisher->setParameters(mainConfig.centerline_lookahead_distance, mainConfig.centerline_max_front_points);
+
     initPerceptionSensors();
     initSensors();
     initLidar();

src/track/centerlinePublisher.cpp

@@ -0,0 +1,215 @@
+#include "track/centerlinePublisher.hpp"
+
+#include "ros2Helpers.hpp"
+#include "transform.hpp"
+
+#include <limits>
+
+namespace
+{
+visualization_msgs::msg::MarkerArray buildCenterlineMarkerArray(
+    const LandmarkList& centerline, const std::string& frame, double time, int markerId, double r = 1.0, double g = 0.0, double b = 0.0)
+{
+    visualization_msgs::msg::MarkerArray markerArray;
+    visualization_msgs::msg::Marker marker;
+    marker.header.frame_id = frame;
+    marker.header.stamp = rclcpp::Time(static_cast<uint64_t>(time * 1e9));
+    marker.ns = "pacsim/centerline";
+    marker.id = markerId;
+    marker.type = visualization_msgs::msg::Marker::SPHERE_LIST;
+    marker.action = visualization_msgs::msg::Marker::MODIFY;
+    marker.pose.orientation.w = 1.0;
+    marker.scale.x = 0.15;
+    marker.scale.y = 0.15;
+    marker.scale.z = 0.15;
+    marker.color.a = 1.0;
+    marker.color.r = r;
+    marker.color.g = g;
+    marker.color.b = b;
+
+    for (const auto& lm : centerline.list)
+    {
+        geometry_msgs::msg::Point p;
+        p.x = lm.position.x();
+        p.y = lm.position.y();
+        p.z = lm.position.z();
+        marker.points.push_back(p);
+    }
+
+    markerArray.markers.push_back(marker);
+    return markerArray;
+}
+} // namespace
+
+void CenterlinePublisher::initialize(std::shared_ptr<rclcpp::Node> node)
+{
+    auto latchedQos = rclcpp::QoS(rclcpp::KeepLast(1)).reliable().transient_local();
+
+    centerlineRawVizPub = node->create_publisher<visualization_msgs::msg::MarkerArray>(
+        "/pacsim/track/centerline_raw", latchedQos);
+    centerlineRawFrontVizPub = node->create_publisher<visualization_msgs::msg::MarkerArray>(
+        "/pacsim/track/centerline_raw_front", latchedQos);
+
+    centerlineSmoothedVizPub = node->create_publisher<visualization_msgs::msg::MarkerArray>(
+        "/pacsim/track/centerline_smoothed", latchedQos);
+    centerlineSmoothedFrontVizPub = node->create_publisher<visualization_msgs::msg::MarkerArray>(
+        "/pacsim/track/centerline_smoothed_front", latchedQos);
+}
+
+void CenterlinePublisher::setTrack(const Track& track, const std::string& frameId, double time)
+{
+    mapFrame = frameId;
+    centerlineRawMapFrame.list = track.centerline_raw;
+    centerlineRawMapFrame.frame_id = frameId;
+    centerlineRawMapFrame.timestamp = time;
+
+    hasCenterlineRaw = !centerlineRawMapFrame.list.empty();
+    hasLastClosestIdx = false;
+    lastClosestIdx = 0;
+
+    if (hasCenterlineRaw)
+    {
+        centerlineRawVizPub->publish(buildCenterlineMarkerArray(centerlineRawMapFrame, frameId, time, 0));
+    }
+
+    centerlineSmoothedMapFrame.list = track.centerline_smoothed;
+    centerlineSmoothedMapFrame.frame_id = frameId;
+    centerlineSmoothedMapFrame.timestamp = time;
+
+    hasCenterlineSmoothed = !centerlineSmoothedMapFrame.list.empty();
+    hasLastClosestIdxSmoothed = false;
+    lastClosestIdxSmoothed = 0;
+
+    if (hasCenterlineSmoothed)
+    {
+        // Use blue for the smoothed centerline visualization
+        centerlineSmoothedVizPub->publish(buildCenterlineMarkerArray(centerlineSmoothedMapFrame, frameId, time, 2, 0.0, 0.0, 1.0));
+    }
+}
+
+void CenterlinePublisher::publishRaw(double time)
+{
+    if (hasCenterlineRaw)
+    {
+        centerlineRawMapFrame.timestamp = time;
+        centerlineRawVizPub->publish(buildCenterlineMarkerArray(centerlineRawMapFrame, mapFrame, time, 0));
+    }
+
+    if (hasCenterlineSmoothed)
+    {
+        centerlineSmoothedMapFrame.timestamp = time;
+        centerlineSmoothedVizPub->publish(buildCenterlineMarkerArray(centerlineSmoothedMapFrame, mapFrame, time, 2, 0.0, 0.0, 1.0));
+    }
+}
+
+void CenterlinePublisher::publishFront(double time, const Eigen::Vector3d& trans, const Eigen::Vector3d& rot)
+{
+    if (!hasCenterlineRaw && !hasCenterlineSmoothed)
+    {
+        return;
+    }
+
+    auto processFront = [&](bool hasCenterline, std::size_t& callCounter, 
+                            LandmarkList& mapFrameData, bool& hasLastIdx, std::size_t& lastIdx, 
+                            auto vizPub, int markerId, double r, double g, double b) {
+        if (!hasCenterline) return;
+
+        ++callCounter;
+        mapFrameData.timestamp = time;
+
+        const auto& centerlinePoints = mapFrameData.list;
+        const std::size_t pointCount = centerlinePoints.size();
+        if (pointCount == 0) return;
+
+        std::size_t closestIdx = 0;
+        double closestDistSq = std::numeric_limits<double>::infinity();
+
+        constexpr std::size_t localSearchWindow = 50;
+        constexpr std::size_t fullScanResyncInterval = 100;
+        const bool doFullScan = !hasLastIdx || pointCount <= (2 * localSearchWindow + 1)
+            || (callCounter % fullScanResyncInterval == 0);
+
+        if (doFullScan)
+        {
+            for (std::size_t idx = 0; idx < pointCount; ++idx)
+            {
+                const double distSq = (centerlinePoints[idx].position - trans).squaredNorm();
+                if (distSq < closestDistSq)
+                {
+                    closestDistSq = distSq;
+                    closestIdx = idx;
+                }
+            }
+        }
+        else
+        {
+            closestIdx = lastIdx;
+            closestDistSq = (centerlinePoints[closestIdx].position - trans).squaredNorm();
+
+            const std::size_t maxOffset = std::min(localSearchWindow, pointCount - 1);
+            for (std::size_t offset = 1; offset <= maxOffset; ++offset)
+            {
+                const std::size_t idxForward = (lastIdx + offset) % pointCount;
+                const std::size_t idxBackward = (lastIdx + pointCount - offset) % pointCount;
+
+                const double distForwardSq = (centerlinePoints[idxForward].position - trans).squaredNorm();
+                if (distForwardSq < closestDistSq)
+                {
+                    closestDistSq = distForwardSq;
+                    closestIdx = idxForward;
+                }
+
+                const double distBackwardSq = (centerlinePoints[idxBackward].position - trans).squaredNorm();
+                if (distBackwardSq < closestDistSq)
+                {
+                    closestDistSq = distBackwardSq;
+                    closestIdx = idxBackward;
+                }
+            }
+        }
+
+        lastIdx = closestIdx;
+        hasLastIdx = true;
+
+        const double lookaheadDistanceM = lookaheadDistanceM_;
+        const std::size_t maxFrontPoints = maxFrontPoints_;
+
+        LandmarkList centerlineFrontMapFrame;
+        centerlineFrontMapFrame.frame_id = mapFrame;
+        centerlineFrontMapFrame.timestamp = time;
+        centerlineFrontMapFrame.list.reserve(std::min(pointCount, maxFrontPoints));
+
+        centerlineFrontMapFrame.list.push_back(centerlinePoints[closestIdx]);
+        std::size_t currentIdx = closestIdx;
+        double accumulatedDistance = 0.0;
+
+        for (std::size_t step = 0; step + 1 < pointCount && centerlineFrontMapFrame.list.size() < maxFrontPoints; ++step)
+        {
+            const std::size_t nextIdx = (currentIdx + 1) % pointCount;
+            const double segmentDistance = (centerlinePoints[nextIdx].position - centerlinePoints[currentIdx].position).norm();
+
+            if (accumulatedDistance + segmentDistance > lookaheadDistanceM)
+            {
+                break;
+            }
+
+            accumulatedDistance += segmentDistance;
+            centerlineFrontMapFrame.list.push_back(centerlinePoints[nextIdx]);
+            currentIdx = nextIdx;
+
+            if (currentIdx == closestIdx)
+            {
+                break;
+            }
+        }
+
+        LandmarkList centerlineFrontCarFrame = transformLmList(centerlineFrontMapFrame, trans, rot);
+        centerlineFrontCarFrame.frame_id = "car";
+        centerlineFrontCarFrame.timestamp = time;
+
+        vizPub->publish(buildCenterlineMarkerArray(centerlineFrontCarFrame, "car", time, markerId, r, g, b));
+    };
+
+    processFront(hasCenterlineRaw, frontPublishCallCounter, centerlineRawMapFrame, hasLastClosestIdx, lastClosestIdx, centerlineRawFrontVizPub, 1, 1.0, 0.0, 0.0);
+    processFront(hasCenterlineSmoothed, frontPublishCallCounterSmoothed, centerlineSmoothedMapFrame, hasLastClosestIdxSmoothed, lastClosestIdxSmoothed, centerlineSmoothedFrontVizPub, 3, 0.0, 0.0, 1.0);
+}