feat(ros2): [2/8] template infrastructure + Ackermann subscriber + types#9745
Conversation
|
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. |
There was a problem hiding this comment.
Pull request overview
This PR advances the ROS 2 refactor by introducing template-based publisher/subscriber plumbing, adding an Ackermann control subscriber + generated message types, and migrating the ego vehicle control subscriber onto the new base. It also extends the callback dispatch path so vehicle control can be routed via either VehicleControl or the new AckermannControl variant alternative.
Changes:
- Add reusable template infrastructure for FastDDS publishers/subscribers (
PublisherImpl<>,SubscriberImpl<>) and new lightweight base classes (BasePublisher,BaseSubscriber). - Add
AckermannControlSubscriber(plusAckermannControlConversion.h) and extendROS2CallbackData+ Unreal-side dispatch (ActorROS2Handler) to apply Ackermann control. - Refactor
ROS2registration to support multiple subscribers per actor, and add unit tests for IMU compass math + Ackermann conversion.
Reviewed changes
Copilot reviewed 29 out of 29 changed files in this pull request and generated 4 comments.
Show a summary per file
| File | Description |
|---|---|
| Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Actor/ActorROS2Handler.h | Adds AckermannControl overload for variant visitation. |
| Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Actor/ActorROS2Handler.cpp | Applies Ackermann control to ACarlaWheeledVehicle. |
| Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Actor/ActorDispatcher.cpp | Reorders deregistration around ROS2 cleanup on actor destruction. |
| LibCarla/source/test/common/test_imu_compass.cpp | Adds unit tests for OrientationFromCompass. |
| LibCarla/source/test/common/test_ackermann_conversion.cpp | Adds unit tests for Ackermann scalar-field conversion + variant dispatch. |
| LibCarla/source/carla/ros2/types/AckermannDrive.h | Adds generated FastDDS type for AckermannDrive. |
| LibCarla/source/carla/ros2/types/AckermannDrive.cpp | Adds generated FastDDS type implementation for AckermannDrive. |
| LibCarla/source/carla/ros2/types/AckermannDrivePubSubTypes.h | Adds generated FastDDS PubSub type for AckermannDrive. |
| LibCarla/source/carla/ros2/types/AckermannDrivePubSubTypes.cpp | Adds generated FastDDS PubSub type implementation for AckermannDrive. |
| LibCarla/source/carla/ros2/types/AckermannDriveStamped.h | Adds generated FastDDS type for AckermannDriveStamped. |
| LibCarla/source/carla/ros2/types/AckermannDriveStamped.cpp | Adds generated FastDDS type implementation for AckermannDriveStamped. |
| LibCarla/source/carla/ros2/types/AckermannDriveStampedPubSubTypes.h | Adds generated FastDDS PubSub type for AckermannDriveStamped. |
| LibCarla/source/carla/ros2/types/AckermannDriveStampedPubSubTypes.cpp | Adds generated FastDDS PubSub type implementation for AckermannDriveStamped. |
| LibCarla/source/carla/ros2/subscribers/SubscriberImpl.h | Introduces templated FastDDS DataReader listener implementation. |
| LibCarla/source/carla/ros2/subscribers/BaseSubscriber.h | Adds new base interface for per-actor subscribers. |
| LibCarla/source/carla/ros2/subscribers/CarlaEgoVehicleControlSubscriber.h | Migrates ego vehicle control subscriber to BaseSubscriber + SubscriberImpl<>. |
| LibCarla/source/carla/ros2/subscribers/CarlaEgoVehicleControlSubscriber.cpp | Implements new template-based ego vehicle control subscriber. |
| LibCarla/source/carla/ros2/subscribers/AckermannControlSubscriber.h | Adds Ackermann control subscriber interface. |
| LibCarla/source/carla/ros2/subscribers/AckermannControlSubscriber.cpp | Implements Ackermann control subscriber using generated types + conversion seam. |
| LibCarla/source/carla/ros2/subscribers/AckermannControlConversion.h | Adds header-only conversion seam for unit testing. |
| LibCarla/source/carla/ros2/ROS2CallbackData.h | Extends callback variant with AckermannControl. |
| LibCarla/source/carla/ros2/ROS2.h | Adds Register/Unregister APIs and replaces single controller with per-actor subscriber multimap. |
| LibCarla/source/carla/ros2/ROS2.cpp | Implements new registration APIs and per-frame dispatch across multiple subscribers. |
| LibCarla/source/carla/ros2/publishers/BasePublisher.h | Adds new base interface for publishers. |
| LibCarla/source/carla/ros2/publishers/PublisherImpl.h | Adds templated FastDDS DataWriter listener implementation. |
| LibCarla/source/carla/ros2/publishers/ImuMath.h | Extracts compass->yaw quaternion math into a testable helper. |
| LibCarla/source/carla/ros2/publishers/CarlaIMUPublisher.cpp | Uses OrientationFromCompass helper for IMU orientation. |
| LibCarla/source/carla/ros2/listeners/CarlaSubscriberListener.h | Deletes legacy subscriber listener trampoline. |
| LibCarla/source/carla/ros2/listeners/CarlaSubscriberListener.cpp | Deletes legacy subscriber listener trampoline implementation. |
Comments suppressed due to low confidence (2)
LibCarla/source/carla/ros2/subscribers/SubscriberImpl.h:134
GetMessage()clears_new_messageand returns_messageby value without any synchronization. Withon_data_available()potentially writing_messageconcurrently, this can return torn/partially-updated data. Guard the read+reset with the same synchronization used in the listener (e.g., lock around copy + flag reset, or swap an optional/message queue).
msg_type GetMessage() {
_new_message = false;
return _message;
}
[[nodiscard]] bool HasNewMessage() const noexcept { return _new_message; }
LibCarla/source/carla/ros2/ROS2.cpp:240
RegisterVehicleunconditionally inserts two subscribers into_subscribersfor the actor. If the actor is re-registered (or if cleanup is missed on destruction), this will accumulate duplicate subscribers/DataReaders for the same actor/topic. Consider erasing existing_subscribersentries for the actor before inserting new ones, or checking whether the expected subscribers already exist.
const std::string base_topic_name = "rt/carla/" + ros_name;
_subscribers.insert({actor, std::make_shared<CarlaEgoVehicleControlSubscriber>(actor, base_topic_name, frame_id)});
_subscribers.insert({actor, std::make_shared<AckermannControlSubscriber>(actor, base_topic_name, frame_id)});
💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.
Threading and lifecycle fixes for the template infrastructure landed on top of a1f3b52: * SubscriberImpl: protect _message with a std::mutex; promote _alive and _new_message to std::atomic<bool>; ignore samples with info.valid_data == false in on_data_available so dispose/unregister notifications no longer flip _new_message. * PublisherImpl: promote _alive to std::atomic<bool>. * ROS2::RegisterVehicle: erase the actor's existing _subscribers range before inserting, and use insert_or_assign for _actor_callbacks so re-registration is idempotent. Same insert_or_assign fix for AddBasicSubscriberCallback. * UActorDispatcher::OnActorDestroyed: call RemoveActorCallback and (in the WITH_ROS2_DEMO build) RemoveBasicSubscriberCallback before RemoveActorRosName, so per-actor subscribers and callbacks are torn down with the actor and SetFrame never dispatches to a dangling pointer.
Follow-up to 7e5d2d2 that closes the remaining races and semantics bugs surfaced by Copilot's second pass: * SubscriberImpl::GetMessage: clear _new_message under _message_mutex so on_data_available cannot drop a pending sample by racing the flag-clear against its own store(true). Same fix applied to the on_data_available side: the lock now spans both the _message write and the flag store. * SubscriberImpl::on_subscription_matched and PublisherImpl::on_publication_matched: derive _alive from info.current_count > 0, not info.total_count > 0. total_count is cumulative and never decrements, so the previous code latched IsAlive() to true forever after the first match. * ROS2::AddActorRosName: insert_or_assign so a re-registered actor actually picks up its new ros_name. The previous unordered_map ::insert silently kept the stale entry.
|
Ready for review. |
Threading and lifecycle fixes for the template infrastructure landed on top of a1f3b52: * SubscriberImpl: protect _message with a std::mutex; promote _alive and _new_message to std::atomic<bool>; ignore samples with info.valid_data == false in on_data_available so dispose/unregister notifications no longer flip _new_message. * PublisherImpl: promote _alive to std::atomic<bool>. * ROS2::RegisterVehicle: erase the actor's existing _subscribers range before inserting, and use insert_or_assign for _actor_callbacks so re-registration is idempotent. Same insert_or_assign fix for AddBasicSubscriberCallback. * UActorDispatcher::OnActorDestroyed: call RemoveActorCallback and (in the WITH_ROS2_DEMO build) RemoveBasicSubscriberCallback before RemoveActorRosName, so per-actor subscribers and callbacks are torn down with the actor and SetFrame never dispatches to a dangling pointer.
Follow-up to 7e5d2d2 that closes the remaining races and semantics bugs surfaced by Copilot's second pass: * SubscriberImpl::GetMessage: clear _new_message under _message_mutex so on_data_available cannot drop a pending sample by racing the flag-clear against its own store(true). Same fix applied to the on_data_available side: the lock now spans both the _message write and the flag store. * SubscriberImpl::on_subscription_matched and PublisherImpl::on_publication_matched: derive _alive from info.current_count > 0, not info.total_count > 0. total_count is cumulative and never decrements, so the previous code latched IsAlive() to true forever after the first match. * ROS2::AddActorRosName: insert_or_assign so a re-registered actor actually picks up its new ros_name. The previous unordered_map ::insert silently kept the stale entry.
bd5ca4c to
656ec1a
Compare
…actor deregistration Two small ROS 2 fixes on ue5-dev plus GTest coverage for the IMU math: * CarlaIMUPublisher::SetData computed yaw as (float(M_PI_2) / 2.0f) - compass, which equals pi/4 - compass and reported the IMU orientation rotated 45 degrees from the intended pi/2 - compass conversion of magnetic heading to REP-103 yaw. Extract the math into a header-only helper carla::ros2::OrientationFromCompass so the GTest can exercise it without a live FastDDS publisher. The helper uses static_cast<float>(M_PI) to stay compatible with the carla-ros2-native ExternalProject, which configures C++17 and therefore cannot include <numbers>. * UActorDispatcher::OnActorDestroyed now runs the ROS 2 unregister callback before the actor registry's Deregister(), and collapses the nested if (CarlaActor) / if (IsActive()) into a single predicate. Today on ue5-dev this is observably a no-op because RemoveActorRosName only consumes the raw AActor pointer; the reorder prepares for the upcoming UnregisterSensor / UnregisterVehicle API, which reads FCarlaActor::GetActorInfo()->Description and needs the actor alive at unregister time. Adds LibCarla/source/test/common/test_imu_compass.cpp with 5 GTest cases covering north, east, south, west, and a regression assertion against the previous pi/4 - compass spelling. Behavior change: clients that compensated for the previous 45-degree IMU yaw offset will see a corrected quaternion. (adapted from ue4-dev 201d375) Co-Authored-By: joel-mb <joel.moriana@gmail.com>
Use a 600x400 RGB camera in the ROS 2 native example stack so the published image has enough resolution for a useful preview in rviz.
…ubscriber, and dual registration API Lands the infrastructure half of upstream commit cd3d640 (ROS2 Refactor carla-simulator#9097) on ue5-dev: * BasePublisher / PublisherImpl<T> and BaseSubscriber / SubscriberImpl<T> template wrappers under LibCarla/source/carla/ros2/. The concrete publisher and subscriber headers forward-declare SubscriberImpl<> and the per-type traits struct so the FastDDS-heavy template definition only surfaces inside the carla-ros2-native ExternalProject build, keeping main carla-server (where FastDDS is not on the include path) compileable. * AckermannControlSubscriber.{h,cpp} consuming the new template, with the scalar-field-to-CARLA-struct mapping extracted into a header-only seam AckermannControlConversion.h so the math is unit-testable from libcarla_test_server / libcarla_test_client without FastDDS. * 8 FastDDS-generated Ackermann message-type files (AckermannDrive[.PubSubTypes][Stamped]) under types/, matching the wire format of the 58 message families ue5-dev already ships. * ROS2::RegisterSensor / UnregisterSensor / RegisterVehicle / UnregisterVehicle public API additions alongside the existing AddActorRosName / RemoveActorRosName / AddActorCallback / RemoveActorCallback. AddActorCallback now delegates to RegisterVehicle so vehicles wired through the legacy entry point receive the new Ackermann subscriber automatically. The dual-API design lets PR-3 (camera publishers) and PR-4 (point-cloud + scalar publishers + legacy listener removal) migrate sensor cohorts one at a time over the new entry point without breaking the chain at any commit. * CarlaEgoVehicleControlSubscriber migrated to BaseSubscriber + SubscriberImpl<> as the proof-of-concept; no behavioral change for ROS 2 vehicle command consumers (topic name and field mapping unchanged). * ROS2CallbackData std::variant extended with AckermannControl; matching ActorROS2Handler operator() overload dispatches Ackermann commands into the existing ACarlaWheeledVehicle::ApplyVehicleAckermannControl path. * Orphaned listeners/CarlaSubscriberListener.{h,cpp} deleted. Its only caller was the now-migrated CarlaEgoVehicleControlSubscriber. Also folds in the BasePublisher / BaseSubscriber virtual-destructor + cast cleanup from 2605780. Test coverage: LibCarla/source/test/common/test_ackermann_conversion.cpp exercises the conversion seam (5 cases) and the std::variant routing (2 cases). Both libcarla_test_server (80/80) and libcarla_test_client (94/94) pass with the new cases green. (adapted from ue4-dev cd3d640) Co-Authored-By: joel-mb <joel.moriana@gmail.com>
Threading and lifecycle fixes for the template infrastructure landed on top of a1f3b52: * SubscriberImpl: protect _message with a std::mutex; promote _alive and _new_message to std::atomic<bool>; ignore samples with info.valid_data == false in on_data_available so dispose/unregister notifications no longer flip _new_message. * PublisherImpl: promote _alive to std::atomic<bool>. * ROS2::RegisterVehicle: erase the actor's existing _subscribers range before inserting, and use insert_or_assign for _actor_callbacks so re-registration is idempotent. Same insert_or_assign fix for AddBasicSubscriberCallback. * UActorDispatcher::OnActorDestroyed: call RemoveActorCallback and (in the WITH_ROS2_DEMO build) RemoveBasicSubscriberCallback before RemoveActorRosName, so per-actor subscribers and callbacks are torn down with the actor and SetFrame never dispatches to a dangling pointer.
Follow-up to 7e5d2d2 that closes the remaining races and semantics bugs surfaced by Copilot's second pass: * SubscriberImpl::GetMessage: clear _new_message under _message_mutex so on_data_available cannot drop a pending sample by racing the flag-clear against its own store(true). Same fix applied to the on_data_available side: the lock now spans both the _message write and the flag store. * SubscriberImpl::on_subscription_matched and PublisherImpl::on_publication_matched: derive _alive from info.current_count > 0, not info.total_count > 0. total_count is cumulative and never decrements, so the previous code latched IsAlive() to true forever after the first match. * ROS2::AddActorRosName: insert_or_assign so a re-registered actor actually picks up its new ros_name. The previous unordered_map ::insert silently kept the stale entry.
656ec1a to
9fe297a
Compare
Description
Introduces the template-based publisher/subscriber infrastructure that PR-3 and PR-4 will use to migrate the existing ROS 2 publishers cohort by cohort, lands the missing
AckermannControlSubscriber(with its 8 FastDDS-generated message-type files), and migratesCarlaEgoVehicleControlSubscriberto the new base as the proof-of-concept.ROS2::RegisterSensor/UnregisterSensor/RegisterVehicle/UnregisterVehicleare added alongside the existingAddActorRosName/RemoveActorRosName/AddActorCallback/RemoveActorCallbackso the chain can land without breaking publishers that have not yet migrated. The legacyAddActorCallbacknow delegates toRegisterVehicle, which means vehicles wired through the old entry point also receive the new Ackermann subscriber automatically.The scalar-field-to-CARLA-struct conversion for the Ackermann message is extracted into a small header-only seam (
AckermannControlConversion.h) so the numeric mapping is unit-testable fromlibcarla_test_server/libcarla_test_clientwithout linking FastDDS. The Pimpl-via-forward-declaration pattern on the two concrete subscribers keeps<fastdds/...>includes inside thecarla-ros2-nativeExternalProject and out of the maincarla-servercompile unit, where FastDDS is not on the include path.The orphaned
CarlaSubscriberListener.{h,cpp}is deleted because the migratedCarlaEgoVehicleControlSubscriberno longer drives the legacy listener trampoline and no other code references it.Closes: #9627
Changes
LibCarla/source/carla/ros2/:publishers/BasePublisher.h,publishers/PublisherImpl.h,subscribers/BaseSubscriber.h,subscribers/SubscriberImpl.h.PublisherImpl<>/SubscriberImpl<>inherit FastDDS'sDataWriterListener/DataReaderListenerso concrete publishers and subscribers no longer carry per-type listener trampolines.subscribers/AckermannControlSubscriber.{h,cpp}consuming the templates, plus the header-onlysubscribers/AckermannControlConversion.hseam used by the GTest.types/AckermannDrive*.{h,cpp}andtypes/AckermannDriveStamped*.{h,cpp}files, matching the wire format of the 58 message-type families already onue5-dev.subscribers/CarlaEgoVehicleControlSubscriber.{h,cpp}migrated toBaseSubscriber+SubscriberImpl<CarlaEgoVehicleControlTraits>as the proof-of-concept. No behavioral change for ROS 2 vehicle command consumers; the topic name and field mapping are unchanged.ROS2.{h,cpp}gainsRegisterSensor/UnregisterSensor/RegisterVehicle/UnregisterVehicle._subscribersbecomes a per-actor multimap ofstd::shared_ptr<BaseSubscriber>;SetFrameiterates and dispatches via the boundActorROS2Handler._controlleris gone.ROS2CallbackDatastd::variantextended withAckermannControl; matchingActorROS2Handler::operator()(carla::ros2::AckermannControl&)overload routes commands into the existingACarlaWheeledVehicle::ApplyVehicleAckermannControl(...)path.listeners/CarlaSubscriberListener.{h,cpp}deleted (no remaining callers after the migration).LibCarla/source/test/common/test_ackermann_conversion.cppwith 5 conversion cases plus 2 variant-dispatch cases. Picks up automatically via the existingtest/common/*.cppGLOB.Where has this been tested?
libcarla_test_server80/80 andlibcarla_test_client94/94 pass; the 7 newAckermannConversion.*+ROS2CallbackData.*cases pass in both suites.carla-client,carla-python-api,carla-serverand the Releasepackagetarget build green with no new warnings on the files this PR touches.Possible Drawbacks
AddActorCallbackentry point now also instantiate anAckermannControlSubscriber, which opens a second FastDDS DataReader on the/ackermann_control_cmdtopic. Idle cost is a participant subscription; functional cost is nil unless a publisher actively writes to that topic.CarlaSubscriberListener.{h,cpp}deletion is observable for any out-of-tree consumer that included those headers. Inside this repo nothing else references them.Series status
Each sub-PR's branch is cut from the previous sub-PR's tip; the GitHub PR opens against
ue5-devand its diff initially includes the predecessor's commit until the predecessor merges, at which point the diff narrows to just the sub-PR's own commit.Related
Part of the 8-PR port of
ue4-devROS 2 enhancements toue5-dev(issue #9627, discussion #9581 Category 2).This change is