fix(ros2): correct degree-to-radian conversion in transform publisher#9639
fix(ros2): correct degree-to-radian conversion in transform publisher#9639youtalk wants to merge 11 commits intocarla-simulator:ue5-devfrom
Conversation
Add foundational CRTP template classes for the new ROS2 publisher and subscriber architecture, replacing the old per-sensor PIMPL pattern: - BasePublisher.h: DDS-independent abstract base (topic, frame_id, actor) - PublisherImpl.h: FastDDS CRTP template (DomainParticipant, Publisher, Topic, DataWriter, TypeSupport init/destroy/Publish) - BaseSubscriber.h: DDS-independent abstract base for subscribers - SubscriberImpl.h: FastDDS template (DataReader, on_data_available) These are added alongside the existing CarlaPublisher/CarlaSubscriber classes which will be removed in a later step. Signed-off-by: Yutaka Kondo <yutaka.kondo@youtalk.jp>
Introduce CarlaCameraPublisher as an abstract base class that manages Image and CameraInfo via two PublisherImpl<T> instances. Migrate CarlaRGBCameraPublisher to a thin subclass providing channel count and encoding. Replace CarlaDepthCameraPublisher, CarlaNormalsCameraPublisher, CarlaOpticalFlowCameraPublisher, CarlaSSCameraPublisher, and CarlaISCameraPublisher with type aliases to CarlaRGBCameraPublisher, eliminating ~2800 lines of duplicated boilerplate. Signed-off-by: Yutaka Kondo <yutaka.kondo@youtalk.jp>
Rewrite Clock, Collision, GNSS, IMU, Radar, Lidar, SemanticLidar, and Transform publishers to use BasePublisher + PublisherImpl<T> templates, replacing the old PIMPL + manual FastDDS boilerplate pattern. Add new shared base classes: - CarlaPointCloudPublisher: abstract base for point cloud publishers (LiDAR, SemanticLiDAR, DVS, Radar) - CarlaDVSPublisher: composite publisher (Image + PointCloud) replacing the old CarlaDVSCameraPublisher Delete unused publishers and old base classes: - CarlaLineInvasionPublisher (NoopSerializer, unused) - CarlaMapSensorPublisher (NoopSerializer, unused) - CarlaSpeedometerSensor (unused) - CarlaPublisher.h (replaced by BasePublisher.h) - CarlaSubscriber.h (replaced by BaseSubscriber.h) - CarlaDVSCameraPublisher (replaced by CarlaDVSPublisher) Signed-off-by: Yutaka Kondo <yutaka.kondo@youtalk.jp>
Delete CarlaListener and CarlaSubscriberListener as PublisherImpl and SubscriberImpl now directly inherit DataWriterListener/DataReaderListener. BasicListener is retained for the WITH_ROS2_DEMO path. Signed-off-by: Yutaka Kondo <yutaka.kondo@youtalk.jp>
Rewrite CarlaEgoVehicleControlSubscriber to use SubscriberImpl template, add new AckermannControlSubscriber with AckermannDrive/AckermannDriveStamped FastDDS types, and extend ROS2CallbackData variant with AckermannControl. Signed-off-by: Yutaka Kondo <yutaka.kondo@youtalk.jp>
Signed-off-by: Yutaka Kondo <yutaka.kondo@youtalk.jp>
Signed-off-by: Yutaka Kondo <yutaka.kondo@youtalk.jp>
Remove the 201d375 changes (actor prefix and unregister ordering) from the main refactor to keep them in a separate PR. Signed-off-by: Yutaka Kondo <yutaka.kondo@youtalk.jp>
Update ROS2 example files from ue4-dev reference: add GNSS and IMU sensors to stack.json configuration, update rviz config with new sensor frames and correct lidar topic path, sync ros2_native.py with upstream, and update README with launch instructions. Build system (CMakeLists.txt) uses dynamic file globs so no changes needed for renamed/added/removed source files. Signed-off-by: Yutaka Kondo <yutaka.kondo@youtalk.jp>
|
Thanks for opening this pull request! The maintainers of this repository would appreciate it if you would update our CHANGELOG.md based on your changes. |
- Move ROS2.cpp compilation from LibCarla server to Ros2Native shared library, since publisher headers now include PublisherImpl.h which depends on FastDDS headers - Add rpclib include path to Ros2Native build for MsgPack.h dependency - Make BasicPublisher/BasicSubscriber standalone (no longer inherit from deleted CarlaPublisher/CarlaSubscriber base classes) - Replace CarlaListener usage in BasicPublisher with inline DataWriterListener Signed-off-by: Yutaka Kondo <yutaka.kondo@youtalk.jp>
The rotation conversion was incorrectly using M_PI_2 (pi/2) instead of M_PI (pi) in the degree-to-radian formula, producing angles that were half the correct value. Also negate the y-axis translation at the source instead of after quaternion computation for consistency with ROS coordinate conventions. Signed-off-by: Yutaka Kondo <yutaka.kondo@youtalk.jp>
afb6c6b to
fefae90
Compare
There was a problem hiding this comment.
Pull request overview
Ports the ROS2 transform conversion fix to ue5-dev while also aligning UE-side sensor publishing and LibCarla ROS2 native code with the new BasePublisher/BaseSubscriber template architecture (incl. Ackermann control support and updated examples).
Changes:
- Fixes TF transform conversion math (deg→rad) and ROS coordinate sign conventions in the transform publisher path.
- Refactors ROS2 actor/sensor/vehicle registration and sensor data publishing calls to the new API (removing stream_id usage).
- Adds Ackermann control message types/subscriber and updates ROS2 native example configs (stack/rviz/script/docs).
Reviewed changes
Copilot reviewed 92 out of 92 changed files in this pull request and generated 9 comments.
Show a summary per file
| File | Description |
|---|---|
| Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Server/CarlaServer.cpp | Switches parent registration call to RegisterActorParent. |
| Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Sensor/RayCastSemanticLidar.cpp | Removes stream_id; passes computed relative/world transform into ROS2 publisher. |
| Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Sensor/RayCastLidar.cpp | Removes stream_id; passes computed relative/world transform into ROS2 publisher. |
| Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Sensor/Radar.cpp | Removes stream_id; passes computed relative/world transform into ROS2 publisher. |
| Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Sensor/PixelReader.h | Updates ROS2 camera publish path; now publishes from serialized buffer header. |
| Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Sensor/ObstacleDetectionSensor.cpp | Removes stream_id; passes computed relative/world transform into ROS2 publisher. |
| Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Sensor/InertialMeasurementUnit.cpp | Removes stream_id; passes computed relative/world transform into ROS2 publisher. |
| Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Sensor/GnssSensor.cpp | Removes stream_id; passes computed relative/world transform into ROS2 publisher. |
| Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Sensor/DVSCamera.cpp | Updates ROS2 DVS publish signature to new API. |
| Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Sensor/CollisionSensor.cpp | Publishes collision using Carla actor id (when available) and new ROS2 API. |
| Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Actor/ActorROS2Handler.h | Adds Ackermann control handler overload. |
| Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Actor/ActorROS2Handler.cpp | Implements Ackermann control application to vehicles. |
| Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Actor/ActorDispatcher.cpp | Migrates ROS2 actor registration to new RegisterSensor/RegisterVehicle APIs. |
| Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Actor/ActorDescription.h | Adds convenience GetAttribute accessor for variations map. |
| Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Actor/ActorBlueprintFunctionLibrary.cpp | Adds ros_frame_id and ros_publish_tf attributes; changes defaults. |
| Ros2Native/LibCarlaRos2Native/CMakeLists.txt | Ensures core ros2 *.cpp are built and adds rpclib include path. |
| Ros2Native/CMakeLists.txt | Plumbs RPCLIB_INCLUDE_PATH into Ros2Native external build. |
| PythonAPI/examples/ros2/stack.json | Updates blueprint id and adds GNSS/IMU sensors; adjusts camera resolution. |
| PythonAPI/examples/ros2/rviz/ros2_native.rviz | Adds GNSS/IMU frames and updates lidar topic naming. |
| PythonAPI/examples/ros2/ros2_native.py | Improves egg import path handling; tweaks timeouts; whitespace cleanup. |
| PythonAPI/examples/ros2/README.md | Updates ROS2 native example launch instructions. |
| LibCarla/source/carla/ros2/types/AckermannDriveStampedPubSubTypes.h | Adds generated FastDDS type support for AckermannDriveStamped. |
| LibCarla/source/carla/ros2/types/AckermannDriveStampedPubSubTypes.cpp | Adds generated FastDDS serialization for AckermannDriveStamped. |
| LibCarla/source/carla/ros2/types/AckermannDriveStamped.h | Adds generated AckermannDriveStamped message type. |
| LibCarla/source/carla/ros2/types/AckermannDriveStamped.cpp | Adds generated AckermannDriveStamped implementation. |
| LibCarla/source/carla/ros2/types/AckermannDrivePubSubTypes.h | Adds generated FastDDS type support for AckermannDrive. |
| LibCarla/source/carla/ros2/types/AckermannDrivePubSubTypes.cpp | Adds generated FastDDS serialization for AckermannDrive. |
| LibCarla/source/carla/ros2/types/AckermannDrive.h | Adds generated AckermannDrive message type. |
| LibCarla/source/carla/ros2/types/AckermannDrive.cpp | Adds generated AckermannDrive implementation. |
| LibCarla/source/carla/ros2/subscribers/SubscriberImpl.h | Introduces templated DDS subscriber implementation. |
| LibCarla/source/carla/ros2/subscribers/CarlaSubscriber.h | Removes legacy subscriber base class. |
| LibCarla/source/carla/ros2/subscribers/CarlaEgoVehicleControlSubscriber.h | Migrates ego control subscriber to BaseSubscriber/SubscriberImpl. |
| LibCarla/source/carla/ros2/subscribers/CarlaEgoVehicleControlSubscriber.cpp | Replaces bespoke DDS code with SubscriberImpl polling path. |
| LibCarla/source/carla/ros2/subscribers/BasicSubscriber.h | Removes inheritance from deleted CarlaSubscriber; inlines fields. |
| LibCarla/source/carla/ros2/subscribers/BaseSubscriber.h | Adds new BaseSubscriber abstraction for subscriber types. |
| LibCarla/source/carla/ros2/subscribers/AckermannControlSubscriber.h | Adds Ackermann control subscriber wrapper. |
| LibCarla/source/carla/ros2/subscribers/AckermannControlSubscriber.cpp | Implements Ackermann message→control conversion. |
| LibCarla/source/carla/ros2/ROS2CallbackData.h | Extends callback variant to include AckermannControl. |
| LibCarla/source/carla/ros2/ROS2.h | Reworks ROS2 API for actor registration and data ingestion without stream_id. |
| LibCarla/source/carla/ros2/publishers/PublisherImpl.h | Introduces templated DDS publisher implementation. |
| LibCarla/source/carla/ros2/publishers/CarlaTransformPublisher.h | Migrates TF publisher to BasePublisher/PublisherImpl design. |
| LibCarla/source/carla/ros2/publishers/CarlaSSCameraPublisher.h | Simplifies SS camera publisher to alias RGB camera publisher. |
| LibCarla/source/carla/ros2/publishers/CarlaSpeedometerSensor.h | Removes legacy speedometer publisher. |
| LibCarla/source/carla/ros2/publishers/CarlaSpeedometerSensor.cpp | Removes legacy speedometer publisher implementation. |
| LibCarla/source/carla/ros2/publishers/CarlaSemanticLidarPublisher.h | Migrates semantic lidar publisher to PointCloud publisher base. |
| LibCarla/source/carla/ros2/publishers/CarlaRGBCameraPublisher.h | Migrates RGB camera publisher to new Camera publisher base. |
| LibCarla/source/carla/ros2/publishers/CarlaRadarPublisher.h | Migrates radar publisher to PointCloud publisher base. |
| LibCarla/source/carla/ros2/publishers/CarlaPublisher.h | Removes legacy publisher base class. |
| LibCarla/source/carla/ros2/publishers/CarlaPointCloudPublisher.h | Adds shared PointCloud2 publisher base. |
| LibCarla/source/carla/ros2/publishers/CarlaPointCloudPublisher.cpp | Implements common PointCloud2 message population logic. |
| LibCarla/source/carla/ros2/publishers/CarlaOpticalFlowCameraPublisher.h | Simplifies optical flow publisher to alias RGB publisher. |
| LibCarla/source/carla/ros2/publishers/CarlaNormalsCameraPublisher.h | Simplifies normals publisher to alias RGB publisher. |
| LibCarla/source/carla/ros2/publishers/CarlaMapSensorPublisher.h | Removes legacy map publisher. |
| LibCarla/source/carla/ros2/publishers/CarlaMapSensorPublisher.cpp | Removes legacy map publisher implementation. |
| LibCarla/source/carla/ros2/publishers/CarlaLineInvasionPublisher.h | Removes legacy lane invasion publisher. |
| LibCarla/source/carla/ros2/publishers/CarlaLineInvasionPublisher.cpp | Removes legacy lane invasion publisher implementation. |
| LibCarla/source/carla/ros2/publishers/CarlaLidarPublisher.h | Migrates lidar publisher to PointCloud publisher base. |
| LibCarla/source/carla/ros2/publishers/CarlaLidarPublisher.cpp | Reimplements lidar point cloud packing/conversion. |
| LibCarla/source/carla/ros2/publishers/CarlaISCameraPublisher.h | Simplifies IS camera publisher to alias RGB publisher. |
| LibCarla/source/carla/ros2/publishers/CarlaIMUPublisher.h | Migrates IMU publisher to BasePublisher/PublisherImpl. |
| LibCarla/source/carla/ros2/publishers/CarlaIMUPublisher.cpp | Reimplements IMU message population under new architecture. |
| LibCarla/source/carla/ros2/publishers/CarlaGNSSPublisher.h | Migrates GNSS publisher to BasePublisher/PublisherImpl. |
| LibCarla/source/carla/ros2/publishers/CarlaGNSSPublisher.cpp | Reimplements GNSS message population under new architecture. |
| LibCarla/source/carla/ros2/publishers/CarlaDVSPublisher.h | Introduces combined DVS image + point cloud publisher. |
| LibCarla/source/carla/ros2/publishers/CarlaDVSPublisher.cpp | Implements DVS point cloud fields/packing. |
| LibCarla/source/carla/ros2/publishers/CarlaDVSCameraPublisher.h | Removes legacy DVS publisher class. |
| LibCarla/source/carla/ros2/publishers/CarlaDepthCameraPublisher.h | Simplifies depth publisher to alias RGB publisher. |
| LibCarla/source/carla/ros2/publishers/CarlaCollisionPublisher.h | Migrates collision publisher to BasePublisher/PublisherImpl. |
| LibCarla/source/carla/ros2/publishers/CarlaCollisionPublisher.cpp | Reimplements collision message population under new architecture. |
| LibCarla/source/carla/ros2/publishers/CarlaClockPublisher.h | Migrates clock publisher to BasePublisher/PublisherImpl. |
| LibCarla/source/carla/ros2/publishers/CarlaClockPublisher.cpp | Reimplements clock message population under new architecture. |
| LibCarla/source/carla/ros2/publishers/CarlaCameraPublisher.h | Adds shared camera (Image + CameraInfo) publisher base. |
| LibCarla/source/carla/ros2/publishers/CarlaCameraPublisher.cpp | Implements common camera info + image population logic. |
| LibCarla/source/carla/ros2/publishers/BasicPublisher.h | Removes inheritance from deleted CarlaPublisher; inlines fields. |
| LibCarla/source/carla/ros2/publishers/BasicPublisher.cpp | Replaces listener wrapper with local DataWriterListener impl. |
| LibCarla/source/carla/ros2/publishers/BasePublisher.h | Adds new BasePublisher abstraction. |
| LibCarla/source/carla/ros2/listeners/CarlaSubscriberListener.h | Removes legacy subscriber listener wrapper. |
| LibCarla/source/carla/ros2/listeners/CarlaSubscriberListener.cpp | Removes legacy subscriber listener wrapper implementation. |
| LibCarla/source/carla/ros2/listeners/CarlaListener.h | Removes legacy publisher listener wrapper. |
| LibCarla/source/carla/ros2/listeners/CarlaListener.cpp | Removes legacy publisher listener wrapper implementation. |
| LibCarla/CMakeLists.txt | Stops compiling ROS2.cpp into LibCarla server; moves it to Ros2Native. |
💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.
| ROS2->RegisterVehicle(static_cast<void*>(&Actor), RosName, RosName, [RosName](void *Actor, carla::ros2::ROS2CallbackData Data) -> void | ||
| { | ||
| AActor *UEActor = reinterpret_cast<AActor *>(Actor); | ||
| ActorROS2Handler Handler(UEActor, RosName); | ||
| std::visit(Handler, Data); | ||
| boost::variant2::visit(Handler, Data); | ||
| }); |
There was a problem hiding this comment.
ROS2CallbackData is defined as std::variant (see LibCarla/source/carla/ros2/ROS2CallbackData.h), but this code calls boost::variant2::visit. This will not compile (and #include <variant> was removed in the same hunk). Use std::visit here, or change ROS2CallbackData to a Boost variant consistently across the codebase.
| FActorAttribute GetAttribute(const FString Name) const { | ||
| if (Variations.Contains(Name)) | ||
| { | ||
| return Variations[Name]; | ||
| } | ||
| else | ||
| { | ||
| return FActorAttribute(); | ||
| } |
There was a problem hiding this comment.
GetAttribute is const but uses Variations[Name]. In Unreal's TMap, operator[] is non-const and may not compile in a const method; even if it compiles, it’s easy to accidentally introduce insertion in the future. Prefer if (const FActorAttribute* Attr = Variations.Find(Name)) return *Attr; (or FindRef) and take Name by const FString& to avoid copying.
|
|
||
| const double cx = static_cast<double>(width) / 2.0; | ||
| const double cy = static_cast<double>(height) / 2.0; | ||
| const double fx = static_cast<double>(width) / (2.0 * std::tan(fov) * M_PI / 360.0); |
There was a problem hiding this comment.
Camera intrinsics computation uses std::tan(fov) where fov appears to be in degrees (it comes from ImageSerializer::ImageHeader::fov_angle). This yields incorrect fx/fy in CameraInfo. Convert to radians before calling tan, e.g., tan(fov * M_PI / 360.0) (or compute a fov_rad = fov * M_PI / 180.0).
| const double fx = static_cast<double>(width) / (2.0 * std::tan(fov) * M_PI / 360.0); | |
| const double half_fov_rad = static_cast<double>(fov) * M_PI / 360.0; | |
| const double fx = static_cast<double>(width) / (2.0 * std::tan(half_fov_rad)); |
| descriptor3.name("pol"); | ||
| descriptor3.offset(12); | ||
| descriptor3.datatype(sensor_msgs::msg::PointField__INT8); | ||
| descriptor3.count(1); |
There was a problem hiding this comment.
GetFields() mistakenly assigns the "pol" field metadata into descriptor3 instead of descriptor4, so the returned field list contains a corrupted "t" field and an uninitialized descriptor4. Assign name/offset/datatype/count to descriptor4 for the polarity field.
| descriptor3.name("pol"); | |
| descriptor3.offset(12); | |
| descriptor3.datatype(sensor_msgs::msg::PointField__INT8); | |
| descriptor3.count(1); | |
| descriptor4.name("pol"); | |
| descriptor4.offset(12); | |
| descriptor4.datatype(sensor_msgs::msg::PointField__INT8); | |
| descriptor4.count(1); |
| namespace carla { | ||
| namespace ros2 { | ||
|
|
||
| const size_t CarlaDVSPointCloudPublisher::GetPointSize() { | ||
| return sizeof(sensor::data::DVSEvent); | ||
| } | ||
|
|
||
| std::vector<sensor_msgs::msg::PointField> CarlaDVSPointCloudPublisher::GetFields() { | ||
|
|
||
| sensor_msgs::msg::PointField descriptor1; | ||
| descriptor1.name("x"); | ||
| descriptor1.offset(0); | ||
| descriptor1.datatype(sensor_msgs::msg::PointField__UINT16); | ||
| descriptor1.count(1); | ||
| sensor_msgs::msg::PointField descriptor2; | ||
| descriptor2.name("y"); | ||
| descriptor2.offset(2); | ||
| descriptor2.datatype(sensor_msgs::msg::PointField__UINT16); | ||
| descriptor2.count(1); | ||
| sensor_msgs::msg::PointField descriptor3; | ||
| descriptor3.name("t"); | ||
| descriptor3.offset(4); | ||
| descriptor3.datatype(sensor_msgs::msg::PointField__FLOAT64); | ||
| descriptor3.count(1); | ||
| sensor_msgs::msg::PointField descriptor4; | ||
| descriptor3.name("pol"); | ||
| descriptor3.offset(12); | ||
| descriptor3.datatype(sensor_msgs::msg::PointField__INT8); | ||
| descriptor3.count(1); | ||
|
|
||
| return {descriptor1, descriptor2, descriptor3, descriptor4}; | ||
| } | ||
|
|
||
| std::vector<uint8_t> CarlaDVSPointCloudPublisher::ComputePointCloud(uint32_t height, uint32_t width, uint8_t *data) { | ||
|
|
||
| sensor::data::DVSEvent* events = reinterpret_cast<sensor::data::DVSEvent*>(data); | ||
| const size_t total_points = height * width; | ||
| for (size_t i = 0; i < total_points; ++i) { | ||
| events[i].y *= -1.0f; | ||
| } | ||
|
|
||
| const size_t total_bytes = total_points * sizeof(sensor::data::DVSEvent); | ||
| std::vector<uint8_t> vector_data(reinterpret_cast<uint8_t*>(events), | ||
| reinterpret_cast<uint8_t*>(events) + total_bytes); |
There was a problem hiding this comment.
DVSEvent::y is a uint16_t (see carla/sensor/data/DVSEvent.h), but this code multiplies it by -1.0f. That will wrap/overflow and produce invalid coordinates in the point cloud. If the intent is ROS coordinate conversion, apply the sign change on a signed/float representation (e.g., store to an int32/float field in the outgoing point cloud buffer) rather than mutating the packed DVSEvent in-place.
| namespace carla { | |
| namespace ros2 { | |
| const size_t CarlaDVSPointCloudPublisher::GetPointSize() { | |
| return sizeof(sensor::data::DVSEvent); | |
| } | |
| std::vector<sensor_msgs::msg::PointField> CarlaDVSPointCloudPublisher::GetFields() { | |
| sensor_msgs::msg::PointField descriptor1; | |
| descriptor1.name("x"); | |
| descriptor1.offset(0); | |
| descriptor1.datatype(sensor_msgs::msg::PointField__UINT16); | |
| descriptor1.count(1); | |
| sensor_msgs::msg::PointField descriptor2; | |
| descriptor2.name("y"); | |
| descriptor2.offset(2); | |
| descriptor2.datatype(sensor_msgs::msg::PointField__UINT16); | |
| descriptor2.count(1); | |
| sensor_msgs::msg::PointField descriptor3; | |
| descriptor3.name("t"); | |
| descriptor3.offset(4); | |
| descriptor3.datatype(sensor_msgs::msg::PointField__FLOAT64); | |
| descriptor3.count(1); | |
| sensor_msgs::msg::PointField descriptor4; | |
| descriptor3.name("pol"); | |
| descriptor3.offset(12); | |
| descriptor3.datatype(sensor_msgs::msg::PointField__INT8); | |
| descriptor3.count(1); | |
| return {descriptor1, descriptor2, descriptor3, descriptor4}; | |
| } | |
| std::vector<uint8_t> CarlaDVSPointCloudPublisher::ComputePointCloud(uint32_t height, uint32_t width, uint8_t *data) { | |
| sensor::data::DVSEvent* events = reinterpret_cast<sensor::data::DVSEvent*>(data); | |
| const size_t total_points = height * width; | |
| for (size_t i = 0; i < total_points; ++i) { | |
| events[i].y *= -1.0f; | |
| } | |
| const size_t total_bytes = total_points * sizeof(sensor::data::DVSEvent); | |
| std::vector<uint8_t> vector_data(reinterpret_cast<uint8_t*>(events), | |
| reinterpret_cast<uint8_t*>(events) + total_bytes); | |
| #include <cstdint> | |
| #include <cstring> | |
| namespace carla { | |
| namespace ros2 { | |
| namespace { | |
| constexpr size_t kDVSPointXOffset = 0; | |
| constexpr size_t kDVSPointYOffset = 4; | |
| constexpr size_t kDVSPointTOffset = 8; | |
| constexpr size_t kDVSPointPolOffset = 16; | |
| constexpr size_t kDVSPointSize = 17; | |
| } // namespace | |
| const size_t CarlaDVSPointCloudPublisher::GetPointSize() { | |
| return kDVSPointSize; | |
| } | |
| std::vector<sensor_msgs::msg::PointField> CarlaDVSPointCloudPublisher::GetFields() { | |
| sensor_msgs::msg::PointField descriptor1; | |
| descriptor1.name("x"); | |
| descriptor1.offset(kDVSPointXOffset); | |
| descriptor1.datatype(sensor_msgs::msg::PointField__UINT16); | |
| descriptor1.count(1); | |
| sensor_msgs::msg::PointField descriptor2; | |
| descriptor2.name("y"); | |
| descriptor2.offset(kDVSPointYOffset); | |
| descriptor2.datatype(sensor_msgs::msg::PointField__INT32); | |
| descriptor2.count(1); | |
| sensor_msgs::msg::PointField descriptor3; | |
| descriptor3.name("t"); | |
| descriptor3.offset(kDVSPointTOffset); | |
| descriptor3.datatype(sensor_msgs::msg::PointField__FLOAT64); | |
| descriptor3.count(1); | |
| sensor_msgs::msg::PointField descriptor4; | |
| descriptor4.name("pol"); | |
| descriptor4.offset(kDVSPointPolOffset); | |
| descriptor4.datatype(sensor_msgs::msg::PointField__INT8); | |
| descriptor4.count(1); | |
| return {descriptor1, descriptor2, descriptor3, descriptor4}; | |
| } | |
| std::vector<uint8_t> CarlaDVSPointCloudPublisher::ComputePointCloud(uint32_t height, uint32_t width, uint8_t *data) { | |
| const sensor::data::DVSEvent* events = reinterpret_cast<const sensor::data::DVSEvent*>(data); | |
| const size_t total_points = height * width; | |
| std::vector<uint8_t> vector_data(total_points * kDVSPointSize); | |
| for (size_t i = 0; i < total_points; ++i) { | |
| uint8_t *point_data = vector_data.data() + (i * kDVSPointSize); | |
| const auto &event = events[i]; | |
| std::memcpy(point_data + kDVSPointXOffset, &event.x, sizeof(event.x)); | |
| const int32_t ros_y = -static_cast<int32_t>(event.y); | |
| std::memcpy(point_data + kDVSPointYOffset, &ros_y, sizeof(ros_y)); | |
| std::memcpy(point_data + kDVSPointTOffset, &event.t, sizeof(event.t)); | |
| std::memcpy(point_data + kDVSPointPolOffset, &event.pol, sizeof(event.pol)); | |
| } |
| ```bash | ||
| ./CarlaUnreal.sh --ros2 | ||
| # If running a package: | ||
| ./CarlaUE4sh --ros2 | ||
|
|
||
| # If running the editor: | ||
| make launch ARGS="--ros2 --editor-flags='--ros2'" | ||
| ``` |
There was a problem hiding this comment.
The launch command in the README references ./CarlaUE4sh --ros2, but the repo documentation consistently uses ./CarlaUnreal.sh --ros2 (e.g., Docs/ros2_native.md:8, Docs/build_linux_ue5.md:128). Update this example to the correct script name (and extension) so the instructions are runnable.
| auto Res = std::async(std::launch::async, [&Sensor, ROS2, &Stream, BufView]() | ||
| { | ||
| // get resolution of camera | ||
| int W = -1, H = -1; | ||
| float Fov = -1.0f; | ||
| auto WidthOpt = Sensor.GetAttribute("image_size_x"); | ||
| if (WidthOpt.has_value()) | ||
| W = FCString::Atoi(*WidthOpt->Value); | ||
| auto HeightOpt = Sensor.GetAttribute("image_size_y"); | ||
| if (HeightOpt.has_value()) | ||
| H = FCString::Atoi(*HeightOpt->Value); | ||
| auto FovOpt = Sensor.GetAttribute("fov"); | ||
| if (FovOpt.has_value()) | ||
| Fov = FCString::Atof(*FovOpt->Value); | ||
| // send data to ROS2 | ||
| AActor* ParentActor = Sensor.GetAttachParentActor(); | ||
| if (ParentActor) | ||
| { | ||
| FTransform LocalTransformRelativeToParent = Sensor.GetActorTransform().GetRelativeTransform(ParentActor->GetActorTransform()); | ||
| ROS2->ProcessDataFromCamera(Stream.GetSensorType(), StreamId, LocalTransformRelativeToParent, W, H, Fov, BufView, &Sensor); | ||
| } | ||
| else | ||
| { | ||
| ROS2->ProcessDataFromCamera(Stream.GetSensorType(), StreamId, Stream.GetSensorTransform(), W, H, Fov, BufView, &Sensor); | ||
| } | ||
| auto Transform = (ParentActor) ? Sensor.GetActorTransform().GetRelativeTransform(ParentActor->GetActorTransform()) : Stream.GetSensorTransform(); | ||
| ROS2->ProcessDataFromCamera(Stream.GetSensorType(), Transform, BufView, &Sensor); | ||
| }); |
There was a problem hiding this comment.
The async lambda captures Stream by reference (&Stream), but Stream is a local variable inside FuncForSending (created at line 145). If the async task runs after FuncForSending returns, this becomes a use-after-free. Capture the needed values by value before launching the async task (e.g., sensor_type / sensor_transform), or avoid std::async here.
| void on_data_available(efd::DataReader* reader) override { | ||
| efd::SampleInfo info; | ||
| msg_type message; | ||
|
|
||
| eprosima::fastrtps::types::ReturnCode_t rcode = reader->take_next_sample(&_message, &info); | ||
| if (rcode == eprosima::fastrtps::types::ReturnCode_t::ReturnCodeValue::RETCODE_OK) { | ||
| // TODO: Process messages directly. | ||
| _new_message = true; | ||
| } else { | ||
| log_error("SubscriberImpl::on_data_available (", this->GetTopicName(), ") failed with code:", rcode()); | ||
| } | ||
| } |
There was a problem hiding this comment.
on_data_available runs on a Fast DDS thread, while HasNewMessage()/GetMessage() are called from the game thread. _new_message and _message are accessed without any synchronization, which is a data race (undefined behavior) and can return torn/partially-written messages. Use a mutex, atomics + double-buffering, or a thread-safe queue to hand off messages between threads safely (and remove the unused local msg_type message).
| std::vector<uint8_t> CarlaLidarPublisher::ComputePointCloud(uint32_t height, uint32_t width, uint8_t *data) { | ||
|
|
||
| return *this; | ||
| } | ||
| sensor::data::LidarDetection* detections = reinterpret_cast<sensor::data::LidarDetection*>(data); | ||
|
|
||
| CarlaLidarPublisher::CarlaLidarPublisher(CarlaLidarPublisher&& other) { | ||
| _frame_id = std::move(other._frame_id); | ||
| _name = std::move(other._name); | ||
| _parent = std::move(other._parent); | ||
| _impl = std::move(other._impl); | ||
| const size_t total_points = height * width; | ||
| for (size_t i = 0; i < total_points; ++i) { | ||
| detections[i].point.y *= -1.0f; | ||
| } | ||
|
|
||
| CarlaLidarPublisher& CarlaLidarPublisher::operator=(CarlaLidarPublisher&& other) { | ||
| _frame_id = std::move(other._frame_id); | ||
| _name = std::move(other._name); | ||
| _parent = std::move(other._parent); | ||
| _impl = std::move(other._impl); | ||
|
|
||
| return *this; | ||
| } | ||
| } | ||
| const size_t total_bytes = total_points * sizeof(sensor::data::LidarDetection); | ||
| std::vector<uint8_t> vector_data(reinterpret_cast<uint8_t*>(detections), | ||
| reinterpret_cast<uint8_t*>(detections) + total_bytes); | ||
| return vector_data; |
There was a problem hiding this comment.
ComputePointCloud reinterprets the raw float buffer as sensor::data::LidarDetection* and mutates it. LidarDetection is a non-trivial class type and is not guaranteed to be layout-compatible with a float[4] buffer; this breaks strict-aliasing rules and can be undefined behavior. Prefer operating on the underlying float* (every 4th value is Y) and then copying into the output std::vector<uint8_t>.
Description
Port transform conversion bugfix from ue4-dev
a46b2690bto ue5-dev.Fixes incorrect degree-to-radian conversion in
CarlaTransformPublisher.cpp:M_PI_2 / 180.0f(pi/360, wrong) →M_PI / 180.0f(pi/180, correct)Depends on #9638
Diff from #9638: fefae90
Where has this been tested?
Possible Drawbacks
This change is