feat(ros2): [3/8] unify camera publishers under CarlaCameraPublisher (carla-simulator#9746)

* feat(ros2): unify camera publishers under CarlaCameraPublisher

Lands the camera-image slice of upstream ROS2 Refactor cd3d640 on
ue5-dev, plus the K-matrix correctness fix from 86ebadc and the
<cmath> include from 02a83ef:

  * New CarlaCameraPublisher.{h,cpp} base inherits BasePublisher (from
    PR-2) and owns the sensor_msgs/Image + sensor_msgs/CameraInfo
    DataWriter pair via PublisherImpl<>. CameraIntrinsics.h hosts the
    pinhole-camera intrinsics math (fx = width / (2 * tan(fov * pi /
    360))) in a header-only seam so the K matrix can be exercised by a
    GTest without linking FastDDS. The PublisherImpl<> forward
    declarations in the header keep <fastdds/...> includes out of
    carla-server's ROS2.cpp compile unit, same pattern PR-2 uses for
    AckermannControlSubscriber.

  * CarlaRGBCameraPublisher collapses to a 24-LOC header subclass with
    GetChannels() = 4 and GetEncoding() = "bgra8". CarlaDepth, CarlaSS,
    CarlaIS, CarlaNormals camera publishers become `using` aliases of
    CarlaRGBCameraPublisher (their wire format is uniform 4-channel
    BGRA uint8 on ue5-dev today: encoding "bgra8", step = width * 4).
    Their per-sensor .cpp files are deleted.

  * CarlaOpticalFlowCameraPublisher stays as a non-alias subclass that
    overrides ComputeImage to reinterpret the post-header buffer as
    const float * and run the HSV colour-wheel encoding. The encoding
    math is lifted out of the publisher into a header-only seam
    OpticalFlowEncoding.h so the GTest can exercise it without
    FastDDS. This intentionally diverges from ue4-dev (which aliased
    OpticalFlow to RGB and shipped raw float bytes as BGRA);
    preserving the encoded form keeps the topic visually useful for
    ROS 2 subscribers consuming ue5-dev today.

  * ROS2.cpp gains a private GetOrCreateCameraSensor<CameraT> template
    that replaces the six per-camera switch cases in GetOrCreateSensor.
    The five BGRA8 camera dispatches in ProcessDataFromCamera collapse
    to one shared block; OpticalFlow keeps its own dispatch because
    the buffer cast differs. A parallel _camera_publishers map tracks
    CarlaCameraPublisher-derived publishers (which do not inherit the
    legacy CarlaPublisher base); RemoveActorRosName clears it alongside
    _publishers and _transforms.

  * CarlaLineInvasionPublisher.{h,cpp} and the four CarlaLineInvasion
    types files are deleted along with their three references in
    ROS2.cpp (include, make_shared, dynamic_pointer_cast). The
    publisher was wired in but had no Unreal-plugin call sites on
    ue5-dev.

  * DVS publisher migration is intentionally deferred to PR-4. The
    unified CarlaDVSPublisher composite needs a CarlaPointCloudPublisher
    base for its event-cloud half, and that base is introduced in PR-4
    alongside the lidar/radar/semantic-lidar migration.

Test coverage:
  * LibCarla/source/test/common/test_camera_intrinsics.cpp exercises
    ComputeIntrinsics against known FOV/resolution combinations and
    guards against the pre-86ebadcee operator-precedence regression
    (the old spelling applied M_PI / 360 to tan(fov) instead of to
    fov, returning ~0.06 for a 90deg FOV instead of the correct 960).
  * LibCarla/source/test/common/test_optical_flow_encoding.cpp
    exercises EncodeFlowPixelToBgra and EncodeFlowImageToBgra with
    7 cases covering zero flow, hue hemispheres, brightness clamp,
    alpha-byte invariant, and buffer length.

(adapted from ue4-dev cd3d640)
(adapted from ue4-dev 86ebadc)

Co-Authored-By: joel-mb <joel.moriana@gmail.com>
Co-Authored-By: csonthomisi <48515100+csonthomisi@users.noreply.github.com>

* fix(ros2): address Copilot review on PR carla-simulator#9746

Round-1 follow-up on top of 30b5c8e addressing the four inline comments
left by GitHub Copilot:

  * test_optical_flow_encoding.cpp: add an explicit <algorithm> include so
    std::max({...}) does not rely on a transitive pull from the production
    header.

  * publishers/OpticalFlowEncoding.h: rewrite the saturation-curve comment.
    With shift = 0.999 and a = 1 / log(0.1 + shift) the intensity reaches 1
    at magnitude ~= 0.1 (not at unit magnitude as the prior comment claimed).
    Math is unchanged; ue5-dev's existing wire format is preserved.

  * publishers/PublisherImpl.h: null-guard Publish(). If Init() failed (or
    was never called), the previous code dereferenced _datawriter and
    crashed; now it logs once and returns false.

  * publishers/CarlaCameraPublisher.cpp: log if either PublisherImpl<>::Init
    fails inside the constructor instead of silently dropping the return
    value. Pairs with the PublisherImpl guard above.

  * Actor/ActorDispatcher.cpp: drop the redundant RemoveActorRosName call
    after RemoveActorCallback. RemoveActorCallback already routes to
    UnregisterVehicle which clears the ros_name maps, so the explicit
    second call was a no-op. Comment updated to reflect the actual cleanup
    chain.

No CHANGELOG entry (consolidated in PR-12).

* fix(ros2): address Copilot round-2 review on PR carla-simulator#9746

Round-2 follow-up on top of 0dbc1ef addressing the three new inline
comments from the post-round-1 Copilot pass:

  * publishers/CarlaCameraPublisher.cpp: Publish() now calls the image and
    camera_info writers unconditionally and combines the results. The
    previous "_impl_camera_info->Publish() && _impl_image->Publish()" form
    short-circuited the image publish whenever the camera_info publish
    failed; the new form preserves the per-sensor publishers' behaviour
    and ships the image regardless of header-writer state.

  * ros2/ROS2CallbackData.h: explicit #include <cstdint>. VehicleControl
    declares an int32_t gear field but the header only pulled <variant>
    and <functional>. Avoids relying on transitive includes.

  * ros2/ROS2.h: explicit #include <string>. Many public APIs take
    std::string arguments; the header previously pulled it transitively
    through <unordered_map> / <vector>.

No CHANGELOG entry (consolidated in PR-12).

* fix(ros2): enforce camera publisher type on cache hit (PR carla-simulator#9746 review)

GetOrCreateCameraSensor cached the camera publisher by actor only, so a
cache hit returned the existing publisher regardless of the requested
CameraT. Under the current one-actor-one-camera-type invariant this is
safe, but if an actor were ever dispatched as two camera types the
first-created type would silently win and, for example, route optical
flow float bytes through an RGB publisher.

The cache-hit path now dynamic_pointer_cast<CameraT> the stored
publisher. RGB/Depth/SS/IS/Normals all alias CarlaRGBCameraPublisher so
a hit across them casts cleanly and is expected; only an RGB to
OpticalFlow mismatch fails the cast, in which case the sample is logged
and skipped (callers already guard on a null publisher) instead of
corrupting the published image.

Add test_camera_publisher_types.cpp to lock the type partitioning the
guard relies on (the BGRA cameras collapse to one type, optical flow
stays a distinct polymorphic sibling). The publisher constructors live
in the FastDDS-linked native build and are not linked into the test
binaries, so the test asserts the relationships at compile time via type
traits.

Add stack_cameras.json under PythonAPI/examples/ros2 spawning all six
unified camera types for manual ROS 2 validation of the publisher path.

* fix(ros2): rate-limit publisher Init-failure log to one-shot

A failed Init() leaves the publisher cached, so Publish() runs once per
frame for the sensor's lifetime. Log the uninitialized-DataWriter guard
only once instead of every frame to avoid flooding the server log.

---------

Co-authored-by: joel-mb <joel.moriana@gmail.com>
Co-authored-by: csonthomisi <48515100+csonthomisi@users.noreply.github.com>

Author: 3 people

LibCarla/source/carla/ros2/ROS2.cpp

@@ -12,9 +12,10 @@
 #include "carla/ros2/ROS2CallbackData.h"
 #include "carla/streaming/detail/Types.h"
 
-#include <unordered_set>
-#include <unordered_map>
 #include <memory>
+#include <string>
+#include <unordered_map>
+#include <unordered_set>
 #include <vector>
 
 // forward declarations
@@ -38,6 +39,7 @@ namespace carla {
 namespace ros2 {
 
   class CarlaPublisher;
+  class CarlaCameraPublisher;
   class CarlaTransformPublisher;
   class CarlaClockPublisher;
   class BaseSubscriber;
@@ -162,6 +164,17 @@ void ProcessDataFromCollisionSensor(
   private:
   std::pair<std::shared_ptr<CarlaPublisher>, std::shared_ptr<CarlaTransformPublisher>> GetOrCreateSensor(int type, carla::streaming::detail::stream_id_type id, void* actor);
 
+  // Camera-side counterpart of GetOrCreateSensor for publishers that inherit
+  // the new CarlaCameraPublisher base. Used by ProcessDataFromCamera for the
+  // unified RGB/Depth/SS/IS/Normals/OpticalFlow path. DVS still uses the
+  // legacy GetOrCreateSensor route until PR-4 introduces CarlaPointCloudPublisher.
+  template <typename CameraT>
+  std::pair<std::shared_ptr<CarlaCameraPublisher>, std::shared_ptr<CarlaTransformPublisher>>
+  GetOrCreateCameraSensor(
+      carla::streaming::detail::stream_id_type id,
+      void *actor,
+      std::string default_prefix);
+
   // sigleton
   ROS2() {};
 
@@ -175,6 +188,7 @@ void ProcessDataFromCollisionSensor(
   std::unordered_map<void *, std::vector<void*> > _actor_parent_ros_name;
   std::shared_ptr<CarlaClockPublisher> _clock_publisher;
   std::unordered_map<void *, std::shared_ptr<CarlaPublisher>> _publishers;
+  std::unordered_map<void *, std::shared_ptr<CarlaCameraPublisher>> _camera_publishers;
   std::unordered_map<void *, std::shared_ptr<CarlaTransformPublisher>> _transforms;
   std::unordered_set<carla::streaming::detail::stream_id_type> _publish_stream;
   std::unordered_map<void *, ActorCallback> _actor_callbacks;

LibCarla/source/carla/ros2/ROS2.h

@@ -6,8 +6,9 @@
 
 #pragma once
 
-#include <variant>
+#include <cstdint>
 #include <functional>
+#include <variant>
 
 namespace carla {
 namespace ros2 {

LibCarla/source/carla/ros2/ROS2CallbackData.h

@@ -0,0 +1,48 @@
+// Copyright (c) 2026 Computer Vision Center (CVC) at the Universitat Autonoma
+// de Barcelona (UAB).
+//
+// This work is licensed under the terms of the MIT license.
+// For a copy, see <https://opensource.org/licenses/MIT>.
+
+#pragma once
+
+#include <cmath>
+#include <cstdint>
+
+namespace carla {
+namespace ros2 {
+
+struct CameraIntrinsics {
+  double fx;
+  double fy;
+  double cx;
+  double cy;
+};
+
+// Builds a pinhole-camera intrinsic matrix from horizontal FOV (degrees) and
+// image dimensions (pixels):
+//   fx = width / (2 * tan(fov_deg * pi / 360))
+//   fy = fx (square pixels)
+//   cx = width  / 2
+//   cy = height / 2
+//
+// This is the corrected form from upstream commit 86ebadcee. The pre-fix
+// spelling read `width / (2 * tan(fov) * M_PI / 360)` which applied the
+// degree-to-radian conversion to `tan(fov)` instead of to `fov`, producing a
+// nonsensical focal length.
+//
+// M_PI is used here, not std::numbers::pi_v, because this header is consumed
+// by the carla-ros2-native ExternalProject in Ros2Native/, which does not
+// configure CMAKE_CXX_STANDARD and therefore defaults to C++17 where <numbers>
+// is unavailable.
+[[nodiscard]] inline CameraIntrinsics ComputeIntrinsics(
+    uint32_t width, uint32_t height, float fov_degrees) noexcept {
+  const double w = static_cast<double>(width);
+  const double h = static_cast<double>(height);
+  const double fov_rad = static_cast<double>(fov_degrees) * M_PI / 360.0;
+  const double fx = w / (2.0 * std::tan(fov_rad));
+  return {fx, fx, w / 2.0, h / 2.0};
+}
+
+}  // namespace ros2
+}  // namespace carla

LibCarla/source/carla/ros2/publishers/CameraIntrinsics.h

@@ -0,0 +1,130 @@
+// Copyright (c) 2026 Computer Vision Center (CVC) at the Universitat Autonoma
+// de Barcelona (UAB).
+//
+// This work is licensed under the terms of the MIT license.
+// For a copy, see <https://opensource.org/licenses/MIT>.
+
+#include "carla/ros2/publishers/CarlaCameraPublisher.h"
+
+#include "carla/Logging.h"
+#include "carla/ros2/publishers/CameraIntrinsics.h"
+#include "carla/ros2/publishers/PublisherImpl.h"
+#include "carla/ros2/types/CameraInfo.h"
+#include "carla/ros2/types/CameraInfoPubSubTypes.h"
+#include "carla/ros2/types/Image.h"
+#include "carla/ros2/types/ImagePubSubTypes.h"
+
+#include <cmath>
+#include <utility>
+
+namespace carla {
+namespace ros2 {
+
+struct CarlaCameraImageMsgTraits {
+  using msg_type = sensor_msgs::msg::Image;
+  using msg_pubsub_type = sensor_msgs::msg::ImagePubSubType;
+};
+
+struct CarlaCameraInfoMsgTraits {
+  using msg_type = sensor_msgs::msg::CameraInfo;
+  using msg_pubsub_type = sensor_msgs::msg::CameraInfoPubSubType;
+};
+
+CarlaCameraPublisher::CarlaCameraPublisher(
+    std::string base_topic_name, std::string frame_id)
+  : BasePublisher(std::move(base_topic_name), std::move(frame_id)),
+    _impl_image(std::make_shared<PublisherImpl<CarlaCameraImageMsgTraits>>()),
+    _impl_camera_info(std::make_shared<PublisherImpl<CarlaCameraInfoMsgTraits>>()) {
+  if (!_impl_image->Init(GetBaseTopicName() + "/image")) {
+    log_error("CarlaCameraPublisher: failed to initialise image writer for", GetBaseTopicName());
+  }
+  if (!_impl_camera_info->Init(GetBaseTopicName() + "/camera_info")) {
+    log_error("CarlaCameraPublisher: failed to initialise camera_info writer for", GetBaseTopicName());
+  }
+}
+
+CarlaCameraPublisher::~CarlaCameraPublisher() = default;
+
+bool CarlaCameraPublisher::Publish() {
+  // Publish both writers unconditionally so a failure on one does not silence
+  // the other. Image goes first to match the previous per-sensor publishers.
+  const bool image_ok = _impl_image->Publish();
+  const bool info_ok = _impl_camera_info->Publish();
+  return image_ok && info_ok;
+}
+
+std::vector<uint8_t> CarlaCameraPublisher::ComputeImage(
+    uint32_t height, uint32_t width, const uint8_t *data) const {
+  const size_t size =
+      static_cast<size_t>(height) * static_cast<size_t>(width) * GetChannels();
+  return std::vector<uint8_t>(data, data + size);
+}
+
+bool CarlaCameraPublisher::WriteCameraInfo(
+    int32_t seconds,
+    uint32_t nanoseconds,
+    uint32_t x_offset,
+    uint32_t y_offset,
+    uint32_t height,
+    uint32_t width,
+    float fov,
+    bool do_rectify) {
+  auto *message = _impl_camera_info->GetMessage();
+  message->header().stamp().sec(seconds);
+  message->header().stamp().nanosec(nanoseconds);
+  message->header().frame_id(GetFrameId());
+
+  const auto k = ComputeIntrinsics(width, height, fov);
+
+  message->height(height);
+  message->width(width);
+  message->distortion_model("plumb_bob");
+  message->D({0.0, 0.0, 0.0, 0.0, 0.0});
+  message->k({k.fx, 0.0, k.cx, 0.0, k.fy, k.cy, 0.0, 0.0, 1.0});
+  message->r({1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0});
+  message->p({k.fx, 0.0, k.cx, 0.0, 0.0, k.fy, k.cy, 0.0, 0.0, 0.0, 1.0, 0.0});
+  message->binning_x(0);
+  message->binning_y(0);
+
+  message->roi().x_offset(x_offset);
+  message->roi().y_offset(y_offset);
+  message->roi().height(height);
+  message->roi().width(width);
+  message->roi().do_rectify(do_rectify);
+
+  return true;
+}
+
+bool CarlaCameraPublisher::WriteImage(
+    int32_t seconds,
+    uint32_t nanoseconds,
+    uint32_t height,
+    uint32_t width,
+    const uint8_t *data) {
+  return WriteImage(seconds, nanoseconds, height, width, ComputeImage(height, width, data));
+}
+
+bool CarlaCameraPublisher::WriteImage(
+    int32_t seconds,
+    uint32_t nanoseconds,
+    uint32_t height,
+    uint32_t width,
+    std::vector<uint8_t> data) {
+  auto *message = _impl_image->GetMessage();
+  message->header().stamp().sec(seconds);
+  message->header().stamp().nanosec(nanoseconds);
+  message->header().frame_id(GetFrameId());
+
+  message->width(width);
+  message->height(height);
+  message->encoding(GetEncoding());
+  message->is_bigendian(0);
+  message->step(width * GetChannels() * static_cast<uint32_t>(sizeof(uint8_t)));
+  // https://github.com/eProsima/Fast-DDS/issues/2330
+  message->data(std::move(data));
+
+  return true;
+}
+
+}  // namespace ros2
+}  // namespace carla

LibCarla/source/carla/ros2/publishers/CarlaCameraPublisher.cpp

@@ -0,0 +1,76 @@
+// Copyright (c) 2026 Computer Vision Center (CVC) at the Universitat Autonoma
+// de Barcelona (UAB).
+//
+// This work is licensed under the terms of the MIT license.
+// For a copy, see <https://opensource.org/licenses/MIT>.
+
+#pragma once
+
+#include "carla/ros2/publishers/BasePublisher.h"
+
+#include <cstdint>
+#include <memory>
+#include <string>
+#include <vector>
+
+namespace carla {
+namespace ros2 {
+
+// Forward declarations keep the FastDDS-heavy PublisherImpl<> definition out of
+// the main carla-server compile unit. The full template definition + Traits
+// instantiations live in CarlaCameraPublisher.cpp, which is built by the
+// carla-ros2-native ExternalProject (where FastDDS headers are on the include
+// path). Same pattern as AckermannControlSubscriber.{h,cpp} from PR-2.
+template <typename Traits> class PublisherImpl;
+struct CarlaCameraImageMsgTraits;
+struct CarlaCameraInfoMsgTraits;
+
+class CarlaCameraPublisher : public BasePublisher {
+public:
+  CarlaCameraPublisher(std::string base_topic_name, std::string frame_id);
+  ~CarlaCameraPublisher() override;
+
+  CarlaCameraPublisher(const CarlaCameraPublisher &) = delete;
+  CarlaCameraPublisher &operator=(const CarlaCameraPublisher &) = delete;
+  CarlaCameraPublisher(CarlaCameraPublisher &&) noexcept = default;
+  CarlaCameraPublisher &operator=(CarlaCameraPublisher &&) noexcept = default;
+
+  bool Publish() override;
+
+  bool WriteCameraInfo(
+      int32_t seconds,
+      uint32_t nanoseconds,
+      uint32_t x_offset,
+      uint32_t y_offset,
+      uint32_t height,
+      uint32_t width,
+      float fov,
+      bool do_rectify);
+
+  bool WriteImage(
+      int32_t seconds,
+      uint32_t nanoseconds,
+      uint32_t height,
+      uint32_t width,
+      const uint8_t *data);
+
+protected:
+  [[nodiscard]] virtual uint8_t GetChannels() const = 0;
+  [[nodiscard]] virtual std::string GetEncoding() const = 0;
+  [[nodiscard]] virtual std::vector<uint8_t> ComputeImage(
+      uint32_t height, uint32_t width, const uint8_t *data) const;
+
+private:
+  bool WriteImage(
+      int32_t seconds,
+      uint32_t nanoseconds,
+      uint32_t height,
+      uint32_t width,
+      std::vector<uint8_t> data);
+
+  std::shared_ptr<PublisherImpl<CarlaCameraImageMsgTraits>> _impl_image;
+  std::shared_ptr<PublisherImpl<CarlaCameraInfoMsgTraits>> _impl_camera_info;
+};
+
+}  // namespace ros2
+}  // namespace carla