diff --git a/CMake/Options.cmake b/CMake/Options.cmake index fecbeab5226..0725b94851e 100644 --- a/CMake/Options.cmake +++ b/CMake/Options.cmake @@ -412,6 +412,12 @@ carla_string_option ( 2.11.2 ) +carla_string_option ( + CARLA_FASTDDS_FILE_VERSION_NAME + "Target Fast-DDS version." + 2.11 +) + carla_string_option ( CARLA_FASTDDS_TAG "Target Fast-DDS git tag." @@ -426,6 +432,12 @@ carla_string_option ( 1.1.x ) +carla_string_option ( + CARLA_FASTCDR_FILE_VERSION_NAME + "Target Fast-CDR version." + 1.1 +) + carla_string_option ( CARLA_FASTCDR_TAG "Target Fast-CDR git tag." diff --git a/LibCarla/source/carla/ros2/ROS2.cpp b/LibCarla/source/carla/ros2/ROS2.cpp index ed563cb6bec..3c6080d6a59 100644 --- a/LibCarla/source/carla/ros2/ROS2.cpp +++ b/LibCarla/source/carla/ros2/ROS2.cpp @@ -4,894 +4,24 @@ // This work is licensed under the terms of the MIT license. // For a copy, see . -#include "carla/Logging.h" #include "carla/ros2/ROS2.h" -#include "carla/geom/GeoLocation.h" -#include "carla/geom/Vector3D.h" -#include "carla/sensor/data/DVSEvent.h" -#include "carla/sensor/data/LidarData.h" -#include "carla/sensor/data/SemanticLidarData.h" -#include "carla/sensor/data/RadarData.h" -#include "carla/sensor/data/Image.h" -#include "carla/sensor/s11n/ImageSerializer.h" -#include "carla/sensor/s11n/SensorHeaderSerializer.h" -#include "publishers/CarlaPublisher.h" -#include "publishers/CarlaClockPublisher.h" -#include "publishers/CarlaRGBCameraPublisher.h" -#include "publishers/CarlaDepthCameraPublisher.h" -#include "publishers/CarlaNormalsCameraPublisher.h" -#include "publishers/CarlaOpticalFlowCameraPublisher.h" -#include "publishers/CarlaSSCameraPublisher.h" -#include "publishers/CarlaISCameraPublisher.h" -#include "publishers/CarlaDVSCameraPublisher.h" -#include "publishers/CarlaLidarPublisher.h" -#include "publishers/CarlaSemanticLidarPublisher.h" -#include "publishers/CarlaRadarPublisher.h" -#include "publishers/CarlaIMUPublisher.h" -#include "publishers/CarlaGNSSPublisher.h" -#include "publishers/CarlaMapSensorPublisher.h" -#include "publishers/CarlaSpeedometerSensor.h" -#include "publishers/CarlaTransformPublisher.h" -#include "publishers/CarlaCollisionPublisher.h" -#include "publishers/CarlaLineInvasionPublisher.h" -#include "publishers/BasicPublisher.h" - -#include "subscribers/CarlaSubscriber.h" -#include "subscribers/CarlaEgoVehicleControlSubscriber.h" -#if defined(WITH_ROS2_DEMO) - #include "subscribers/BasicSubscriber.h" -#endif - -#include namespace carla { namespace ros2 { -// static fields -std::shared_ptr ROS2::_instance; - -// list of sensors (should be equal to the list of SensorsRegistry -enum ESensors { - CollisionSensor, - DepthCamera, - NormalsCamera, - DVSCamera, - GnssSensor, - InertialMeasurementUnit, - LaneInvasionSensor, - ObstacleDetectionSensor, - OpticalFlowCamera, - Radar, - RayCastSemanticLidar, - RayCastLidar, - RssSensor, - SceneCaptureCamera, - SemanticSegmentationCamera, - InstanceSegmentationCamera, - WorldObserver, - CameraGBufferUint8, - CameraGBufferFloat -}; - -void ROS2::Enable(bool enable) { - _enabled = enable; - log_info("ROS2 enabled: ", _enabled); - _clock_publisher = std::make_shared("clock", ""); - _clock_publisher->Init(); -#if defined(WITH_ROS2_DEMO) - _basic_publisher = std::make_shared("basic_publisher", ""); - _basic_publisher->Init(); -#endif -} - -void ROS2::SetFrame(uint64_t frame) { - _frame = frame; - //log_info("ROS2 new frame: ", _frame); - if (_controller) { - void* actor = _controller->GetVehicle(); - if (_controller->IsAlive()) { - if (_controller->HasNewMessage()) { - auto it = _actor_callbacks.find(actor); - if (it != _actor_callbacks.end()) { - VehicleControl control = _controller->GetMessage(); - it->second(actor, control); - } - } - } else { - RemoveActorCallback(actor); - } - } -#if defined(WITH_ROS2_DEMO) - if (_basic_subscriber) - { - void* actor = _basic_subscriber->GetActor(); - if (!_basic_subscriber->IsAlive()){ - RemoveBasicSubscriberCallback(actor); - } - if (actor&& _basic_subscriber->HasNewMessage()) - { - auto it = _actor_message_callbacks.find(actor); - if (it != _actor_message_callbacks.end()) { - MessageControl control; - control.message = _basic_subscriber->GetMessage(); - it->second(actor, control); - } - } - } -#endif -} - void ROS2::SetTimestamp(double timestamp) { - double integral; - const double fractional = modf(timestamp, &integral); - const double multiplier = 1000000000.0; - _seconds = static_cast(integral); - _nanoseconds = static_cast(fractional * multiplier); - _clock_publisher->SetData(_seconds, _nanoseconds); - _clock_publisher->Publish(); -#if defined(WITH_ROS2_DEMO) - _basic_publisher->SetData("Hello from Carla!"); - _basic_publisher->Publish(); -#endif -} - -void ROS2::AddActorRosName(void *actor, std::string ros_name) { - _actor_ros_name.insert({actor, ros_name}); -} - -void ROS2::AddActorParentRosName(void *actor, void* parent) { - auto it = _actor_parent_ros_name.find(actor); - if (it != _actor_parent_ros_name.end()) { - it->second.push_back(parent); - } else { - _actor_parent_ros_name.insert({actor, {parent}}); - } -} - -void ROS2::RemoveActorRosName(void *actor) { - _actor_ros_name.erase(actor); - _actor_parent_ros_name.erase(actor); - - _publishers.erase(actor); - _transforms.erase(actor); -} - -void ROS2::UpdateActorRosName(void *actor, std::string ros_name) { - auto it = _actor_ros_name.find(actor); - if (it != _actor_ros_name.end()) { - it->second = ros_name; - } -} - -std::string ROS2::GetActorRosName(void *actor) { - auto it = _actor_ros_name.find(actor); - if (it != _actor_ros_name.end()) { - return it->second; - } else { - return std::string(""); - } -} - -std::string ROS2::GetActorParentRosName(void *actor) { - auto it = _actor_parent_ros_name.find(actor); - if (it != _actor_parent_ros_name.end()) - { - const std::string current_actor_name = GetActorRosName(actor); - std::string parent_name; - for (auto parent_it = it->second.cbegin(); parent_it != it->second.cend(); ++parent_it) - { - const std::string name = GetActorRosName(*parent_it); - if (name == current_actor_name) - { - continue; - } - if (name.empty()) - { - continue; - } - parent_name = name + '/' + parent_name; + if (IsEnabled()){ + double integral; + const double fractional = modf(timestamp, &integral); + const double multiplier = 1000000000.0; + _seconds = static_cast(integral); + _nanoseconds = static_cast(fractional * multiplier); } - if (parent_name.back() == '/') - parent_name.pop_back(); - return parent_name; - } - else - return std::string(""); -} - -void ROS2::AddBasicSubscriberCallback(void* actor, std::string ros_name, ActorMessageCallback callback) { - #if defined(WITH_ROS2_DEMO) - _actor_message_callbacks.insert({actor, std::move(callback)}); - - _basic_subscriber.reset(); - _basic_subscriber = std::make_shared(actor, ros_name.c_str()); - _basic_subscriber->Init(); - #endif -} - -void ROS2::RemoveBasicSubscriberCallback(void* actor) { - #if defined(WITH_ROS2_DEMO) - _basic_subscriber.reset(); - _actor_message_callbacks.erase(actor); - #endif } -void ROS2::AddActorCallback(void* actor, std::string ros_name, ActorCallback callback) { - _actor_callbacks.insert({actor, std::move(callback)}); - - _controller.reset(); - _controller = std::make_shared(actor, ros_name.c_str()); - _controller->Init(); -} - -void ROS2::RemoveActorCallback(void* actor) { - _controller.reset(); - _actor_callbacks.erase(actor); -} - -std::pair, std::shared_ptr> ROS2::GetOrCreateSensor(int type, carla::streaming::detail::stream_id_type id, void* actor) { - auto it_publishers = _publishers.find(actor); - auto it_transforms = _transforms.find(actor); - std::shared_ptr publisher {}; - std::shared_ptr transform {}; - if (it_publishers != _publishers.end()) { - publisher = it_publishers->second; - if (it_transforms != _transforms.end()) { - transform = it_transforms->second; - } - } else { - //Sensor not found, creating one of the given type - const std::string string_id = std::to_string(id); - std::string ros_name = GetActorRosName(actor); - std::string parent_ros_name = GetActorParentRosName(actor); - switch(type) { - case ESensors::CollisionSensor: { - if (ros_name == "collision__") { - ros_name.pop_back(); - ros_name.pop_back(); - ros_name += string_id; - UpdateActorRosName(actor, ros_name); - } - std::shared_ptr new_publisher = std::make_shared(ros_name.c_str(), parent_ros_name.c_str()); - if (new_publisher->Init()) { - _publishers.insert({actor, new_publisher}); - publisher = new_publisher; - } - std::shared_ptr new_transform = std::make_shared(ros_name.c_str(), parent_ros_name.c_str()); - if (new_transform->Init()) { - _transforms.insert({actor, new_transform}); - transform = new_transform; - } - } break; - case ESensors::DepthCamera: { - if (ros_name == "depth__") { - ros_name.pop_back(); - ros_name.pop_back(); - ros_name += string_id; - UpdateActorRosName(actor, ros_name); - } - std::shared_ptr new_publisher = std::make_shared(ros_name.c_str(), parent_ros_name.c_str()); - if (new_publisher->Init()) { - _publishers.insert({actor, new_publisher}); - publisher = new_publisher; - } - std::shared_ptr new_transform = std::make_shared(ros_name.c_str(), parent_ros_name.c_str()); - if (new_transform->Init()) { - _transforms.insert({actor, new_transform}); - transform = new_transform; - } - } break; - case ESensors::NormalsCamera: { - if (ros_name == "normals__") { - ros_name.pop_back(); - ros_name.pop_back(); - ros_name += string_id; - UpdateActorRosName(actor, ros_name); - } - std::shared_ptr new_publisher = std::make_shared(ros_name.c_str(), parent_ros_name.c_str()); - if (new_publisher->Init()) { - _publishers.insert({actor, new_publisher}); - publisher = new_publisher; - } - std::shared_ptr new_transform = std::make_shared(ros_name.c_str(), parent_ros_name.c_str()); - if (new_transform->Init()) { - _transforms.insert({actor, new_transform}); - transform = new_transform; - } - } break; - case ESensors::DVSCamera: { - if (ros_name == "dvs__") { - ros_name.pop_back(); - ros_name.pop_back(); - ros_name += string_id; - UpdateActorRosName(actor, ros_name); - } - std::shared_ptr new_publisher = std::make_shared(ros_name.c_str(), parent_ros_name.c_str()); - if (new_publisher->Init()) { - _publishers.insert({actor, new_publisher}); - publisher = new_publisher; - } - std::shared_ptr new_transform = std::make_shared(ros_name.c_str(), parent_ros_name.c_str()); - if (new_transform->Init()) { - _transforms.insert({actor, new_transform}); - transform = new_transform; - } - } break; - case ESensors::GnssSensor: { - if (ros_name == "gnss__") { - ros_name.pop_back(); - ros_name.pop_back(); - ros_name += string_id; - UpdateActorRosName(actor, ros_name); - } - std::shared_ptr new_publisher = std::make_shared(ros_name.c_str(), parent_ros_name.c_str()); - if (new_publisher->Init()) { - _publishers.insert({actor, new_publisher}); - publisher = new_publisher; - } - std::shared_ptr new_transform = std::make_shared(ros_name.c_str(), parent_ros_name.c_str()); - if (new_transform->Init()) { - _transforms.insert({actor, new_transform}); - transform = new_transform; - } - } break; - case ESensors::InertialMeasurementUnit: { - if (ros_name == "imu__") { - ros_name.pop_back(); - ros_name.pop_back(); - ros_name += string_id; - UpdateActorRosName(actor, ros_name); - } - std::shared_ptr new_publisher = std::make_shared(ros_name.c_str(), parent_ros_name.c_str()); - if (new_publisher->Init()) { - _publishers.insert({actor, new_publisher}); - publisher = new_publisher; - } - std::shared_ptr new_transform = std::make_shared(ros_name.c_str(), parent_ros_name.c_str()); - if (new_transform->Init()) { - _transforms.insert({actor, new_transform}); - transform = new_transform; - } - } break; - case ESensors::LaneInvasionSensor: { - if (ros_name == "lane_invasion__") { - ros_name.pop_back(); - ros_name.pop_back(); - ros_name += string_id; - UpdateActorRosName(actor, ros_name); - } - std::shared_ptr new_publisher = std::make_shared(ros_name.c_str(), parent_ros_name.c_str()); - if (new_publisher->Init()) { - _publishers.insert({actor, new_publisher}); - publisher = new_publisher; - } - std::shared_ptr new_transform = std::make_shared(ros_name.c_str(), parent_ros_name.c_str()); - if (new_transform->Init()) { - _transforms.insert({actor, new_transform}); - transform = new_transform; - } - } break; - case ESensors::ObstacleDetectionSensor: { - std::cerr << "Obstacle detection sensor does not have an available publisher" << std::endl; - } break; - case ESensors::OpticalFlowCamera: { - if (ros_name == "optical_flow__") { - ros_name.pop_back(); - ros_name.pop_back(); - ros_name += string_id; - UpdateActorRosName(actor, ros_name); - } - std::shared_ptr new_publisher = std::make_shared(ros_name.c_str(), parent_ros_name.c_str()); - if (new_publisher->Init()) { - _publishers.insert({actor, new_publisher}); - publisher = new_publisher; - } - std::shared_ptr new_transform = std::make_shared(ros_name.c_str(), parent_ros_name.c_str()); - if (new_transform->Init()) { - _transforms.insert({actor, new_transform}); - transform = new_transform; - } - } break; - case ESensors::Radar: { - if (ros_name == "radar__") { - ros_name.pop_back(); - ros_name.pop_back(); - ros_name += string_id; - UpdateActorRosName(actor, ros_name); - } - std::shared_ptr new_publisher = std::make_shared(ros_name.c_str(), parent_ros_name.c_str()); - if (new_publisher->Init()) { - _publishers.insert({actor, new_publisher}); - publisher = new_publisher; - } - std::shared_ptr new_transform = std::make_shared(ros_name.c_str(), parent_ros_name.c_str()); - if (new_transform->Init()) { - _transforms.insert({actor, new_transform}); - transform = new_transform; - } - } break; - case ESensors::RayCastSemanticLidar: { - if (ros_name == "ray_cast_semantic__") { - ros_name.pop_back(); - ros_name.pop_back(); - ros_name += string_id; - UpdateActorRosName(actor, ros_name); - } - std::shared_ptr new_publisher = std::make_shared(ros_name.c_str(), parent_ros_name.c_str()); - if (new_publisher->Init()) { - _publishers.insert({actor, new_publisher}); - publisher = new_publisher; - } - std::shared_ptr new_transform = std::make_shared(ros_name.c_str(), parent_ros_name.c_str()); - if (new_transform->Init()) { - _transforms.insert({actor, new_transform}); - transform = new_transform; - } - } break; - case ESensors::RayCastLidar: { - if (ros_name == "ray_cast__") { - ros_name.pop_back(); - ros_name.pop_back(); - ros_name += string_id; - UpdateActorRosName(actor, ros_name); - } - std::shared_ptr new_publisher = std::make_shared(ros_name.c_str(), parent_ros_name.c_str()); - if (new_publisher->Init()) { - _publishers.insert({actor, new_publisher}); - publisher = new_publisher; - } - std::shared_ptr new_transform = std::make_shared(ros_name.c_str(), parent_ros_name.c_str()); - if (new_transform->Init()) { - _transforms.insert({actor, new_transform}); - transform = new_transform; - } - } break; - case ESensors::RssSensor: { - std::cerr << "RSS sensor does not have an available publisher" << std::endl; - } break; - case ESensors::SceneCaptureCamera: { - if (ros_name == "rgb__") { - ros_name.pop_back(); - ros_name.pop_back(); - ros_name += string_id; - UpdateActorRosName(actor, ros_name); - } - std::shared_ptr new_publisher = std::make_shared(ros_name.c_str(), parent_ros_name.c_str()); - if (new_publisher->Init()) { - _publishers.insert({actor, new_publisher}); - publisher = new_publisher; - } - std::shared_ptr new_transform = std::make_shared(ros_name.c_str(), parent_ros_name.c_str()); - if (new_transform->Init()) { - _transforms.insert({actor, new_transform}); - transform = new_transform; - } - } break; - case ESensors::SemanticSegmentationCamera: { - if (ros_name == "semantic_segmentation__") { - ros_name.pop_back(); - ros_name.pop_back(); - ros_name += string_id; - UpdateActorRosName(actor, ros_name); - } - std::shared_ptr new_publisher = std::make_shared(ros_name.c_str(), parent_ros_name.c_str()); - if (new_publisher->Init()) { - _publishers.insert({actor, new_publisher}); - publisher = new_publisher; - } - std::shared_ptr new_transform = std::make_shared(ros_name.c_str(), parent_ros_name.c_str()); - if (new_transform->Init()) { - _transforms.insert({actor, new_transform}); - transform = new_transform; - } - } break; - case ESensors::InstanceSegmentationCamera: { - if (ros_name == "instance_segmentation__") { - ros_name.pop_back(); - ros_name.pop_back(); - ros_name += string_id; - UpdateActorRosName(actor, ros_name); - } - std::shared_ptr new_publisher = std::make_shared(ros_name.c_str(), parent_ros_name.c_str()); - if (new_publisher->Init()) { - _publishers.insert({actor, new_publisher}); - publisher = new_publisher; - } - std::shared_ptr new_transform = std::make_shared(ros_name.c_str(), parent_ros_name.c_str()); - if (new_transform->Init()) { - _transforms.insert({actor, new_transform}); - transform = new_transform; - } - } break; - case ESensors::WorldObserver: { - std::cerr << "World obserser does not have an available publisher" << std::endl; - } break; - case ESensors::CameraGBufferUint8: { - std::cerr << "Camera GBuffer uint8 does not have an available publisher" << std::endl; - } break; - case ESensors::CameraGBufferFloat: { - std::cerr << "Camera GBuffer float does not have an available publisher" << std::endl; - } break; - default: { - std::cerr << "Unknown sensor type" << std::endl; - } - } - } - return { publisher, transform }; -} - -void ROS2::ProcessDataFromCamera( - uint64_t sensor_type, - carla::streaming::detail::stream_id_type stream_id, - const carla::geom::Transform sensor_transform, - int W, int H, float Fov, - const carla::SharedBufferView buffer, - void *actor) { - - switch (sensor_type) { - case ESensors::CollisionSensor: - log_info("Sensor Collision to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "buffer.", buffer->size()); - break; - case ESensors::DepthCamera: - { - log_info("Sensor DepthCamera to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "buffer.", buffer->size()); - auto sensors = GetOrCreateSensor(ESensors::DepthCamera, stream_id, actor); - if (sensors.first) { - std::shared_ptr publisher = std::dynamic_pointer_cast(sensors.first); - const carla::sensor::s11n::ImageSerializer::ImageHeader *header = - reinterpret_cast(buffer->data()); - if (!header) - return; - if (!publisher->HasBeenInitialized()) - publisher->InitInfoData(0, 0, H, W, Fov, true); - publisher->SetImageData(_seconds, _nanoseconds, header->height, header->width, (const uint8_t*) (buffer->data() + carla::sensor::s11n::ImageSerializer::header_offset)); - publisher->SetCameraInfoData(_seconds, _nanoseconds); - publisher->Publish(); - } - if (sensors.second) { - std::shared_ptr publisher = std::dynamic_pointer_cast(sensors.second); - publisher->SetData(_seconds, _nanoseconds, (const float*)&sensor_transform.location, (const float*)&sensor_transform.rotation); - publisher->Publish(); - } - } - break; - case ESensors::NormalsCamera: - log_info("Sensor NormalsCamera to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "buffer.", buffer->size()); - { - auto sensors = GetOrCreateSensor(ESensors::NormalsCamera, stream_id, actor); - if (sensors.first) { - std::shared_ptr publisher = std::dynamic_pointer_cast(sensors.first); - const carla::sensor::s11n::ImageSerializer::ImageHeader *header = - reinterpret_cast(buffer->data()); - if (!header) - return; - if (!publisher->HasBeenInitialized()) - publisher->InitInfoData(0, 0, H, W, Fov, true); - publisher->SetImageData(_seconds, _nanoseconds, header->height, header->width, (const uint8_t*) (buffer->data() + carla::sensor::s11n::ImageSerializer::header_offset)); - publisher->SetCameraInfoData(_seconds, _nanoseconds); - publisher->Publish(); - } - if (sensors.second) { - std::shared_ptr publisher = std::dynamic_pointer_cast(sensors.second); - publisher->SetData(_seconds, _nanoseconds, (const float*)&sensor_transform.location, (const float*)&sensor_transform.rotation); - publisher->Publish(); - } - } - break; - case ESensors::LaneInvasionSensor: - log_info("Sensor LaneInvasionSensor to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "buffer.", buffer->size()); - { - auto sensors = GetOrCreateSensor(ESensors::LaneInvasionSensor, stream_id, actor); - if (sensors.first) { - std::shared_ptr publisher = std::dynamic_pointer_cast(sensors.first); - publisher->SetData(_seconds, _nanoseconds, (const int32_t*) buffer->data()); - publisher->Publish(); - } - if (sensors.second) { - std::shared_ptr publisher = std::dynamic_pointer_cast(sensors.second); - publisher->SetData(_seconds, _nanoseconds, (const float*)&sensor_transform.location, (const float*)&sensor_transform.rotation); - publisher->Publish(); - } - } - break; - case ESensors::OpticalFlowCamera: - log_info("Sensor OpticalFlowCamera to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "buffer.", buffer->size()); - { - auto sensors = GetOrCreateSensor(ESensors::OpticalFlowCamera, stream_id, actor); - if (sensors.first) { - std::shared_ptr publisher = std::dynamic_pointer_cast(sensors.first); - const carla::sensor::s11n::OpticalFlowImageSerializer::ImageHeader *header = - reinterpret_cast(buffer->data()); - if (!header) - return; - if (!publisher->HasBeenInitialized()) - publisher->InitInfoData(0, 0, H, W, Fov, true); - publisher->SetImageData(_seconds, _nanoseconds, header->height, header->width, (const float*) (buffer->data() + carla::sensor::s11n::OpticalFlowImageSerializer::header_offset)); - publisher->SetCameraInfoData(_seconds, _nanoseconds); - publisher->Publish(); - } - if (sensors.second) { - std::shared_ptr publisher = std::dynamic_pointer_cast(sensors.second); - publisher->SetData(_seconds, _nanoseconds, (const float*)&sensor_transform.location, (const float*)&sensor_transform.rotation); - publisher->Publish(); - } - } - break; - case ESensors::RssSensor: - log_info("Sensor RssSensor to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "buffer.", buffer->size()); - break; - case ESensors::SceneCaptureCamera: - { - log_info("Sensor SceneCaptureCamera to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "buffer.", buffer->size()); - { - auto sensors = GetOrCreateSensor(ESensors::SceneCaptureCamera, stream_id, actor); - if (sensors.first) { - std::shared_ptr publisher = std::dynamic_pointer_cast(sensors.first); - const carla::sensor::s11n::ImageSerializer::ImageHeader *header = - reinterpret_cast(buffer->data()); - if (!header) - return; - if (!publisher->HasBeenInitialized()) - publisher->InitInfoData(0, 0, H, W, Fov, true); - publisher->SetImageData(_seconds, _nanoseconds, header->height, header->width, (const uint8_t*) (buffer->data() + carla::sensor::s11n::ImageSerializer::header_offset)); - publisher->SetCameraInfoData(_seconds, _nanoseconds); - publisher->Publish(); - } - if (sensors.second) { - std::shared_ptr publisher = std::dynamic_pointer_cast(sensors.second); - publisher->SetData(_seconds, _nanoseconds, (const float*)&sensor_transform.location, (const float*)&sensor_transform.rotation); - publisher->Publish(); - } - } - break; - } - case ESensors::SemanticSegmentationCamera: - log_info("Sensor SemanticSegmentationCamera to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "buffer.", buffer->size()); - { - auto sensors = GetOrCreateSensor(ESensors::SemanticSegmentationCamera, stream_id, actor); - if (sensors.first) { - std::shared_ptr publisher = std::dynamic_pointer_cast(sensors.first); - const carla::sensor::s11n::ImageSerializer::ImageHeader *header = - reinterpret_cast(buffer->data()); - if (!header) - return; - if (!publisher->HasBeenInitialized()) - publisher->InitInfoData(0, 0, H, W, Fov, true); - publisher->SetImageData(_seconds, _nanoseconds, header->height, header->width, (const uint8_t*) (buffer->data() + carla::sensor::s11n::ImageSerializer::header_offset)); - publisher->SetCameraInfoData(_seconds, _nanoseconds); - publisher->Publish(); - } - if (sensors.second) { - std::shared_ptr publisher = std::dynamic_pointer_cast(sensors.second); - publisher->SetData(_seconds, _nanoseconds, (const float*)&sensor_transform.location, (const float*)&sensor_transform.rotation); - publisher->Publish(); - } - } - break; - case ESensors::InstanceSegmentationCamera: - log_info("Sensor InstanceSegmentationCamera to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "buffer.", buffer->size()); - { - auto sensors = GetOrCreateSensor(ESensors::InstanceSegmentationCamera, stream_id, actor); - if (sensors.first) { - std::shared_ptr publisher = std::dynamic_pointer_cast(sensors.first); - const carla::sensor::s11n::ImageSerializer::ImageHeader *header = - reinterpret_cast(buffer->data()); - if (!header) - return; - if (!publisher->HasBeenInitialized()) - publisher->InitInfoData(0, 0, H, W, Fov, true); - publisher->SetImageData(_seconds, _nanoseconds, header->height, header->width, (const uint8_t*) (buffer->data() + carla::sensor::s11n::ImageSerializer::header_offset)); - publisher->SetCameraInfoData(_seconds, _nanoseconds); - publisher->Publish(); - } - if (sensors.second) { - std::shared_ptr publisher = std::dynamic_pointer_cast(sensors.second); - publisher->SetData(_seconds, _nanoseconds, (const float*)&sensor_transform.location, (const float*)&sensor_transform.rotation); - publisher->Publish(); - } - } - break; - case ESensors::WorldObserver: - log_info("Sensor WorldObserver to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "buffer.", buffer->size()); - break; - case ESensors::CameraGBufferUint8: - log_info("Sensor CameraGBufferUint8 to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "buffer.", buffer->size()); - break; - case ESensors::CameraGBufferFloat: - log_info("Sensor CameraGBufferFloat to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "buffer.", buffer->size()); - break; - default: - log_info("Sensor to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "buffer.", buffer->size()); - } -} - -void ROS2::ProcessDataFromGNSS( - uint64_t sensor_type, - carla::streaming::detail::stream_id_type stream_id, - const carla::geom::Transform sensor_transform, - const carla::geom::GeoLocation &data, - void *actor) { - log_info("Sensor GnssSensor to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "geo.", data.latitude, data.longitude, data.altitude); - auto sensors = GetOrCreateSensor(ESensors::GnssSensor, stream_id, actor); - if (sensors.first) { - std::shared_ptr publisher = std::dynamic_pointer_cast(sensors.first); - publisher->SetData(_seconds, _nanoseconds, reinterpret_cast(&data)); - publisher->Publish(); - } - if (sensors.second) { - std::shared_ptr publisher = std::dynamic_pointer_cast(sensors.second); - publisher->SetData(_seconds, _nanoseconds, (const float*)&sensor_transform.location, (const float*)&sensor_transform.rotation); - publisher->Publish(); - } -} - -void ROS2::ProcessDataFromIMU( - uint64_t sensor_type, - carla::streaming::detail::stream_id_type stream_id, - const carla::geom::Transform sensor_transform, - carla::geom::Vector3D accelerometer, - carla::geom::Vector3D gyroscope, - float compass, - void *actor) { - log_info("Sensor InertialMeasurementUnit to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "imu.", accelerometer.x, gyroscope.x, compass); - auto sensors = GetOrCreateSensor(ESensors::InertialMeasurementUnit, stream_id, actor); - if (sensors.first) { - std::shared_ptr publisher = std::dynamic_pointer_cast(sensors.first); - publisher->SetData(_seconds, _nanoseconds, reinterpret_cast(&accelerometer), reinterpret_cast(&gyroscope), compass); - publisher->Publish(); - } - if (sensors.second) { - std::shared_ptr publisher = std::dynamic_pointer_cast(sensors.second); - publisher->SetData(_seconds, _nanoseconds, (const float*)&sensor_transform.location, (const float*)&sensor_transform.rotation); - publisher->Publish(); - } -} - -void ROS2::ProcessDataFromDVS( - uint64_t sensor_type, - carla::streaming::detail::stream_id_type stream_id, - const carla::geom::Transform sensor_transform, - const carla::SharedBufferView buffer, - int W, int H, float Fov, - void *actor) { - log_info("Sensor DVS to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id); - auto sensors = GetOrCreateSensor(ESensors::DVSCamera, stream_id, actor); - if (sensors.first) { - std::shared_ptr publisher = std::dynamic_pointer_cast(sensors.first); - const carla::sensor::s11n::ImageSerializer::ImageHeader *header = - reinterpret_cast(buffer->data()); - if (!header) - return; - if (!publisher->HasBeenInitialized()) - publisher->InitInfoData(0, 0, H, W, Fov, true); - size_t elements = (buffer->size() - carla::sensor::s11n::ImageSerializer::header_offset) / sizeof(carla::sensor::data::DVSEvent); - publisher->SetImageData(_seconds, _nanoseconds, elements, header->height, header->width, (const uint8_t*) (buffer->data() + carla::sensor::s11n::ImageSerializer::header_offset)); - publisher->SetCameraInfoData(_seconds, _nanoseconds); - publisher->SetPointCloudData(1, elements * sizeof(carla::sensor::data::DVSEvent), elements, (const uint8_t*) (buffer->data() + carla::sensor::s11n::ImageSerializer::header_offset)); - publisher->Publish(); - } - if (sensors.second) { - std::shared_ptr publisher = std::dynamic_pointer_cast(sensors.second); - publisher->SetData(_seconds, _nanoseconds, (const float*)&sensor_transform.location, (const float*)&sensor_transform.rotation); - publisher->Publish(); - } -} - -void ROS2::ProcessDataFromLidar( - uint64_t sensor_type, - carla::streaming::detail::stream_id_type stream_id, - const carla::geom::Transform sensor_transform, - carla::sensor::data::LidarData &data, - void *actor) { - log_info("Sensor Lidar to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "points.", data._points.size()); - auto sensors = GetOrCreateSensor(ESensors::RayCastLidar, stream_id, actor); - if (sensors.first) { - std::shared_ptr publisher = std::dynamic_pointer_cast(sensors.first); - size_t width = data._points.size(); - size_t height = 1; - publisher->SetData(_seconds, _nanoseconds, height, width, (float*)data._points.data()); - publisher->Publish(); - } - if (sensors.second) { - std::shared_ptr publisher = std::dynamic_pointer_cast(sensors.second); - publisher->SetData(_seconds, _nanoseconds, (const float*)&sensor_transform.location, (const float*)&sensor_transform.rotation); - publisher->Publish(); - } -} - -void ROS2::ProcessDataFromSemanticLidar( - uint64_t sensor_type, - carla::streaming::detail::stream_id_type stream_id, - const carla::geom::Transform sensor_transform, - carla::sensor::data::SemanticLidarData &data, - void *actor) { - static_assert(sizeof(float) == sizeof(uint32_t), "Invalid float size"); - log_info("Sensor SemanticLidar to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "points.", data._ser_points.size()); - auto sensors = GetOrCreateSensor(ESensors::RayCastSemanticLidar, stream_id, actor); - if (sensors.first) { - std::shared_ptr publisher = std::dynamic_pointer_cast(sensors.first); - size_t width = data._ser_points.size(); - size_t height = 1; - publisher->SetData(_seconds, _nanoseconds, 6, height, width, (float*)data._ser_points.data()); - publisher->Publish(); - } - if (sensors.second) { - std::shared_ptr publisher = std::dynamic_pointer_cast(sensors.second); - publisher->SetData(_seconds, _nanoseconds, (const float*)&sensor_transform.location, (const float*)&sensor_transform.rotation); - publisher->Publish(); - } -} - -void ROS2::ProcessDataFromRadar( - uint64_t sensor_type, - carla::streaming::detail::stream_id_type stream_id, - const carla::geom::Transform sensor_transform, - const carla::sensor::data::RadarData &data, - void *actor) { - log_info("Sensor Radar to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "points.", data._detections.size()); - auto sensors = GetOrCreateSensor(ESensors::Radar, stream_id, actor); - if (sensors.first) { - std::shared_ptr publisher = std::dynamic_pointer_cast(sensors.first); - size_t elements = data.GetDetectionCount(); - size_t width = elements * sizeof(carla::sensor::data::RadarDetection); - size_t height = 1; - publisher->SetData(_seconds, _nanoseconds, height, width, elements, (const uint8_t*)data._detections.data()); - publisher->Publish(); - } - if (sensors.second) { - std::shared_ptr publisher = std::dynamic_pointer_cast(sensors.second); - publisher->SetData(_seconds, _nanoseconds, (const float*)&sensor_transform.location, (const float*)&sensor_transform.rotation); - publisher->Publish(); - } -} - -void ROS2::ProcessDataFromObstacleDetection( - uint64_t sensor_type, - carla::streaming::detail::stream_id_type stream_id, - const carla::geom::Transform sensor_transform, - AActor *first_ctor, - AActor *second_actor, - float distance, - void *actor) { - log_info("Sensor ObstacleDetector to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "distance.", distance); -} - -void ROS2::ProcessDataFromCollisionSensor( - uint64_t sensor_type, - carla::streaming::detail::stream_id_type stream_id, - const carla::geom::Transform sensor_transform, - uint32_t other_actor, - carla::geom::Vector3D impulse, - void* actor) { - auto sensors = GetOrCreateSensor(ESensors::CollisionSensor, stream_id, actor); - if (sensors.first) { - std::shared_ptr publisher = std::dynamic_pointer_cast(sensors.first); - publisher->SetData(_seconds, _nanoseconds, other_actor, impulse.x, impulse.y, impulse.z); - publisher->Publish(); - } - if (sensors.second) { - std::shared_ptr publisher = std::dynamic_pointer_cast(sensors.second); - publisher->SetData(_seconds, _nanoseconds, (const float*)&sensor_transform.location, (const float*)&sensor_transform.rotation); - publisher->Publish(); - } -} - -void ROS2::Shutdown() { - for (auto& element : _publishers) { - element.second.reset(); - } - for (auto& element : _transforms) { - element.second.reset(); - } - _clock_publisher.reset(); - _controller.reset(); - _enabled = false; -#if defined(WITH_ROS2_DEMO) - _basic_publisher.reset(); - _basic_subscriber.reset(); -#endif -} +// static fields +std::shared_ptr ROS2::_instance; } // namespace ros2 } // namespace carla diff --git a/LibCarla/source/carla/ros2/ROS2.h b/LibCarla/source/carla/ros2/ROS2.h index 93b084dc5b8..69577ddb15c 100644 --- a/LibCarla/source/carla/ros2/ROS2.h +++ b/LibCarla/source/carla/ros2/ROS2.h @@ -6,6 +6,7 @@ #pragma once +#include "carla/ros2/ROS2Interfaces.h" #include "carla/Buffer.h" #include "carla/BufferView.h" #include "carla/geom/Transform.h" @@ -17,33 +18,11 @@ #include #include -// forward declarations -class AActor; -namespace carla { - namespace geom { - class GeoLocation; - struct Vector3D; - } - namespace sensor { - namespace data { - struct DVSEvent; - class LidarData; - class SemanticLidarData; - class RadarData; - } - } -} +struct FActorDescription; namespace carla { namespace ros2 { - class CarlaPublisher; - class CarlaTransformPublisher; - class CarlaClockPublisher; - class CarlaEgoVehicleControlSubscriber; - class BasicSubscriber; - class BasicPublisher; - class ROS2 { public: @@ -52,103 +31,27 @@ class ROS2 ROS2(const ROS2& obj) = delete; static std::shared_ptr GetInstance() { if (!_instance) + { _instance = std::shared_ptr(new ROS2); + auto ROS2Interfaces = carla::ros2::ROS2Interfaces::GetInstance(); + ROS2Interfaces->RegisterInterface(_instance); + } return _instance; } - // general - void Enable(bool enable); - void Shutdown(); - bool IsEnabled() { return _enabled; } - void SetFrame(uint64_t frame); - void SetTimestamp(double timestamp); - - // ros_name managing - void AddActorRosName(void *actor, std::string ros_name); - void AddActorParentRosName(void *actor, void* parent); - void RemoveActorRosName(void *actor); - void UpdateActorRosName(void *actor, std::string ros_name); - std::string GetActorRosName(void *actor); - std::string GetActorParentRosName(void *actor); - - // callbacks - void AddActorCallback(void* actor, std::string ros_name, ActorCallback callback); - void RemoveActorCallback(void* actor); - void RemoveBasicSubscriberCallback(void* actor); - void AddBasicSubscriberCallback(void* actor, std::string ros_name, ActorMessageCallback callback); - - // enabling streams to publish - void EnableStream(carla::streaming::detail::stream_id_type id) { _publish_stream.insert(id); } - bool IsStreamEnabled(carla::streaming::detail::stream_id_type id) { return _publish_stream.count(id) > 0; } - void ResetStreams() { _publish_stream.clear(); } - - // receiving data to publish - void ProcessDataFromCamera( - uint64_t sensor_type, - carla::streaming::detail::stream_id_type stream_id, - const carla::geom::Transform sensor_transform, - int W, int H, float Fov, - const carla::SharedBufferView buffer, - void *actor = nullptr); - void ProcessDataFromGNSS( - uint64_t sensor_type, - carla::streaming::detail::stream_id_type stream_id, - const carla::geom::Transform sensor_transform, - const carla::geom::GeoLocation &data, - void *actor = nullptr); - void ProcessDataFromIMU( - uint64_t sensor_type, - carla::streaming::detail::stream_id_type stream_id, - const carla::geom::Transform sensor_transform, - carla::geom::Vector3D accelerometer, - carla::geom::Vector3D gyroscope, - float compass, - void *actor = nullptr); - void ProcessDataFromDVS( - uint64_t sensor_type, - carla::streaming::detail::stream_id_type stream_id, - const carla::geom::Transform sensor_transform, - const carla::SharedBufferView buffer, - int W, int H, float Fov, - void *actor = nullptr); - void ProcessDataFromLidar( - uint64_t sensor_type, - carla::streaming::detail::stream_id_type stream_id, - const carla::geom::Transform sensor_transform, - carla::sensor::data::LidarData &data, - void *actor = nullptr); - void ProcessDataFromSemanticLidar( - uint64_t sensor_type, - carla::streaming::detail::stream_id_type stream_id, - const carla::geom::Transform sensor_transform, - carla::sensor::data::SemanticLidarData &data, - void *actor = nullptr); - void ProcessDataFromRadar( - uint64_t sensor_type, - carla::streaming::detail::stream_id_type stream_id, - const carla::geom::Transform sensor_transform, - const carla::sensor::data::RadarData &data, - void *actor = nullptr); - void ProcessDataFromObstacleDetection( - uint64_t sensor_type, - carla::streaming::detail::stream_id_type stream_id, - const carla::geom::Transform sensor_transform, - AActor *first_actor, - AActor *second_actor, - float distance, - void *actor = nullptr); -void ProcessDataFromCollisionSensor( - uint64_t sensor_type, - carla::streaming::detail::stream_id_type stream_id, - const carla::geom::Transform sensor_transform, - uint32_t other_actor, - carla::geom::Vector3D impulse, - void* actor); + virtual ~ROS2() {}; - private: - std::pair, std::shared_ptr> GetOrCreateSensor(int type, carla::streaming::detail::stream_id_type id, void* actor); - - // sigleton + // general + virtual void Enable(bool enable) { _enabled = enable; }; + virtual void Shutdown() { _enabled = false; }; + virtual bool IsEnabled() { return _enabled; }; + virtual void SetFrame(uint64_t frame) { _frame = frame; }; + virtual void SetTimestamp(double timestamp); + virtual void RegisterActor(FActorDescription& Description, std::string RosName, void* Actor) {}; + virtual void RemoveActor(void* Actor) {}; + virtual void RemoveActorRosPublishers(void *actor) {}; + + // singleton ROS2() {}; static std::shared_ptr _instance; @@ -157,19 +60,7 @@ void ProcessDataFromCollisionSensor( uint64_t _frame { 0 }; int32_t _seconds { 0 }; uint32_t _nanoseconds { 0 }; - std::unordered_map _actor_ros_name; - std::unordered_map > _actor_parent_ros_name; - std::shared_ptr _controller; - std::shared_ptr _clock_publisher; - std::unordered_map> _publishers; - std::unordered_map> _transforms; - std::unordered_set _publish_stream; - std::unordered_map _actor_callbacks; -#if defined(WITH_ROS2_DEMO) - std::shared_ptr _basic_subscriber; - std::shared_ptr _basic_publisher; - std::unordered_map _actor_message_callbacks; -#endif + }; } // namespace ros2 diff --git a/LibCarla/source/carla/ros2/ROS2Carla.cpp b/LibCarla/source/carla/ros2/ROS2Carla.cpp new file mode 100644 index 00000000000..71395b5f4e9 --- /dev/null +++ b/LibCarla/source/carla/ros2/ROS2Carla.cpp @@ -0,0 +1,850 @@ +// Copyright (c) 2024 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 . + +#include "carla/Logging.h" +#include "ROS2Carla.h" +#include "carla/geom/GeoLocation.h" +#include "carla/geom/Vector3D.h" +#include "carla/sensor/data/DVSEvent.h" +#include "carla/sensor/data/LidarData.h" +#include "carla/sensor/data/SemanticLidarData.h" +#include "carla/sensor/data/RadarData.h" +#include "carla/sensor/data/Image.h" +#include "carla/sensor/s11n/ImageSerializer.h" +#include "carla/sensor/s11n/SensorHeaderSerializer.h" + +#include "publishers/CarlaPublisher.h" +#include "publishers/CarlaClockPublisher.h" +#include "publishers/CarlaRGBCameraPublisher.h" +#include "publishers/CarlaDepthCameraPublisher.h" +#include "publishers/CarlaNormalsCameraPublisher.h" +#include "publishers/CarlaOpticalFlowCameraPublisher.h" +#include "publishers/CarlaSSCameraPublisher.h" +#include "publishers/CarlaISCameraPublisher.h" +#include "publishers/CarlaDVSCameraPublisher.h" +#include "publishers/CarlaLidarPublisher.h" +#include "publishers/CarlaSemanticLidarPublisher.h" +#include "publishers/CarlaRadarPublisher.h" +#include "publishers/CarlaIMUPublisher.h" +#include "publishers/CarlaGNSSPublisher.h" +#include "publishers/CarlaMapSensorPublisher.h" +#include "publishers/CarlaSpeedometerSensor.h" +#include "publishers/CarlaTransformPublisher.h" +#include "publishers/CarlaCollisionPublisher.h" +#include "publishers/CarlaLineInvasionPublisher.h" +#include "publishers/BasicPublisher.h" + +#include "subscribers/CarlaSubscriber.h" +#include "subscribers/CarlaEgoVehicleControlSubscriber.h" +#if defined(WITH_ROS2_DEMO) + #include "subscribers/BasicSubscriber.h" +#endif + +#include + +namespace carla { +namespace ros2 { + +// static fields +std::shared_ptr ROS2Carla::_instance; + +// list of sensors (should be equal to the list of SensorsRegistry +enum ESensors { + CollisionSensor, + DepthCamera, + NormalsCamera, + DVSCamera, + GnssSensor, + InertialMeasurementUnit, + LaneInvasionSensor, + ObstacleDetectionSensor, + OpticalFlowCamera, + Radar, + RayCastSemanticLidar, + RayCastLidar, + RssSensor, + SceneCaptureCamera, + SemanticSegmentationCamera, + InstanceSegmentationCamera, + WorldObserver, + CameraGBufferUint8, + CameraGBufferFloat +}; + +void ROS2Carla::Enable(bool enable) { + _enabled = enable; + log_info("ROS2Carla enabled: ", _enabled); + _clock_publisher = std::make_shared("clock", ""); + _clock_publisher->Init(); +#if defined(WITH_ROS2Carla_DEMO) + _basic_publisher = std::make_shared("basic_publisher", ""); + _basic_publisher->Init(); +#endif +} + +void ROS2Carla::SetFrame(uint64_t frame) { + _frame = frame; + //log_info("ROS2Carla new frame: ", _frame); + if (_controller) { + void* actor = _controller->GetVehicle(); + if (_controller->IsAlive()) { + if (_controller->HasNewMessage()) { + auto it = _actor_callbacks.find(actor); + if (it != _actor_callbacks.end()) { + VehicleControl control = _controller->GetMessage(); + it->second(actor, control); + } + } + } else { + RemoveActorCallback(actor); + } + } +#if defined(WITH_ROS2Carla_DEMO) + if (_basic_subscriber) + { + void* actor = _basic_subscriber->GetActor(); + if (!_basic_subscriber->IsAlive()){ + RemoveBasicSubscriberCallback(actor); + } + if (actor&& _basic_subscriber->HasNewMessage()) + { + auto it = _actor_message_callbacks.find(actor); + if (it != _actor_message_callbacks.end()) { + MessageControl control; + control.message = _basic_subscriber->GetMessage(); + it->second(actor, control); + } + } + } +#endif +} + +void ROS2Carla::SetTimestamp(double timestamp) { + if (IsEnabled()){ + double integral; + const double fractional = modf(timestamp, &integral); + const double multiplier = 1000000000.0; + _seconds = static_cast(integral); + _nanoseconds = static_cast(fractional * multiplier); + _clock_publisher->SetData(_seconds, _nanoseconds); + _clock_publisher->Publish(); + #if defined(WITH_ROS2Carla_DEMO) + _basic_publisher->SetData("Hello from Carla!"); + _basic_publisher->Publish(); + #endif + } + +} + +void ROS2Carla::Shutdown() { + for (auto& element : _publishers) { + element.second.reset(); + } + for (auto& element : _transforms) { + element.second.reset(); + } + _clock_publisher.reset(); + _controller.reset(); + _enabled = false; +#if defined(WITH_ROS2Carla_DEMO) + _basic_publisher.reset(); + _basic_subscriber.reset(); +#endif +} + +void ROS2Carla::RemoveActorRosPublishers(void *actor) +{ + auto p_it = _publishers.find(actor); + if (p_it != _publishers.end()) { + _publishers.erase(actor); + } + + auto t_it = _transforms.find(actor); + if (t_it != _transforms.end()) { + _transforms.erase(actor); + } +} + +void ROS2Carla::AddBasicSubscriberCallback(void* actor, std::string ros_name, ActorMessageCallback callback) { + #if defined(WITH_ROS2Carla_DEMO) + _actor_message_callbacks.insert({actor, std::move(callback)}); + + _basic_subscriber.reset(); + _basic_subscriber = std::make_shared(actor, ros_name.c_str()); + _basic_subscriber->Init(); + #endif +} + +void ROS2Carla::RemoveBasicSubscriberCallback(void* actor) { + #if defined(WITH_ROS2Carla_DEMO) + _basic_subscriber.reset(); + _actor_message_callbacks.erase(actor); + #endif +} + +void ROS2Carla::AddActorCallback(void* actor, std::string ros_name, ActorCallback callback) { + _actor_callbacks.insert({actor, std::move(callback)}); + + _controller.reset(); + _controller = std::make_shared(actor, ros_name.c_str()); + _controller->Init(); +} + +void ROS2Carla::RemoveActorCallback(void* actor) { + _controller.reset(); + _actor_callbacks.erase(actor); +} + +std::pair, std::shared_ptr> ROS2Carla::GetOrCreateSensor(int type, carla::streaming::detail::stream_id_type id, void* actor) { + auto it_publishers = _publishers.find(actor); + auto it_transforms = _transforms.find(actor); + std::shared_ptr publisher {}; + std::shared_ptr transform {}; + auto ROS2Interfaces = carla::ros2::ROS2Interfaces::GetInstance(); + if (it_publishers != _publishers.end()) { + publisher = it_publishers->second; + if (it_transforms != _transforms.end()) { + transform = it_transforms->second; + } + } else { + //Sensor not found, creating one of the given type + const std::string string_id = std::to_string(id); + std::string ros_name = ROS2Interfaces->GetActorRosName(actor); + std::string parent_ros_name = ROS2Interfaces->GetActorParentRosName(actor); + switch(type) { + case ESensors::CollisionSensor: { + if (ros_name == "collision__") { + ros_name.pop_back(); + ros_name.pop_back(); + ros_name += string_id; + ROS2Interfaces->UpdateActorRosName(actor, ros_name); + } + std::shared_ptr new_publisher = std::make_shared(ros_name.c_str(), parent_ros_name.c_str()); + if (new_publisher->Init()) { + _publishers.insert({actor, new_publisher}); + publisher = new_publisher; + } + std::shared_ptr new_transform = std::make_shared(ros_name.c_str(), parent_ros_name.c_str()); + if (new_transform->Init()) { + _transforms.insert({actor, new_transform}); + transform = new_transform; + } + } break; + case ESensors::DepthCamera: { + if (ros_name == "depth__") { + ros_name.pop_back(); + ros_name.pop_back(); + ros_name += string_id; + ROS2Interfaces->UpdateActorRosName(actor, ros_name); + } + std::shared_ptr new_publisher = std::make_shared(ros_name.c_str(), parent_ros_name.c_str()); + if (new_publisher->Init()) { + _publishers.insert({actor, new_publisher}); + publisher = new_publisher; + } + std::shared_ptr new_transform = std::make_shared(ros_name.c_str(), parent_ros_name.c_str()); + if (new_transform->Init()) { + _transforms.insert({actor, new_transform}); + transform = new_transform; + } + } break; + case ESensors::NormalsCamera: { + if (ros_name == "normals__") { + ros_name.pop_back(); + ros_name.pop_back(); + ros_name += string_id; + ROS2Interfaces->UpdateActorRosName(actor, ros_name); + } + std::shared_ptr new_publisher = std::make_shared(ros_name.c_str(), parent_ros_name.c_str()); + if (new_publisher->Init()) { + _publishers.insert({actor, new_publisher}); + publisher = new_publisher; + } + std::shared_ptr new_transform = std::make_shared(ros_name.c_str(), parent_ros_name.c_str()); + if (new_transform->Init()) { + _transforms.insert({actor, new_transform}); + transform = new_transform; + } + } break; + case ESensors::DVSCamera: { + if (ros_name == "dvs__") { + ros_name.pop_back(); + ros_name.pop_back(); + ros_name += string_id; + ROS2Interfaces->UpdateActorRosName(actor, ros_name); + } + std::shared_ptr new_publisher = std::make_shared(ros_name.c_str(), parent_ros_name.c_str()); + if (new_publisher->Init()) { + _publishers.insert({actor, new_publisher}); + publisher = new_publisher; + } + std::shared_ptr new_transform = std::make_shared(ros_name.c_str(), parent_ros_name.c_str()); + if (new_transform->Init()) { + _transforms.insert({actor, new_transform}); + transform = new_transform; + } + } break; + case ESensors::GnssSensor: { + if (ros_name == "gnss__") { + ros_name.pop_back(); + ros_name.pop_back(); + ros_name += string_id; + ROS2Interfaces->UpdateActorRosName(actor, ros_name); + } + std::shared_ptr new_publisher = std::make_shared(ros_name.c_str(), parent_ros_name.c_str()); + if (new_publisher->Init()) { + _publishers.insert({actor, new_publisher}); + publisher = new_publisher; + } + std::shared_ptr new_transform = std::make_shared(ros_name.c_str(), parent_ros_name.c_str()); + if (new_transform->Init()) { + _transforms.insert({actor, new_transform}); + transform = new_transform; + } + } break; + case ESensors::InertialMeasurementUnit: { + if (ros_name == "imu__") { + ros_name.pop_back(); + ros_name.pop_back(); + ros_name += string_id; + ROS2Interfaces->UpdateActorRosName(actor, ros_name); + } + std::shared_ptr new_publisher = std::make_shared(ros_name.c_str(), parent_ros_name.c_str()); + if (new_publisher->Init()) { + _publishers.insert({actor, new_publisher}); + publisher = new_publisher; + } + std::shared_ptr new_transform = std::make_shared(ros_name.c_str(), parent_ros_name.c_str()); + if (new_transform->Init()) { + _transforms.insert({actor, new_transform}); + transform = new_transform; + } + } break; + case ESensors::LaneInvasionSensor: { + if (ros_name == "lane_invasion__") { + ros_name.pop_back(); + ros_name.pop_back(); + ros_name += string_id; + ROS2Interfaces->UpdateActorRosName(actor, ros_name); + } + std::shared_ptr new_publisher = std::make_shared(ros_name.c_str(), parent_ros_name.c_str()); + if (new_publisher->Init()) { + _publishers.insert({actor, new_publisher}); + publisher = new_publisher; + } + std::shared_ptr new_transform = std::make_shared(ros_name.c_str(), parent_ros_name.c_str()); + if (new_transform->Init()) { + _transforms.insert({actor, new_transform}); + transform = new_transform; + } + } break; + case ESensors::ObstacleDetectionSensor: { + std::cerr << "Obstacle detection sensor does not have an available publisher" << std::endl; + } break; + case ESensors::OpticalFlowCamera: { + if (ros_name == "optical_flow__") { + ros_name.pop_back(); + ros_name.pop_back(); + ros_name += string_id; + ROS2Interfaces->UpdateActorRosName(actor, ros_name); + } + std::shared_ptr new_publisher = std::make_shared(ros_name.c_str(), parent_ros_name.c_str()); + if (new_publisher->Init()) { + _publishers.insert({actor, new_publisher}); + publisher = new_publisher; + } + std::shared_ptr new_transform = std::make_shared(ros_name.c_str(), parent_ros_name.c_str()); + if (new_transform->Init()) { + _transforms.insert({actor, new_transform}); + transform = new_transform; + } + } break; + case ESensors::Radar: { + if (ros_name == "radar__") { + ros_name.pop_back(); + ros_name.pop_back(); + ros_name += string_id; + ROS2Interfaces->UpdateActorRosName(actor, ros_name); + } + std::shared_ptr new_publisher = std::make_shared(ros_name.c_str(), parent_ros_name.c_str()); + if (new_publisher->Init()) { + _publishers.insert({actor, new_publisher}); + publisher = new_publisher; + } + std::shared_ptr new_transform = std::make_shared(ros_name.c_str(), parent_ros_name.c_str()); + if (new_transform->Init()) { + _transforms.insert({actor, new_transform}); + transform = new_transform; + } + } break; + case ESensors::RayCastSemanticLidar: { + if (ros_name == "ray_cast_semantic__") { + ros_name.pop_back(); + ros_name.pop_back(); + ros_name += string_id; + ROS2Interfaces->UpdateActorRosName(actor, ros_name); + } + std::shared_ptr new_publisher = std::make_shared(ros_name.c_str(), parent_ros_name.c_str()); + if (new_publisher->Init()) { + _publishers.insert({actor, new_publisher}); + publisher = new_publisher; + } + std::shared_ptr new_transform = std::make_shared(ros_name.c_str(), parent_ros_name.c_str()); + if (new_transform->Init()) { + _transforms.insert({actor, new_transform}); + transform = new_transform; + } + } break; + case ESensors::RayCastLidar: { + if (ros_name == "ray_cast__") { + ros_name.pop_back(); + ros_name.pop_back(); + ros_name += string_id; + ROS2Interfaces->UpdateActorRosName(actor, ros_name); + } + std::shared_ptr new_publisher = std::make_shared(ros_name.c_str(), parent_ros_name.c_str()); + if (new_publisher->Init()) { + _publishers.insert({actor, new_publisher}); + publisher = new_publisher; + } + std::shared_ptr new_transform = std::make_shared(ros_name.c_str(), parent_ros_name.c_str()); + if (new_transform->Init()) { + _transforms.insert({actor, new_transform}); + transform = new_transform; + } + } break; + case ESensors::RssSensor: { + std::cerr << "RSS sensor does not have an available publisher" << std::endl; + } break; + case ESensors::SceneCaptureCamera: { + if (ros_name == "rgb__") { + ros_name.pop_back(); + ros_name.pop_back(); + ros_name += string_id; + ROS2Interfaces->UpdateActorRosName(actor, ros_name); + } + std::shared_ptr new_publisher = std::make_shared(ros_name.c_str(), parent_ros_name.c_str()); + if (new_publisher->Init()) { + _publishers.insert({actor, new_publisher}); + publisher = new_publisher; + } + std::shared_ptr new_transform = std::make_shared(ros_name.c_str(), parent_ros_name.c_str()); + if (new_transform->Init()) { + _transforms.insert({actor, new_transform}); + transform = new_transform; + } + } break; + case ESensors::SemanticSegmentationCamera: { + if (ros_name == "semantic_segmentation__") { + ros_name.pop_back(); + ros_name.pop_back(); + ros_name += string_id; + ROS2Interfaces->UpdateActorRosName(actor, ros_name); + } + std::shared_ptr new_publisher = std::make_shared(ros_name.c_str(), parent_ros_name.c_str()); + if (new_publisher->Init()) { + _publishers.insert({actor, new_publisher}); + publisher = new_publisher; + } + std::shared_ptr new_transform = std::make_shared(ros_name.c_str(), parent_ros_name.c_str()); + if (new_transform->Init()) { + _transforms.insert({actor, new_transform}); + transform = new_transform; + } + } break; + case ESensors::InstanceSegmentationCamera: { + if (ros_name == "instance_segmentation__") { + ros_name.pop_back(); + ros_name.pop_back(); + ros_name += string_id; + ROS2Interfaces->UpdateActorRosName(actor, ros_name); + } + std::shared_ptr new_publisher = std::make_shared(ros_name.c_str(), parent_ros_name.c_str()); + if (new_publisher->Init()) { + _publishers.insert({actor, new_publisher}); + publisher = new_publisher; + } + std::shared_ptr new_transform = std::make_shared(ros_name.c_str(), parent_ros_name.c_str()); + if (new_transform->Init()) { + _transforms.insert({actor, new_transform}); + transform = new_transform; + } + } break; + case ESensors::WorldObserver: { + std::cerr << "World obserser does not have an available publisher" << std::endl; + } break; + case ESensors::CameraGBufferUint8: { + std::cerr << "Camera GBuffer uint8 does not have an available publisher" << std::endl; + } break; + case ESensors::CameraGBufferFloat: { + std::cerr << "Camera GBuffer float does not have an available publisher" << std::endl; + } break; + default: { + std::cerr << "Unknown sensor type" << std::endl; + } + } + } + return { publisher, transform }; +} + +void ROS2Carla::ProcessDataFromCamera( + uint64_t sensor_type, + carla::streaming::detail::stream_id_type stream_id, + const carla::geom::Transform sensor_transform, + int W, int H, float Fov, + const carla::SharedBufferView buffer, + void *actor) { + + switch (sensor_type) { + case ESensors::CollisionSensor: + log_info("Sensor Collision to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "buffer.", buffer->size()); + break; + case ESensors::DepthCamera: + { + log_info("Sensor DepthCamera to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "buffer.", buffer->size()); + auto sensors = GetOrCreateSensor(ESensors::DepthCamera, stream_id, actor); + if (sensors.first) { + std::shared_ptr publisher = std::dynamic_pointer_cast(sensors.first); + const carla::sensor::s11n::ImageSerializer::ImageHeader *header = + reinterpret_cast(buffer->data()); + if (!header) + return; + if (!publisher->HasBeenInitialized() || !(publisher->GetFov() == Fov)) + publisher->InitInfoData(0, 0, H, W, Fov, true); + publisher->SetImageData(_seconds, _nanoseconds, header->height, header->width, (const uint8_t*) (buffer->data() + carla::sensor::s11n::ImageSerializer::header_offset)); + publisher->SetCameraInfoData(_seconds, _nanoseconds); + publisher->Publish(); + } + if (sensors.second) { + std::shared_ptr publisher = std::dynamic_pointer_cast(sensors.second); + publisher->SetData(_seconds, _nanoseconds, (const float*)&sensor_transform.location, (const float*)&sensor_transform.rotation); + publisher->Publish(); + } + } + break; + case ESensors::NormalsCamera: + log_info("Sensor NormalsCamera to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "buffer.", buffer->size()); + { + auto sensors = GetOrCreateSensor(ESensors::NormalsCamera, stream_id, actor); + if (sensors.first) { + std::shared_ptr publisher = std::dynamic_pointer_cast(sensors.first); + const carla::sensor::s11n::ImageSerializer::ImageHeader *header = + reinterpret_cast(buffer->data()); + if (!header) + return; + if (!publisher->HasBeenInitialized() || !(publisher->GetFov() == Fov)) + publisher->InitInfoData(0, 0, H, W, Fov, true); + publisher->SetImageData(_seconds, _nanoseconds, header->height, header->width, (const uint8_t*) (buffer->data() + carla::sensor::s11n::ImageSerializer::header_offset)); + publisher->SetCameraInfoData(_seconds, _nanoseconds); + publisher->Publish(); + } + if (sensors.second) { + std::shared_ptr publisher = std::dynamic_pointer_cast(sensors.second); + publisher->SetData(_seconds, _nanoseconds, (const float*)&sensor_transform.location, (const float*)&sensor_transform.rotation); + publisher->Publish(); + } + } + break; + case ESensors::LaneInvasionSensor: + log_info("Sensor LaneInvasionSensor to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "buffer.", buffer->size()); + { + auto sensors = GetOrCreateSensor(ESensors::LaneInvasionSensor, stream_id, actor); + if (sensors.first) { + std::shared_ptr publisher = std::dynamic_pointer_cast(sensors.first); + publisher->SetData(_seconds, _nanoseconds, (const int32_t*) buffer->data()); + publisher->Publish(); + } + if (sensors.second) { + std::shared_ptr publisher = std::dynamic_pointer_cast(sensors.second); + publisher->SetData(_seconds, _nanoseconds, (const float*)&sensor_transform.location, (const float*)&sensor_transform.rotation); + publisher->Publish(); + } + } + break; + case ESensors::OpticalFlowCamera: + log_info("Sensor OpticalFlowCamera to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "buffer.", buffer->size()); + { + auto sensors = GetOrCreateSensor(ESensors::OpticalFlowCamera, stream_id, actor); + if (sensors.first) { + std::shared_ptr publisher = std::dynamic_pointer_cast(sensors.first); + const carla::sensor::s11n::OpticalFlowImageSerializer::ImageHeader *header = + reinterpret_cast(buffer->data()); + if (!header) + return; + if (!publisher->HasBeenInitialized() || !(publisher->GetFov() == Fov)) + publisher->InitInfoData(0, 0, H, W, Fov, true); + publisher->SetImageData(_seconds, _nanoseconds, header->height, header->width, (const float*) (buffer->data() + carla::sensor::s11n::OpticalFlowImageSerializer::header_offset)); + publisher->SetCameraInfoData(_seconds, _nanoseconds); + publisher->Publish(); + } + if (sensors.second) { + std::shared_ptr publisher = std::dynamic_pointer_cast(sensors.second); + publisher->SetData(_seconds, _nanoseconds, (const float*)&sensor_transform.location, (const float*)&sensor_transform.rotation); + publisher->Publish(); + } + } + break; + case ESensors::RssSensor: + log_info("Sensor RssSensor to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "buffer.", buffer->size()); + break; + case ESensors::SceneCaptureCamera: + { + log_info("Sensor SceneCaptureCamera to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "buffer.", buffer->size()); + { + auto sensors = GetOrCreateSensor(ESensors::SceneCaptureCamera, stream_id, actor); + if (sensors.first) { + std::shared_ptr publisher = std::dynamic_pointer_cast(sensors.first); + const carla::sensor::s11n::ImageSerializer::ImageHeader *header = + reinterpret_cast(buffer->data()); + if (!header) + return; + if (!publisher->HasBeenInitialized() || !(publisher->GetFov() == Fov)) + publisher->InitInfoData(0, 0, H, W, Fov, true); + publisher->SetImageData(_seconds, _nanoseconds, header->height, header->width, (const uint8_t*) (buffer->data() + carla::sensor::s11n::ImageSerializer::header_offset)); + publisher->SetCameraInfoData(_seconds, _nanoseconds); + publisher->Publish(); + } + if (sensors.second) { + std::shared_ptr publisher = std::dynamic_pointer_cast(sensors.second); + publisher->SetData(_seconds, _nanoseconds, (const float*)&sensor_transform.location, (const float*)&sensor_transform.rotation); + publisher->Publish(); + } + } + break; + } + case ESensors::SemanticSegmentationCamera: + log_info("Sensor SemanticSegmentationCamera to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "buffer.", buffer->size()); + { + auto sensors = GetOrCreateSensor(ESensors::SemanticSegmentationCamera, stream_id, actor); + if (sensors.first) { + std::shared_ptr publisher = std::dynamic_pointer_cast(sensors.first); + const carla::sensor::s11n::ImageSerializer::ImageHeader *header = + reinterpret_cast(buffer->data()); + if (!header) + return; + if (!publisher->HasBeenInitialized() || !(publisher->GetFov() == Fov)) + publisher->InitInfoData(0, 0, H, W, Fov, true); + publisher->SetImageData(_seconds, _nanoseconds, header->height, header->width, (const uint8_t*) (buffer->data() + carla::sensor::s11n::ImageSerializer::header_offset)); + publisher->SetCameraInfoData(_seconds, _nanoseconds); + publisher->Publish(); + } + if (sensors.second) { + std::shared_ptr publisher = std::dynamic_pointer_cast(sensors.second); + publisher->SetData(_seconds, _nanoseconds, (const float*)&sensor_transform.location, (const float*)&sensor_transform.rotation); + publisher->Publish(); + } + } + break; + case ESensors::InstanceSegmentationCamera: + log_info("Sensor InstanceSegmentationCamera to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "buffer.", buffer->size()); + { + auto sensors = GetOrCreateSensor(ESensors::InstanceSegmentationCamera, stream_id, actor); + if (sensors.first) { + std::shared_ptr publisher = std::dynamic_pointer_cast(sensors.first); + const carla::sensor::s11n::ImageSerializer::ImageHeader *header = + reinterpret_cast(buffer->data()); + if (!header) + return; + if (!publisher->HasBeenInitialized() || !(publisher->GetFov() == Fov)) + publisher->InitInfoData(0, 0, H, W, Fov, true); + publisher->SetImageData(_seconds, _nanoseconds, header->height, header->width, (const uint8_t*) (buffer->data() + carla::sensor::s11n::ImageSerializer::header_offset)); + publisher->SetCameraInfoData(_seconds, _nanoseconds); + publisher->Publish(); + } + if (sensors.second) { + std::shared_ptr publisher = std::dynamic_pointer_cast(sensors.second); + publisher->SetData(_seconds, _nanoseconds, (const float*)&sensor_transform.location, (const float*)&sensor_transform.rotation); + publisher->Publish(); + } + } + break; + case ESensors::WorldObserver: + log_info("Sensor WorldObserver to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "buffer.", buffer->size()); + break; + case ESensors::CameraGBufferUint8: + log_info("Sensor CameraGBufferUint8 to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "buffer.", buffer->size()); + break; + case ESensors::CameraGBufferFloat: + log_info("Sensor CameraGBufferFloat to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "buffer.", buffer->size()); + break; + default: + log_info("Sensor to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "buffer.", buffer->size()); + } +} + +void ROS2Carla::ProcessDataFromGNSS( + uint64_t sensor_type, + carla::streaming::detail::stream_id_type stream_id, + const carla::geom::Transform sensor_transform, + const carla::geom::GeoLocation &data, + void *actor) { + log_info("Sensor GnssSensor to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "geo.", data.latitude, data.longitude, data.altitude); + auto sensors = GetOrCreateSensor(ESensors::GnssSensor, stream_id, actor); + if (sensors.first) { + std::shared_ptr publisher = std::dynamic_pointer_cast(sensors.first); + publisher->SetData(_seconds, _nanoseconds, reinterpret_cast(&data)); + publisher->Publish(); + } + if (sensors.second) { + std::shared_ptr publisher = std::dynamic_pointer_cast(sensors.second); + publisher->SetData(_seconds, _nanoseconds, (const float*)&sensor_transform.location, (const float*)&sensor_transform.rotation); + publisher->Publish(); + } +} + +void ROS2Carla::ProcessDataFromIMU( + uint64_t sensor_type, + carla::streaming::detail::stream_id_type stream_id, + const carla::geom::Transform sensor_transform, + carla::geom::Vector3D accelerometer, + carla::geom::Vector3D gyroscope, + float compass, + void *actor) { + log_info("Sensor InertialMeasurementUnit to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "imu.", accelerometer.x, gyroscope.x, compass); + auto sensors = GetOrCreateSensor(ESensors::InertialMeasurementUnit, stream_id, actor); + if (sensors.first) { + std::shared_ptr publisher = std::dynamic_pointer_cast(sensors.first); + publisher->SetData(_seconds, _nanoseconds, reinterpret_cast(&accelerometer), reinterpret_cast(&gyroscope), compass); + publisher->Publish(); + } + if (sensors.second) { + std::shared_ptr publisher = std::dynamic_pointer_cast(sensors.second); + publisher->SetData(_seconds, _nanoseconds, (const float*)&sensor_transform.location, (const float*)&sensor_transform.rotation); + publisher->Publish(); + } +} + +void ROS2Carla::ProcessDataFromDVS( + uint64_t sensor_type, + carla::streaming::detail::stream_id_type stream_id, + const carla::geom::Transform sensor_transform, + const carla::SharedBufferView buffer, + int W, int H, float Fov, + void *actor) { + log_info("Sensor DVS to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id); + auto sensors = GetOrCreateSensor(ESensors::DVSCamera, stream_id, actor); + if (sensors.first) { + std::shared_ptr publisher = std::dynamic_pointer_cast(sensors.first); + const carla::sensor::s11n::ImageSerializer::ImageHeader *header = + reinterpret_cast(buffer->data()); + if (!header) + return; + if (!publisher->HasBeenInitialized() || !(publisher->GetFov() == Fov)) + publisher->InitInfoData(0, 0, H, W, Fov, true); + size_t elements = (buffer->size() - carla::sensor::s11n::ImageSerializer::header_offset) / sizeof(carla::sensor::data::DVSEvent); + publisher->SetImageData(_seconds, _nanoseconds, elements, header->height, header->width, (const uint8_t*) (buffer->data() + carla::sensor::s11n::ImageSerializer::header_offset)); + publisher->SetCameraInfoData(_seconds, _nanoseconds); + publisher->SetPointCloudData(1, elements * sizeof(carla::sensor::data::DVSEvent), elements, (const uint8_t*) (buffer->data() + carla::sensor::s11n::ImageSerializer::header_offset)); + publisher->Publish(); + } + if (sensors.second) { + std::shared_ptr publisher = std::dynamic_pointer_cast(sensors.second); + publisher->SetData(_seconds, _nanoseconds, (const float*)&sensor_transform.location, (const float*)&sensor_transform.rotation); + publisher->Publish(); + } +} + +void ROS2Carla::ProcessDataFromLidar( + uint64_t sensor_type, + carla::streaming::detail::stream_id_type stream_id, + const carla::geom::Transform sensor_transform, + carla::sensor::data::LidarData &data, + void *actor) { + log_info("Sensor Lidar to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "points.", data._points.size()); + auto sensors = GetOrCreateSensor(ESensors::RayCastLidar, stream_id, actor); + if (sensors.first) { + std::shared_ptr publisher = std::dynamic_pointer_cast(sensors.first); + size_t width = data._points.size(); + size_t height = 1; + publisher->SetData(_seconds, _nanoseconds, height, width, (float*)data._points.data()); + publisher->Publish(); + } + if (sensors.second) { + std::shared_ptr publisher = std::dynamic_pointer_cast(sensors.second); + publisher->SetData(_seconds, _nanoseconds, (const float*)&sensor_transform.location, (const float*)&sensor_transform.rotation); + publisher->Publish(); + } +} + +void ROS2Carla::ProcessDataFromSemanticLidar( + uint64_t sensor_type, + carla::streaming::detail::stream_id_type stream_id, + const carla::geom::Transform sensor_transform, + carla::sensor::data::SemanticLidarData &data, + void *actor) { + static_assert(sizeof(float) == sizeof(uint32_t), "Invalid float size"); + log_info("Sensor SemanticLidar to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "points.", data._ser_points.size()); + auto sensors = GetOrCreateSensor(ESensors::RayCastSemanticLidar, stream_id, actor); + if (sensors.first) { + std::shared_ptr publisher = std::dynamic_pointer_cast(sensors.first); + size_t width = data._ser_points.size(); + size_t height = 1; + publisher->SetData(_seconds, _nanoseconds, 6, height, width, (float*)data._ser_points.data()); + publisher->Publish(); + } + if (sensors.second) { + std::shared_ptr publisher = std::dynamic_pointer_cast(sensors.second); + publisher->SetData(_seconds, _nanoseconds, (const float*)&sensor_transform.location, (const float*)&sensor_transform.rotation); + publisher->Publish(); + } +} + +void ROS2Carla::ProcessDataFromRadar( + uint64_t sensor_type, + carla::streaming::detail::stream_id_type stream_id, + const carla::geom::Transform sensor_transform, + const carla::sensor::data::RadarData &data, + void *actor) { + log_info("Sensor Radar to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "points.", data._detections.size()); + auto sensors = GetOrCreateSensor(ESensors::Radar, stream_id, actor); + if (sensors.first) { + std::shared_ptr publisher = std::dynamic_pointer_cast(sensors.first); + size_t elements = data.GetDetectionCount(); + size_t width = elements * sizeof(carla::sensor::data::RadarDetection); + size_t height = 1; + publisher->SetData(_seconds, _nanoseconds, height, width, elements, (const uint8_t*)data._detections.data()); + publisher->Publish(); + } + if (sensors.second) { + std::shared_ptr publisher = std::dynamic_pointer_cast(sensors.second); + publisher->SetData(_seconds, _nanoseconds, (const float*)&sensor_transform.location, (const float*)&sensor_transform.rotation); + publisher->Publish(); + } +} + +void ROS2Carla::ProcessDataFromObstacleDetection( + uint64_t sensor_type, + carla::streaming::detail::stream_id_type stream_id, + const carla::geom::Transform sensor_transform, + AActor *first_ctor, + AActor *second_actor, + float distance, + void *actor) { + log_info("Sensor ObstacleDetector to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "distance.", distance); +} + +void ROS2Carla::ProcessDataFromCollisionSensor( + uint64_t sensor_type, + carla::streaming::detail::stream_id_type stream_id, + const carla::geom::Transform sensor_transform, + uint32_t other_actor, + carla::geom::Vector3D impulse, + void* actor) { + auto sensors = GetOrCreateSensor(ESensors::CollisionSensor, stream_id, actor); + if (sensors.first) { + std::shared_ptr publisher = std::dynamic_pointer_cast(sensors.first); + publisher->SetData(_seconds, _nanoseconds, other_actor, impulse.x, impulse.y, impulse.z); + publisher->Publish(); + } + if (sensors.second) { + std::shared_ptr publisher = std::dynamic_pointer_cast(sensors.second); + publisher->SetData(_seconds, _nanoseconds, (const float*)&sensor_transform.location, (const float*)&sensor_transform.rotation); + publisher->Publish(); + } +} + +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/ROS2Carla.h b/LibCarla/source/carla/ros2/ROS2Carla.h new file mode 100644 index 00000000000..1934fd49ff7 --- /dev/null +++ b/LibCarla/source/carla/ros2/ROS2Carla.h @@ -0,0 +1,172 @@ +// Copyright (c) 2024 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 . + +#pragma once + +#include "carla/ros2/ROS2Interfaces.h" +#include "carla/Buffer.h" +#include "carla/BufferView.h" +#include "carla/geom/Transform.h" +#include "carla/ros2/ROS2.h" +#include "carla/ros2/ROS2CallbackData.h" +#include "carla/streaming/detail/Types.h" + +#include +#include +#include +#include + +// forward declarations +class AActor; +namespace carla { + namespace geom { + class GeoLocation; + struct Vector3D; + } + namespace sensor { + namespace data { + struct DVSEvent; + class LidarData; + class SemanticLidarData; + class RadarData; + } + } +} + +namespace carla { +namespace ros2 { + + class CarlaPublisher; + class CarlaTransformPublisher; + class CarlaClockPublisher; + class CarlaEgoVehicleControlSubscriber; + class BasicSubscriber; + class BasicPublisher; + +class ROS2Carla : public ROS2 +{ + public: + + // deleting copy constructor for singleton + ROS2Carla(const ROS2Carla& obj) = delete; + static std::shared_ptr GetInstance() { + if (!_instance) + { + _instance = std::shared_ptr(new ROS2Carla); + auto ROS2Interfaces = carla::ros2::ROS2Interfaces::GetInstance(); + ROS2Interfaces->RegisterInterface(_instance); + } + return _instance; + } + + virtual void Enable(bool enable) override; + virtual void Shutdown() override; + virtual bool IsEnabled() override { return _enabled; } + virtual void SetFrame(uint64_t frame) override; + virtual void SetTimestamp(double timestamp) override; + + // ros_name managing + virtual void RemoveActorRosPublishers(void *actor) override; + + // callbacks + void AddActorCallback(void* actor, std::string ros_name, ActorCallback callback); + void RemoveActorCallback(void* actor); + void RemoveBasicSubscriberCallback(void* actor); + void AddBasicSubscriberCallback(void* actor, std::string ros_name, ActorMessageCallback callback); + + // receiving data to publish + void ProcessDataFromCamera( + uint64_t sensor_type, + carla::streaming::detail::stream_id_type stream_id, + const carla::geom::Transform sensor_transform, + int W, int H, float Fov, + const carla::SharedBufferView buffer, + void *actor = nullptr); + void ProcessDataFromGNSS( + uint64_t sensor_type, + carla::streaming::detail::stream_id_type stream_id, + const carla::geom::Transform sensor_transform, + const carla::geom::GeoLocation &data, + void *actor = nullptr); + void ProcessDataFromIMU( + uint64_t sensor_type, + carla::streaming::detail::stream_id_type stream_id, + const carla::geom::Transform sensor_transform, + carla::geom::Vector3D accelerometer, + carla::geom::Vector3D gyroscope, + float compass, + void *actor = nullptr); + void ProcessDataFromDVS( + uint64_t sensor_type, + carla::streaming::detail::stream_id_type stream_id, + const carla::geom::Transform sensor_transform, + const carla::SharedBufferView buffer, + int W, int H, float Fov, + void *actor = nullptr); + void ProcessDataFromLidar( + uint64_t sensor_type, + carla::streaming::detail::stream_id_type stream_id, + const carla::geom::Transform sensor_transform, + carla::sensor::data::LidarData &data, + void *actor = nullptr); + void ProcessDataFromSemanticLidar( + uint64_t sensor_type, + carla::streaming::detail::stream_id_type stream_id, + const carla::geom::Transform sensor_transform, + carla::sensor::data::SemanticLidarData &data, + void *actor = nullptr); + void ProcessDataFromRadar( + uint64_t sensor_type, + carla::streaming::detail::stream_id_type stream_id, + const carla::geom::Transform sensor_transform, + const carla::sensor::data::RadarData &data, + void *actor = nullptr); + void ProcessDataFromObstacleDetection( + uint64_t sensor_type, + carla::streaming::detail::stream_id_type stream_id, + const carla::geom::Transform sensor_transform, + AActor *first_actor, + AActor *second_actor, + float distance, + void *actor = nullptr); + void ProcessDataFromCollisionSensor( + uint64_t sensor_type, + carla::streaming::detail::stream_id_type stream_id, + const carla::geom::Transform sensor_transform, + uint32_t other_actor, + carla::geom::Vector3D impulse, + void* actor); + + // enabling streams to publish + virtual void EnableStream(carla::streaming::detail::stream_id_type id) { _publish_stream.insert(id); } + virtual bool IsStreamEnabled(carla::streaming::detail::stream_id_type id) { return _publish_stream.count(id) > 0; } + virtual void ResetStreams() { _publish_stream.clear(); } + + static std::shared_ptr _instance; + + private: + std::pair, std::shared_ptr> GetOrCreateSensor(int type, carla::streaming::detail::stream_id_type id, void* actor); + + // singleton + ROS2Carla() {}; + + + + std::shared_ptr _controller; + std::shared_ptr _clock_publisher; + std::unordered_map> _publishers; + std::unordered_map> _transforms; + std::unordered_set _publish_stream; + std::unordered_map _actor_callbacks; +#if defined(WITH_ROS2Carla_DEMO) + std::shared_ptr _basic_subscriber; + std::shared_ptr _basic_publisher; + std::unordered_map _actor_message_callbacks; +#endif +}; + +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/ROS2Interfaces.cpp b/LibCarla/source/carla/ros2/ROS2Interfaces.cpp new file mode 100644 index 00000000000..a06fef81846 --- /dev/null +++ b/LibCarla/source/carla/ros2/ROS2Interfaces.cpp @@ -0,0 +1,287 @@ +// Copyright (c) 2024 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 . + +#include "carla/Logging.h" +#include +#include "carla/ros2/ROS2.h" +#include "carla/geom/GeoLocation.h" +#include "carla/geom/Vector3D.h" +#include "carla/sensor/data/DVSEvent.h" +#include "carla/sensor/data/LidarData.h" +#include "carla/sensor/data/SemanticLidarData.h" +#include "carla/sensor/data/RadarData.h" +#include "carla/sensor/data/Image.h" +#include "carla/sensor/s11n/ImageSerializer.h" +#include "carla/sensor/s11n/SensorHeaderSerializer.h" + +#include "publishers/CarlaPublisher.h" +#include "publishers/CarlaClockPublisher.h" +#include "publishers/CarlaRGBCameraPublisher.h" +#include "publishers/CarlaDepthCameraPublisher.h" +#include "publishers/CarlaNormalsCameraPublisher.h" +#include "publishers/CarlaOpticalFlowCameraPublisher.h" +#include "publishers/CarlaSSCameraPublisher.h" +#include "publishers/CarlaISCameraPublisher.h" +#include "publishers/CarlaDVSCameraPublisher.h" +#include "publishers/CarlaLidarPublisher.h" +#include "publishers/CarlaSemanticLidarPublisher.h" +#include "publishers/CarlaRadarPublisher.h" +#include "publishers/CarlaIMUPublisher.h" +#include "publishers/CarlaGNSSPublisher.h" +#include "publishers/CarlaMapSensorPublisher.h" +#include "publishers/CarlaSpeedometerSensor.h" +#include "publishers/CarlaTransformPublisher.h" +#include "publishers/CarlaCollisionPublisher.h" +#include "publishers/CarlaLineInvasionPublisher.h" +#include "publishers/BasicPublisher.h" + +#include "subscribers/CarlaSubscriber.h" +#include "subscribers/CarlaEgoVehicleControlSubscriber.h" +#if defined(WITH_ROS2_DEMO) + #include "subscribers/BasicSubscriber.h" +#endif + +#include + +namespace carla { +namespace ros2 { + +// static fields +std::shared_ptr ROS2Interfaces::_instance = nullptr; + +void ROS2Interfaces::Enable(bool enabled) +{ + bool needToCleanInterfaces = false; + for (std::weak_ptr interfaceWeakPtr : _interfaces) + { + if (std::shared_ptr interfacePtr = interfaceWeakPtr.lock()) + { + interfacePtr->Enable(enabled); + } + else + { + needToCleanInterfaces = true; + } + + } + if (needToCleanInterfaces) + { + CleanExpiredInterfaces(); + } +} + +void ROS2Interfaces::Shutdown(){ + bool needToCleanInterfaces = false; + for (std::weak_ptr interfaceWeakPtr : _interfaces) + { + if (std::shared_ptr interfacePtr = interfaceWeakPtr.lock()) + { + if (interfacePtr->IsEnabled()) + interfacePtr->Shutdown(); + } + else + { + needToCleanInterfaces = true; + } + } + + if (needToCleanInterfaces) + { + CleanExpiredInterfaces(); + } + +} + +void ROS2Interfaces::SetFrame(uint64_t frame) +{ + bool needToCleanInterfaces = false; + for (std::weak_ptr interfaceWeakPtr : _interfaces) + { + if (std::shared_ptr interfacePtr = interfaceWeakPtr.lock()) + { + interfacePtr->SetFrame(frame); + } + else + { + needToCleanInterfaces = true; + } + } + if (needToCleanInterfaces) + { + CleanExpiredInterfaces(); + } +} + +void ROS2Interfaces::SetTimestamp(double timestamp) +{ + bool needToCleanInterfaces = false; + for (std::weak_ptr interfaceWeakPtr : _interfaces) + { + if (std::shared_ptr interfacePtr = interfaceWeakPtr.lock()) + { + interfacePtr->SetTimestamp(timestamp); + } + else + { + needToCleanInterfaces = true; + } + } + if (needToCleanInterfaces) + { + CleanExpiredInterfaces(); + } +} + +void ROS2Interfaces::RegisterActorWithInterfaces(FActorDescription& Description, std::string RosName, void* Actor) +{ + bool needToCleanInterfaces = false; + for (std::weak_ptr interfaceWeakPtr : _interfaces) + { + if (std::shared_ptr interfacePtr = interfaceWeakPtr.lock()) + { + interfacePtr->RegisterActor(Description, RosName, Actor); + } + else + { + needToCleanInterfaces = true; + } + } + if (needToCleanInterfaces) + { + CleanExpiredInterfaces(); + } +} + +void ROS2Interfaces::RemoveActorFromInterfaces(void* Actor) +{ + bool needToCleanInterfaces = false; + for (std::weak_ptr interfaceWeakPtr : _interfaces) + { + if (std::shared_ptr interfacePtr = interfaceWeakPtr.lock()) + { + interfacePtr->RemoveActor(Actor); + } + else + { + needToCleanInterfaces = true; + } + } + if (needToCleanInterfaces) + { + CleanExpiredInterfaces(); + } +} + +void ROS2Interfaces::RegisterInterface(std::shared_ptr newInterface) +{ + _interfaces.push_back(newInterface); +} + +void ROS2Interfaces::CleanExpiredInterfaces() +{ + _interfaces.erase(std::remove_if(_interfaces.begin(), _interfaces.end(), + [](const std::weak_ptr& weakPtr) { + return weakPtr.expired(); + }), + _interfaces.end()); +} + +void ROS2Interfaces::UnregisterInterface(std::shared_ptr interfaceToRemove) +{ + _interfaces.erase(std::remove_if(_interfaces.begin(), _interfaces.end(), + [&interfaceToRemove](const std::weak_ptr& weakPtr) { + if (auto sp = weakPtr.lock()) { + return sp == interfaceToRemove; + } + return false; // Skip expired weak_ptrs + }), + _interfaces.end()); +} + +void ROS2Interfaces::AddActorRosName(void *actor, std::string ros_name) { + _actor_ros_name.insert({actor, ros_name}); +} + +void ROS2Interfaces::AddActorParentRosName(void *actor, void* parent) { + auto it = _actor_parent_ros_name.find(actor); + if (it != _actor_parent_ros_name.end()) { + it->second.push_back(parent); + } else { + _actor_parent_ros_name.insert({actor, {parent}}); + } +} + +void ROS2Interfaces::RemoveActorRosName(void *actor) { + _actor_ros_name.erase(actor); + _actor_parent_ros_name.erase(actor); + + bool needToCleanInterfaces = false; + for (std::weak_ptr interfaceWeakPtr : _interfaces) + { + if (std::shared_ptr interfacePtr = interfaceWeakPtr.lock()) + { + interfacePtr->RemoveActorRosPublishers(actor); + } + else + { + needToCleanInterfaces = true; + } + } + if (needToCleanInterfaces) + { + CleanExpiredInterfaces(); + } +} + +void ROS2Interfaces::UpdateActorRosName(void *actor, std::string ros_name) { + auto it = _actor_ros_name.find(actor); + if (it != _actor_ros_name.end()) { + it->second = ros_name; + } +} + +std::string ROS2Interfaces::GetActorRosName(void *actor) { + auto it = _actor_ros_name.find(actor); + if (it != _actor_ros_name.end()) { + return it->second; + } else { + return std::string(""); + } +} + +std::string ROS2Interfaces::GetActorParentRosName(void *actor, bool chain_names) { + auto it = _actor_parent_ros_name.find(actor); + if (it != _actor_parent_ros_name.end()) + { + const std::string current_actor_name = GetActorRosName(actor); + std::string parent_name; + for (auto parent_it = it->second.cbegin(); parent_it != it->second.cend(); ++parent_it) + { + const std::string name = GetActorRosName(*parent_it); + if (name == current_actor_name) + { + continue; + } + if (name.empty()) + { + continue; + } + if (!chain_names) + { + return name; + } + parent_name = name + '/' + parent_name; + } + if (parent_name.back() == '/') + parent_name.pop_back(); + return parent_name; + } + else + return std::string(""); +} + +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/ROS2Interfaces.h b/LibCarla/source/carla/ros2/ROS2Interfaces.h new file mode 100644 index 00000000000..f93e6f7c8da --- /dev/null +++ b/LibCarla/source/carla/ros2/ROS2Interfaces.h @@ -0,0 +1,75 @@ +// Copyright (c) 2024 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 . + +#pragma once + +#include "carla/Buffer.h" +#include "carla/BufferView.h" +#include "carla/geom/Transform.h" +#include "carla/ros2/ROS2CallbackData.h" +#include "carla/streaming/detail/Types.h" + +#include +#include +#include +#include + + +struct FActorDescription; + +namespace carla { +namespace ros2 { + +class ROS2; + +class ROS2Interfaces +{ + public: + + // deleting copy constructor for singleton + ROS2Interfaces(const ROS2Interfaces& obj) = delete; + static std::shared_ptr GetInstance() { + if (!_instance) + _instance = std::shared_ptr(new ROS2Interfaces); + return _instance; + } + + // general + void Enable(bool enabled); + void Shutdown(); + void SetFrame(uint64_t frame); + void SetTimestamp(double timestamp); + void RegisterInterface(std::shared_ptr newInterface); + void RegisterActorWithInterfaces(FActorDescription& Description, std::string RosName, void* Actor); + void RemoveActorFromInterfaces(void* Actor); + void UnregisterInterface(std::shared_ptr interfaceToRemove); + void CleanExpiredInterfaces(); + + // ros_name managing + void AddActorRosName(void *actor, std::string ros_name); + void AddActorParentRosName(void *actor, void* parent); + void RemoveActorRosName(void *actor); + void UpdateActorRosName(void *actor, std::string ros_name); + std::string GetActorRosName(void *actor); + std::string GetActorParentRosName(void *actor, bool chain_names=true); + + // singleton + ROS2Interfaces() {}; + + private: + + static std::shared_ptr _instance; + + bool _enabled = false; + std::vector> _interfaces; + + std::unordered_map _actor_ros_name; + std::unordered_map > _actor_parent_ros_name; + +}; + +} // namespace ros2 +} // namespace carla diff --git a/LibCarla/source/carla/ros2/listeners/CarlaVehicleSubscriberListener.cpp b/LibCarla/source/carla/ros2/listeners/CarlaVehicleSubscriberListener.cpp new file mode 100644 index 00000000000..d97f76c571e --- /dev/null +++ b/LibCarla/source/carla/ros2/listeners/CarlaVehicleSubscriberListener.cpp @@ -0,0 +1,114 @@ +#include "CarlaVehicleSubscriberListener.h" +#include + +#include +#include +#include +#include +#include "carla/ros2/types/CarlaEgoVehicleControl.h" +#include "carla/ros2/subscribers/CarlaEgoVehicleControlSubscriber.h" +#include "carla/ros2/ROS2CallbackData.h" + +namespace carla { +namespace ros2 { + + namespace efd = eprosima::fastdds::dds; + using erc = eprosima::fastrtps::types::ReturnCode_t; + + class CarlaVehicleSubscriberListenerImpl : public efd::DataReaderListener { + public: + void on_subscription_matched( + efd::DataReader* reader, + const efd::SubscriptionMatchedStatus& info) override; + void on_data_available(efd::DataReader* reader) override; + + int _matched {0}; + bool _first_connected {false}; + CarlaEgoVehicleControlSubscriber* _owner {nullptr}; + carla_msgs::msg::CarlaEgoVehicleControl _message {}; + }; + + void CarlaVehicleSubscriberListenerImpl::on_subscription_matched(efd::DataReader* reader, const efd::SubscriptionMatchedStatus& info) + { + if (info.current_count_change == 1) { + _matched = info.total_count; + _first_connected = true; + } else if (info.current_count_change == -1) { + _matched = info.total_count; + if (_matched == 0) { + _owner->DestroySubscriber(); + } + } else { + std::cerr << info.current_count_change + << " is not a valid value for PublicationMatchedStatus current count change" << std::endl; + } + } + + void CarlaVehicleSubscriberListenerImpl::on_data_available(efd::DataReader* reader) + { + efd::SampleInfo info; + eprosima::fastrtps::types::ReturnCode_t rcode = reader->take_next_sample(&_message, &info); + if (rcode == erc::ReturnCodeValue::RETCODE_OK) { + VehicleControl control; + control.throttle = _message.throttle(); + control.steer = _message.steer(); + control.brake = _message.brake(); + control.hand_brake = _message.hand_brake(); + control.reverse = _message.reverse(); + control.gear = _message.gear(); + control.manual_gear_shift = _message.manual_gear_shift(); + _owner->ForwardMessage(control); + } + if (rcode == erc::ReturnCodeValue::RETCODE_ERROR) { + std::cerr << "RETCODE_ERROR" << std::endl; + } + if (rcode == erc::ReturnCodeValue::RETCODE_UNSUPPORTED) { + std::cerr << "RETCODE_UNSUPPORTED" << std::endl; + } + if (rcode == erc::ReturnCodeValue::RETCODE_BAD_PARAMETER) { + std::cerr << "RETCODE_BAD_PARAMETER" << std::endl; + } + if (rcode == erc::ReturnCodeValue::RETCODE_PRECONDITION_NOT_MET) { + std::cerr << "RETCODE_PRECONDITION_NOT_MET" << std::endl; + } + if (rcode == erc::ReturnCodeValue::RETCODE_OUT_OF_RESOURCES) { + std::cerr << "RETCODE_OUT_OF_RESOURCES" << std::endl; + } + if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ENABLED) { + std::cerr << "RETCODE_NOT_ENABLED" << std::endl; + } + if (rcode == erc::ReturnCodeValue::RETCODE_IMMUTABLE_POLICY) { + std::cerr << "RETCODE_IMMUTABLE_POLICY" << std::endl; + } + if (rcode == erc::ReturnCodeValue::RETCODE_INCONSISTENT_POLICY) { + std::cerr << "RETCODE_INCONSISTENT_POLICY" << std::endl; + } + if (rcode == erc::ReturnCodeValue::RETCODE_ALREADY_DELETED) { + std::cerr << "RETCODE_ALREADY_DELETED" << std::endl; + } + if (rcode == erc::ReturnCodeValue::RETCODE_TIMEOUT) { + std::cerr << "RETCODE_TIMEOUT" << std::endl; + } + if (rcode == erc::ReturnCodeValue::RETCODE_NO_DATA) { + std::cerr << "RETCODE_NO_DATA" << std::endl; + } + if (rcode == erc::ReturnCodeValue::RETCODE_ILLEGAL_OPERATION) { + std::cerr << "RETCODE_ILLEGAL_OPERATION" << std::endl; + } + if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ALLOWED_BY_SECURITY) { + std::cerr << "RETCODE_NOT_ALLOWED_BY_SECURITY" << std::endl; + } + } + + void CarlaVehicleSubscriberListener::SetOwner(CarlaEgoVehicleControlSubscriber* owner) { + _impl->_owner = owner; + } + + CarlaVehicleSubscriberListener::CarlaVehicleSubscriberListener(CarlaEgoVehicleControlSubscriber* owner) : + _impl(std::make_unique()) { + _impl->_owner = owner; + } + + CarlaVehicleSubscriberListener::~CarlaVehicleSubscriberListener() {} + +}} diff --git a/LibCarla/source/carla/ros2/listeners/CarlaVehicleSubscriberListener.h b/LibCarla/source/carla/ros2/listeners/CarlaVehicleSubscriberListener.h new file mode 100644 index 00000000000..187db8e36c9 --- /dev/null +++ b/LibCarla/source/carla/ros2/listeners/CarlaVehicleSubscriberListener.h @@ -0,0 +1,30 @@ +// Copyright (c) 2024 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 . + +#pragma once +#define _GLIBCXX_USE_CXX11_ABI 0 + +#include + +namespace carla { +namespace ros2 { + + class CarlaVehicleSubscriberListenerImpl; + class CarlaEgoVehicleControlSubscriber; + + class CarlaVehicleSubscriberListener { + public: + CarlaVehicleSubscriberListener(CarlaEgoVehicleControlSubscriber* owner); + ~CarlaVehicleSubscriberListener(); + CarlaVehicleSubscriberListener(const CarlaVehicleSubscriberListener&) = delete; + CarlaVehicleSubscriberListener& operator=(const CarlaVehicleSubscriberListener&) = delete; + CarlaVehicleSubscriberListener(CarlaVehicleSubscriberListener&&) = delete; + CarlaVehicleSubscriberListener& operator=(CarlaVehicleSubscriberListener&&) = delete; + + void SetOwner(CarlaEgoVehicleControlSubscriber* owner); + + std::unique_ptr _impl; + }; +} +} diff --git a/LibCarla/source/carla/ros2/plugin-utils/disable-fastdds-include.h b/LibCarla/source/carla/ros2/plugin-utils/disable-fastdds-include.h new file mode 100644 index 00000000000..a9eb443cb9f --- /dev/null +++ b/LibCarla/source/carla/ros2/plugin-utils/disable-fastdds-include.h @@ -0,0 +1,5 @@ +#undef _MSC_VER +#undef __INTERNALDEBUG +#undef __APPLE__ +#undef __FreeBSD__ +#undef __VXWORKS__ \ No newline at end of file diff --git a/LibCarla/source/carla/ros2/plugin-utils/enable-fastdds-include.h b/LibCarla/source/carla/ros2/plugin-utils/enable-fastdds-include.h new file mode 100644 index 00000000000..871a49c63ff --- /dev/null +++ b/LibCarla/source/carla/ros2/plugin-utils/enable-fastdds-include.h @@ -0,0 +1,5 @@ +#define _MSC_VER 0 +#define __INTERNALDEBUG 1 +#define __APPLE__ 0 +#define __FreeBSD__ 0 +#define __VXWORKS__ 0 \ No newline at end of file diff --git a/LibCarla/source/carla/ros2/publishers/CarlaDVSCameraPublisher.cpp b/LibCarla/source/carla/ros2/publishers/CarlaDVSCameraPublisher.cpp index ad39e49a1b4..3eb0f3809b9 100644 --- a/LibCarla/source/carla/ros2/publishers/CarlaDVSCameraPublisher.cpp +++ b/LibCarla/source/carla/ros2/publishers/CarlaDVSCameraPublisher.cpp @@ -67,10 +67,15 @@ namespace ros2 { return _info->_init; } + float CarlaDVSCameraPublisher::GetFov() const { + return _fov; + } + void CarlaDVSCameraPublisher::InitInfoData(uint32_t x_offset, uint32_t y_offset, uint32_t height, uint32_t width, float fov, bool do_rectify) { _info->_ci = std::move(sensor_msgs::msg::CameraInfo(height, width, fov)); SetInfoRegionOfInterest(x_offset, y_offset, height, width, do_rectify); _info->_init = true; + _fov = fov; } bool CarlaDVSCameraPublisher::Init() { diff --git a/LibCarla/source/carla/ros2/publishers/CarlaDVSCameraPublisher.h b/LibCarla/source/carla/ros2/publishers/CarlaDVSCameraPublisher.h index 0bf3e6b3435..d7dd3ed9798 100644 --- a/LibCarla/source/carla/ros2/publishers/CarlaDVSCameraPublisher.h +++ b/LibCarla/source/carla/ros2/publishers/CarlaDVSCameraPublisher.h @@ -30,6 +30,7 @@ namespace ros2 { bool Publish(); bool HasBeenInitialized() const; + float GetFov() const; void SetImageData(int32_t seconds, uint32_t nanoseconds, size_t elements, size_t height, size_t width, const uint8_t* data); void SetCameraInfoData(int32_t seconds, uint32_t nanoseconds); void SetPointCloudData(size_t height, size_t width, size_t elements, const uint8_t* data); @@ -49,6 +50,7 @@ namespace ros2 { bool PublishPointCloud(); private: + float _fov; std::shared_ptr _impl; std::shared_ptr _info; std::shared_ptr _point_cloud; diff --git a/LibCarla/source/carla/ros2/publishers/CarlaDepthCameraPublisher.cpp b/LibCarla/source/carla/ros2/publishers/CarlaDepthCameraPublisher.cpp index 01d002bb1df..af0e9fcbd22 100644 --- a/LibCarla/source/carla/ros2/publishers/CarlaDepthCameraPublisher.cpp +++ b/LibCarla/source/carla/ros2/publishers/CarlaDepthCameraPublisher.cpp @@ -54,10 +54,15 @@ namespace ros2 { return _impl_info->_init; } + float CarlaDepthCameraPublisher::GetFov() const { + return _fov; + } + void CarlaDepthCameraPublisher::InitInfoData(uint32_t x_offset, uint32_t y_offset, uint32_t height, uint32_t width, float fov, bool do_rectify) { _impl_info->_info = std::move(sensor_msgs::msg::CameraInfo(height, width, fov)); SetInfoRegionOfInterest(x_offset, y_offset, height, width, do_rectify); _impl_info->_init = true; + _fov = fov; } bool CarlaDepthCameraPublisher::Init() { diff --git a/LibCarla/source/carla/ros2/publishers/CarlaDepthCameraPublisher.h b/LibCarla/source/carla/ros2/publishers/CarlaDepthCameraPublisher.h index 0ddcd31ca2f..256d5ccdbe9 100644 --- a/LibCarla/source/carla/ros2/publishers/CarlaDepthCameraPublisher.h +++ b/LibCarla/source/carla/ros2/publishers/CarlaDepthCameraPublisher.h @@ -29,6 +29,7 @@ namespace ros2 { bool Publish(); bool HasBeenInitialized() const; + float GetFov() const; void SetImageData(int32_t seconds, uint32_t nanoseconds, size_t height, size_t width, const uint8_t* data); void SetCameraInfoData(int32_t seconds, uint32_t nanoseconds); const char* type() const override { return "depth camera"; } @@ -43,6 +44,7 @@ namespace ros2 { void SetData(int32_t seconds, uint32_t nanoseconds, size_t height, size_t width, std::vector&& data); private: + float _fov; std::shared_ptr _impl; std::shared_ptr _impl_info; }; diff --git a/LibCarla/source/carla/ros2/publishers/CarlaISCameraPublisher.cpp b/LibCarla/source/carla/ros2/publishers/CarlaISCameraPublisher.cpp index 78570e0a69d..4aeb566ea25 100644 --- a/LibCarla/source/carla/ros2/publishers/CarlaISCameraPublisher.cpp +++ b/LibCarla/source/carla/ros2/publishers/CarlaISCameraPublisher.cpp @@ -53,10 +53,15 @@ namespace ros2 { return _impl_info->_init; } + float CarlaISCameraPublisher::GetFov() const { + return _fov; + } + void CarlaISCameraPublisher::InitInfoData(uint32_t x_offset, uint32_t y_offset, uint32_t height, uint32_t width, float fov, bool do_rectify) { _impl_info->_info = std::move(sensor_msgs::msg::CameraInfo(height, width, fov)); SetInfoRegionOfInterest(x_offset, y_offset, height, width, do_rectify); _impl_info->_init = true; + _fov = fov; } bool CarlaISCameraPublisher::Init() { diff --git a/LibCarla/source/carla/ros2/publishers/CarlaISCameraPublisher.h b/LibCarla/source/carla/ros2/publishers/CarlaISCameraPublisher.h index e63a69bedd7..eaa7554890e 100644 --- a/LibCarla/source/carla/ros2/publishers/CarlaISCameraPublisher.h +++ b/LibCarla/source/carla/ros2/publishers/CarlaISCameraPublisher.h @@ -29,6 +29,7 @@ namespace ros2 { bool Publish(); bool HasBeenInitialized() const; + float GetFov() const; void SetImageData(int32_t seconds, uint32_t nanoseconds, size_t height, size_t width, const uint8_t* data); void SetCameraInfoData(int32_t seconds, uint32_t nanoseconds); const char* type() const override { return "instance segmentation"; } @@ -43,6 +44,7 @@ namespace ros2 { void SetData(int32_t seconds, uint32_t nanoseconds, size_t height, size_t width, std::vector&& data); private: + float _fov; std::shared_ptr _impl; std::shared_ptr _impl_info; }; diff --git a/LibCarla/source/carla/ros2/publishers/CarlaNormalsCameraPublisher.cpp b/LibCarla/source/carla/ros2/publishers/CarlaNormalsCameraPublisher.cpp index 4b2fce968a7..834cb929f5e 100644 --- a/LibCarla/source/carla/ros2/publishers/CarlaNormalsCameraPublisher.cpp +++ b/LibCarla/source/carla/ros2/publishers/CarlaNormalsCameraPublisher.cpp @@ -54,10 +54,15 @@ namespace ros2 { return _impl_info->_init; } + float CarlaNormalsCameraPublisher::GetFov() const { + return _fov; + } + void CarlaNormalsCameraPublisher::InitInfoData(uint32_t x_offset, uint32_t y_offset, uint32_t height, uint32_t width, float fov, bool do_rectify) { _impl_info->_info = std::move(sensor_msgs::msg::CameraInfo(height, width, fov)); SetInfoRegionOfInterest(x_offset, y_offset, height, width, do_rectify); _impl_info->_init = true; + _fov = fov; } bool CarlaNormalsCameraPublisher::Init() { diff --git a/LibCarla/source/carla/ros2/publishers/CarlaNormalsCameraPublisher.h b/LibCarla/source/carla/ros2/publishers/CarlaNormalsCameraPublisher.h index db2030c4234..f5cbac99019 100644 --- a/LibCarla/source/carla/ros2/publishers/CarlaNormalsCameraPublisher.h +++ b/LibCarla/source/carla/ros2/publishers/CarlaNormalsCameraPublisher.h @@ -29,6 +29,7 @@ namespace ros2 { bool Publish(); bool HasBeenInitialized() const; + float GetFov() const; void SetImageData(int32_t seconds, uint32_t nanoseconds, size_t height, size_t width, const uint8_t* data); void SetCameraInfoData(int32_t seconds, uint32_t nanoseconds); @@ -44,6 +45,7 @@ namespace ros2 { void SetData(int32_t seconds, uint32_t nanoseconds, size_t height, size_t width, std::vector&& data); private: + float _fov; std::shared_ptr _impl; std::shared_ptr _impl_info; }; diff --git a/LibCarla/source/carla/ros2/publishers/CarlaOpticalFlowCameraPublisher.cpp b/LibCarla/source/carla/ros2/publishers/CarlaOpticalFlowCameraPublisher.cpp index f3095b3b0be..d7b85cbd579 100644 --- a/LibCarla/source/carla/ros2/publishers/CarlaOpticalFlowCameraPublisher.cpp +++ b/LibCarla/source/carla/ros2/publishers/CarlaOpticalFlowCameraPublisher.cpp @@ -58,10 +58,15 @@ namespace ros2 { return _impl_info->_init; } + float CarlaOpticalFlowCameraPublisher::GetFov() const { + return _fov; + } + void CarlaOpticalFlowCameraPublisher::InitInfoData(uint32_t x_offset, uint32_t y_offset, uint32_t height, uint32_t width, float fov, bool do_rectify) { _impl_info->_info = std::move(sensor_msgs::msg::CameraInfo(height, width, fov)); SetInfoRegionOfInterest(x_offset, y_offset, height, width, do_rectify); _impl_info->_init = true; + _fov = fov; } bool CarlaOpticalFlowCameraPublisher::Init() { diff --git a/LibCarla/source/carla/ros2/publishers/CarlaOpticalFlowCameraPublisher.h b/LibCarla/source/carla/ros2/publishers/CarlaOpticalFlowCameraPublisher.h index b230db3f086..2236eb57414 100644 --- a/LibCarla/source/carla/ros2/publishers/CarlaOpticalFlowCameraPublisher.h +++ b/LibCarla/source/carla/ros2/publishers/CarlaOpticalFlowCameraPublisher.h @@ -29,6 +29,7 @@ namespace ros2 { bool Publish(); bool HasBeenInitialized() const; + float GetFov() const; void SetImageData(int32_t seconds, uint32_t nanoseconds, size_t height, size_t width, const float* data); void SetCameraInfoData(int32_t seconds, uint32_t nanoseconds); const char* type() const override { return "optical flow camera"; } @@ -43,6 +44,7 @@ namespace ros2 { void SetData(int32_t seconds, uint32_t nanoseconds, size_t height, size_t width, std::vector&& data); private: + float _fov; std::shared_ptr _impl; std::shared_ptr _impl_info; }; diff --git a/LibCarla/source/carla/ros2/publishers/CarlaRGBCameraPublisher.cpp b/LibCarla/source/carla/ros2/publishers/CarlaRGBCameraPublisher.cpp index 1ca42f567aa..49c7a22f98a 100644 --- a/LibCarla/source/carla/ros2/publishers/CarlaRGBCameraPublisher.cpp +++ b/LibCarla/source/carla/ros2/publishers/CarlaRGBCameraPublisher.cpp @@ -54,10 +54,15 @@ namespace ros2 { return _impl_info->_init; } + float CarlaRGBCameraPublisher::GetFov() const { + return _fov; + } + void CarlaRGBCameraPublisher::InitInfoData(uint32_t x_offset, uint32_t y_offset, uint32_t height, uint32_t width, float fov, bool do_rectify) { _impl_info->_info = std::move(sensor_msgs::msg::CameraInfo(height, width, fov)); SetInfoRegionOfInterest(x_offset, y_offset, height, width, do_rectify); _impl_info->_init = true; + _fov = fov; } bool CarlaRGBCameraPublisher::Init() { diff --git a/LibCarla/source/carla/ros2/publishers/CarlaRGBCameraPublisher.h b/LibCarla/source/carla/ros2/publishers/CarlaRGBCameraPublisher.h index 26a081d7321..e032a2d0aaf 100644 --- a/LibCarla/source/carla/ros2/publishers/CarlaRGBCameraPublisher.h +++ b/LibCarla/source/carla/ros2/publishers/CarlaRGBCameraPublisher.h @@ -29,6 +29,7 @@ namespace ros2 { bool Publish(); bool HasBeenInitialized() const; + float GetFov() const; void SetImageData(int32_t seconds, uint32_t nanoseconds, uint32_t height, uint32_t width, const uint8_t* data); void SetCameraInfoData(int32_t seconds, uint32_t nanoseconds); const char* type() const override { return "rgb camera"; } @@ -43,6 +44,7 @@ namespace ros2 { void SetInfoRegionOfInterest(uint32_t x_offset, uint32_t y_offset, uint32_t height, uint32_t width, bool do_rectify); private: + float _fov; std::shared_ptr _impl; std::shared_ptr _impl_info; }; diff --git a/LibCarla/source/carla/ros2/publishers/CarlaSSCameraPublisher.cpp b/LibCarla/source/carla/ros2/publishers/CarlaSSCameraPublisher.cpp index 23cb56852c9..490e5c05fc5 100644 --- a/LibCarla/source/carla/ros2/publishers/CarlaSSCameraPublisher.cpp +++ b/LibCarla/source/carla/ros2/publishers/CarlaSSCameraPublisher.cpp @@ -53,10 +53,15 @@ namespace ros2 { return _impl_info->_init; } + float CarlaSSCameraPublisher::GetFov() const { + return _fov; + } + void CarlaSSCameraPublisher::InitInfoData(uint32_t x_offset, uint32_t y_offset, uint32_t height, uint32_t width, float fov, bool do_rectify) { _impl_info->_info = std::move(sensor_msgs::msg::CameraInfo(height, width, fov)); SetInfoRegionOfInterest(x_offset, y_offset, height, width, do_rectify); _impl_info->_init = true; + _fov = fov; } bool CarlaSSCameraPublisher::Init() { diff --git a/LibCarla/source/carla/ros2/publishers/CarlaSSCameraPublisher.h b/LibCarla/source/carla/ros2/publishers/CarlaSSCameraPublisher.h index 61bdbcb00d1..7519cc0f95c 100644 --- a/LibCarla/source/carla/ros2/publishers/CarlaSSCameraPublisher.h +++ b/LibCarla/source/carla/ros2/publishers/CarlaSSCameraPublisher.h @@ -29,6 +29,7 @@ namespace ros2 { bool Publish(); bool HasBeenInitialized() const; + float GetFov() const; void SetImageData(int32_t seconds, uint32_t nanoseconds, size_t height, size_t width, const uint8_t* data); void SetCameraInfoData(int32_t seconds, uint32_t nanoseconds); const char* type() const override { return "semantic segmentation"; } @@ -43,6 +44,7 @@ namespace ros2 { void SetData(int32_t seconds, uint32_t nanoseconds, size_t height, size_t width, std::vector&& data); private: + float _fov; std::shared_ptr _impl; std::shared_ptr _impl_info; }; diff --git a/LibCarla/source/carla/ros2/publishers/CarlaTransformPublisher.cpp b/LibCarla/source/carla/ros2/publishers/CarlaTransformPublisher.cpp index fb622f14fdf..5cfc4a292c7 100644 --- a/LibCarla/source/carla/ros2/publishers/CarlaTransformPublisher.cpp +++ b/LibCarla/source/carla/ros2/publishers/CarlaTransformPublisher.cpp @@ -186,7 +186,11 @@ namespace ros2 { std_msgs::msg::Header header; header.stamp(std::move(time)); - header.frame_id(_parent); + + if (!_parent_standalone.empty()) + header.frame_id(_parent_standalone); + else + header.frame_id(_parent); geometry_msgs::msg::Transform t; t.rotation(_impl->vec_rotation); @@ -199,10 +203,11 @@ namespace ros2 { _impl->_transform.transforms({ts}); } - CarlaTransformPublisher::CarlaTransformPublisher(const char* ros_name, const char* parent) : + CarlaTransformPublisher::CarlaTransformPublisher(const char* ros_name, const char* parent, const char* parent_standalone) : _impl(std::make_shared()) { _name = ros_name; _parent = parent; + _parent_standalone = parent_standalone; } CarlaTransformPublisher::~CarlaTransformPublisher() { @@ -226,6 +231,7 @@ namespace ros2 { _frame_id = other._frame_id; _name = other._name; _parent = other._parent; + _parent_standalone = other._parent_standalone; _impl = other._impl; } @@ -233,6 +239,7 @@ namespace ros2 { _frame_id = other._frame_id; _name = other._name; _parent = other._parent; + _parent_standalone = other._parent_standalone; _impl = other._impl; return *this; @@ -242,6 +249,7 @@ namespace ros2 { _frame_id = std::move(other._frame_id); _name = std::move(other._name); _parent = std::move(other._parent); + _parent_standalone = std::move(other._parent_standalone); _impl = std::move(other._impl); } @@ -249,6 +257,7 @@ namespace ros2 { _frame_id = std::move(other._frame_id); _name = std::move(other._name); _parent = std::move(other._parent); + _parent_standalone = std::move(other._parent_standalone); _impl = std::move(other._impl); return *this; diff --git a/LibCarla/source/carla/ros2/publishers/CarlaTransformPublisher.h b/LibCarla/source/carla/ros2/publishers/CarlaTransformPublisher.h index 1e98f347f23..d626fe3fef9 100644 --- a/LibCarla/source/carla/ros2/publishers/CarlaTransformPublisher.h +++ b/LibCarla/source/carla/ros2/publishers/CarlaTransformPublisher.h @@ -16,7 +16,7 @@ namespace ros2 { class CarlaTransformPublisher : public CarlaPublisher { public: - CarlaTransformPublisher(const char* ros_name = "", const char* parent = ""); + CarlaTransformPublisher(const char* ros_name = "", const char* parent = "", const char* parent_standalone = ""); ~CarlaTransformPublisher(); CarlaTransformPublisher(const CarlaTransformPublisher&); CarlaTransformPublisher& operator=(const CarlaTransformPublisher&); @@ -30,6 +30,7 @@ namespace ros2 { private: std::shared_ptr _impl; + std::string _parent_standalone = ""; }; } } diff --git a/LibCarla/source/carla/ros2/subscribers/CarlaEgoVehicleControlSubscriber.cpp b/LibCarla/source/carla/ros2/subscribers/CarlaEgoVehicleControlSubscriber.cpp index 0089336e07c..d87c3c6a796 100644 --- a/LibCarla/source/carla/ros2/subscribers/CarlaEgoVehicleControlSubscriber.cpp +++ b/LibCarla/source/carla/ros2/subscribers/CarlaEgoVehicleControlSubscriber.cpp @@ -2,7 +2,7 @@ #include "carla/ros2/types/CarlaEgoVehicleControl.h" #include "carla/ros2/types/CarlaEgoVehicleControlPubSubTypes.h" -#include "carla/ros2/listeners/CarlaSubscriberListener.h" +#include "carla/ros2/listeners/CarlaVehicleSubscriberListener.h" #include #include @@ -34,7 +34,7 @@ namespace ros2 { efd::Topic* _topic { nullptr }; efd::DataReader* _datareader { nullptr }; efd::TypeSupport _type { new carla_msgs::msg::CarlaEgoVehicleControlPubSubType() }; - CarlaSubscriberListener _listener {nullptr}; + CarlaVehicleSubscriberListener _listener {nullptr}; carla_msgs::msg::CarlaEgoVehicleControl _event {}; VehicleControl _control {}; bool _new_message {false}; diff --git a/LibCarla/source/carla/sensor/RawData.h b/LibCarla/source/carla/sensor/RawData.h index 64348c9fa5e..da49ae74638 100644 --- a/LibCarla/source/carla/sensor/RawData.h +++ b/LibCarla/source/carla/sensor/RawData.h @@ -8,7 +8,7 @@ #include "carla/Buffer.h" #include "carla/sensor/s11n/SensorHeaderSerializer.h" -#include "carla/ros2/ROS2.h" +#include "carla/ros2/ROS2Carla.h" #include #include @@ -89,7 +89,7 @@ namespace sensor { template friend class CompositeSerializer; - friend class carla::ros2::ROS2; + friend class carla::ros2::ROS2Carla; RawData(Buffer &&buffer) : _buffer(std::move(buffer)) {} diff --git a/LibCarla/source/carla/sensor/data/ImageTmpl.h b/LibCarla/source/carla/sensor/data/ImageTmpl.h index 6b2944af4ee..3c6b5512fa7 100644 --- a/LibCarla/source/carla/sensor/data/ImageTmpl.h +++ b/LibCarla/source/carla/sensor/data/ImageTmpl.h @@ -13,7 +13,7 @@ #include "carla/sensor/s11n/GBufferUint8Serializer.h" #include "carla/sensor/s11n/GBufferFloatSerializer.h" #include "carla/sensor/s11n/NormalsImageSerializer.h" -#include "carla/ros2/ROS2.h" +#include "carla/ros2/ROS2Carla.h" namespace carla { namespace sensor { @@ -23,7 +23,7 @@ namespace data { template class ImageTmpl : public Array { using Super = Array; - friend class carla::ros2::ROS2; + friend class carla::ros2::ROS2Carla; protected: using Serializer = s11n::ImageSerializer; diff --git a/LibCarla/source/carla/sensor/data/LidarData.h b/LibCarla/source/carla/sensor/data/LidarData.h index 80ab455d93f..a699e3429c7 100644 --- a/LibCarla/source/carla/sensor/data/LidarData.h +++ b/LibCarla/source/carla/sensor/data/LidarData.h @@ -15,7 +15,7 @@ namespace carla { namespace ros2 { - class ROS2; + class ROS2Carla; } namespace sensor { @@ -112,7 +112,7 @@ namespace data { friend class s11n::LidarSerializer; friend class s11n::LidarHeaderView; - friend class carla::ros2::ROS2; + friend class carla::ros2::ROS2Carla; }; } // namespace s11n diff --git a/LibCarla/source/carla/sensor/data/RadarData.h b/LibCarla/source/carla/sensor/data/RadarData.h index 2953ae65b1f..bcedbdea18b 100644 --- a/LibCarla/source/carla/sensor/data/RadarData.h +++ b/LibCarla/source/carla/sensor/data/RadarData.h @@ -13,7 +13,7 @@ namespace carla { namespace ros2 { - class ROS2; + class ROS2Carla; } namespace sensor { @@ -78,7 +78,7 @@ namespace data { std::vector _detections; friend class s11n::RadarSerializer; - friend class carla::ros2::ROS2; + friend class carla::ros2::ROS2Carla; }; } // namespace s11n diff --git a/LibCarla/source/carla/sensor/data/SemanticLidarData.h b/LibCarla/source/carla/sensor/data/SemanticLidarData.h index e8883b4c8e6..f65704ec14e 100644 --- a/LibCarla/source/carla/sensor/data/SemanticLidarData.h +++ b/LibCarla/source/carla/sensor/data/SemanticLidarData.h @@ -15,7 +15,7 @@ namespace carla { namespace ros2 { - class ROS2; + class ROS2Carla; } namespace sensor { @@ -144,7 +144,7 @@ namespace data { friend class s11n::SemanticLidarHeaderView; friend class s11n::SemanticLidarSerializer; - friend class carla::ros2::ROS2; + friend class carla::ros2::ROS2Carla; }; diff --git a/Ros2Native/CMakeLists.txt b/Ros2Native/CMakeLists.txt index 18b8d61a454..9846a626152 100644 --- a/Ros2Native/CMakeLists.txt +++ b/Ros2Native/CMakeLists.txt @@ -68,6 +68,24 @@ add_custom_command ( ${CARLA_PLUGIN_BINARY_PATH} ) +add_custom_command ( + TARGET carla-ros2-native-lib + POST_BUILD + COMMAND + ${CMAKE_COMMAND} -E copy + ${CARLA_PLUGIN_BINARY_PATH}/libfastcdr.so + ${CARLA_PLUGIN_BINARY_PATH}/libfastcdr-${CARLA_FASTCDR_FILE_VERSION_NAME}.lib +) + +add_custom_command ( + TARGET carla-ros2-native-lib + POST_BUILD + COMMAND + ${CMAKE_COMMAND} -E copy + ${CARLA_PLUGIN_BINARY_PATH}/libfastrtps.so + ${CARLA_PLUGIN_BINARY_PATH}/libfastrtps-${CARLA_FASTDDS_FILE_VERSION_NAME}.lib +) + carla_add_custom_target ( carla-ros2-native "Build the ROS2-Native CARLA subproject." diff --git a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Actor/ActorBlueprintFunctionLibrary.cpp b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Actor/ActorBlueprintFunctionLibrary.cpp index 3e995682305..ff9d723df49 100644 --- a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Actor/ActorBlueprintFunctionLibrary.cpp +++ b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Actor/ActorBlueprintFunctionLibrary.cpp @@ -158,13 +158,16 @@ class FActorDefinitionValidator FScopedStack Stack; }; -template -static FString JoinStrings(const FString &Separator, ARGS && ... Args) +static FString VectorToFString(const FVector &TextVector) { - return FString::Join(TArray{std::forward(Args) ...}, *Separator); + return UActorBlueprintFunctionLibrary::JoinStrings( + TEXT(","), + FString::SanitizeFloat(TextVector.X), + FString::SanitizeFloat(TextVector.Y), + FString::SanitizeFloat(TextVector.Z)); } -static FString ColorToFString(const FColor &Color) +FString UActorBlueprintFunctionLibrary::ColorToFString(const FColor &Color) { return JoinStrings( TEXT(","), @@ -173,15 +176,6 @@ static FString ColorToFString(const FColor &Color) FString::FromInt(Color.B)); } -static FString VectorToFString(const FVector &TextVector) -{ - return JoinStrings( - TEXT(","), - FString::SanitizeFloat(TextVector.X), - FString::SanitizeFloat(TextVector.Y), - FString::SanitizeFloat(TextVector.Z)); -} - /// ============================================================================ /// -- Actor definition validators --------------------------------------------- /// ============================================================================ @@ -202,30 +196,7 @@ bool UActorBlueprintFunctionLibrary::CheckActorDefinitions(const TArray -static void FillIdAndTags(FActorDefinition &Def, TStrs && ... Strings) -{ - Def.Id = JoinStrings(TEXT("."), std::forward(Strings) ...).ToLower(); - Def.Tags = JoinStrings(TEXT(","), std::forward(Strings) ...).ToLower(); - - // each actor gets an actor role name attribute (empty by default) - FActorVariation ActorRole; - ActorRole.Id = TEXT("role_name"); - ActorRole.Type = EActorAttributeType::String; - ActorRole.RecommendedValues = { TEXT("default") }; - ActorRole.bRestrictToRecommended = false; - Def.Variations.Emplace(ActorRole); - - // ROS2 - FActorVariation Var; - Var.Id = TEXT("ros_name"); - Var.Type = EActorAttributeType::String; - Var.RecommendedValues = { Def.Id }; - Var.bRestrictToRecommended = false; - Def.Variations.Emplace(Var); -} - -static void AddRecommendedValuesForActorRoleName( +void UActorBlueprintFunctionLibrary::AddRecommendedValuesForActorRoleName( FActorDefinition &Definition, TArray &&RecommendedValues) { @@ -239,7 +210,7 @@ static void AddRecommendedValuesForActorRoleName( } } -static void AddRecommendedValuesForSensorRoleNames(FActorDefinition &Definition) +void UActorBlueprintFunctionLibrary::AddRecommendedValuesForSensorRoleNames(FActorDefinition &Definition) { AddRecommendedValuesForActorRoleName(Definition, {TEXT("front"), TEXT("back"), TEXT("left"), TEXT( "right"), TEXT("front_left"), TEXT("front_right"), TEXT("back_left"), TEXT("back_right")}); @@ -275,7 +246,7 @@ static void AddVariationsForTrigger(FActorDefinition &Def) { FActorVariation ExtentCoordinate; - ExtentCoordinate.Id = JoinStrings(TEXT("_"), Extent, Coordinate); + ExtentCoordinate.Id = UActorBlueprintFunctionLibrary::JoinStrings(TEXT("_"), Extent, Coordinate); ExtentCoordinate.Type = EActorAttributeType::Float; ExtentCoordinate.RecommendedValues = { TEXT("1.0f") }; ExtentCoordinate.bRestrictToRecommended = false; @@ -1263,24 +1234,6 @@ void UActorBlueprintFunctionLibrary::MakeVehicleDefinition( Success = CheckActorDefinition(Definition); } -template -static void FillActorDefinitionArray( - const TArray &ParameterArray, - TArray &Definitions, - Functor Maker) -{ - for (auto &Item : ParameterArray) - { - FActorDefinition Definition; - bool Success = false; - Maker(Item, Success, Definition); - if (Success) - { - Definitions.Emplace(std::move(Definition)); - } - } -} - void UActorBlueprintFunctionLibrary::MakeVehicleDefinitions( const TArray &ParameterArray, TArray &Definitions) diff --git a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Actor/ActorBlueprintFunctionLibrary.h b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Actor/ActorBlueprintFunctionLibrary.h index f40e728a500..d0e7edbef92 100644 --- a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Actor/ActorBlueprintFunctionLibrary.h +++ b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Actor/ActorBlueprintFunctionLibrary.h @@ -26,7 +26,7 @@ struct FLidarDescription; struct FActorDescription; UCLASS() -class UActorBlueprintFunctionLibrary : public UBlueprintFunctionLibrary +class CARLA_API UActorBlueprintFunctionLibrary : public UBlueprintFunctionLibrary { GENERATED_BODY() @@ -53,6 +53,60 @@ class UActorBlueprintFunctionLibrary : public UBlueprintFunctionLibrary /// ========================================================================== /// @{ + static void AddRecommendedValuesForActorRoleName( + FActorDefinition &Definition, + TArray &&RecommendedValues); + + static void AddRecommendedValuesForSensorRoleNames(FActorDefinition &Definition); + + template + static void FillActorDefinitionArray( + const TArray &ParameterArray, + TArray &Definitions, + Functor Maker) + { + for (auto &Item : ParameterArray) + { + FActorDefinition Definition; + bool Success = false; + Maker(Item, Success, Definition); + if (Success) + { + Definitions.Emplace(std::move(Definition)); + } + } + } + template + static FString JoinStrings(const FString &Separator, ARGS && ... Args) + { + return FString::Join(TArray{std::forward(Args) ...}, *Separator); + } + + template + static void FillIdAndTags(FActorDefinition &Def, TStrs && ... Strings) + { + Def.Id = JoinStrings(TEXT("."), std::forward(Strings) ...).ToLower(); + Def.Tags = JoinStrings(TEXT(","), std::forward(Strings) ...).ToLower(); + + // each actor gets an actor role name attribute (empty by default) + FActorVariation ActorRole; + ActorRole.Id = TEXT("role_name"); + ActorRole.Type = EActorAttributeType::String; + ActorRole.RecommendedValues = { TEXT("default") }; + ActorRole.bRestrictToRecommended = false; + Def.Variations.Emplace(ActorRole); + + // ROS2 + FActorVariation Var; + Var.Id = TEXT("ros_name"); + Var.Type = EActorAttributeType::String; + Var.RecommendedValues = { Def.Id }; + Var.bRestrictToRecommended = false; + Def.Variations.Emplace(Var); + } + + static FString ColorToFString(const FColor &Color); + static FActorDefinition MakeGenericDefinition( const FString &Category, const FString &Type, diff --git a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Actor/ActorData.h b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Actor/ActorData.h index 797cb3a4995..b7791193834 100644 --- a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Actor/ActorData.h +++ b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Actor/ActorData.h @@ -25,7 +25,7 @@ class UCarlaEpisode; class UTrafficLightController; class FCarlaActor; -class FActorData +class CARLA_API FActorData { public: diff --git a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Actor/ActorDispatcher.cpp b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Actor/ActorDispatcher.cpp index 8f8ce6932f7..8f702fcacf3 100644 --- a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Actor/ActorDispatcher.cpp +++ b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Actor/ActorDispatcher.cpp @@ -16,12 +16,18 @@ #include "GameFramework/Controller.h" #ifdef WITH_ROS2 #include - #include "carla/ros2/ROS2.h" + #include + #include #include #include #endif #include +std::shared_ptr UActorDispatcher::GetInterfaces() +{ + return carla::ros2::ROS2Interfaces::GetInstance(); +} + void UActorDispatcher::Bind(FActorDefinition Definition, SpawnFunctionType Functor) { if (UActorBlueprintFunctionLibrary::CheckActorDefinition(Definition)) @@ -156,6 +162,11 @@ bool UActorDispatcher::DestroyActor(FCarlaActor::IdType ActorId) } } + #ifdef WITH_ROS2 + auto ROS2Interfaces = carla::ros2::ROS2Interfaces::GetInstance(); + ROS2Interfaces->RemoveActorFromInterfaces(reinterpret_cast(Actor)); + #endif + Registry.Deregister(ActorId); return true; @@ -173,8 +184,9 @@ FCarlaActor* UActorDispatcher::RegisterActor( // ROS2 mapping of actor->ros_name #ifdef WITH_ROS2 - auto ROS2 = carla::ros2::ROS2::GetInstance(); - if (ROS2->IsEnabled()) + auto ROS2Carla = carla::ros2::ROS2Carla::GetInstance(); + auto ROS2Interfaces = carla::ros2::ROS2Interfaces::GetInstance(); + if (ROS2Carla->IsEnabled()) { // actor ros_name std::string RosName; @@ -190,18 +202,18 @@ FCarlaActor* UActorDispatcher::RegisterActor( if(RosName.find("vehicle") != std::string::npos) { std::string VehicleName = "vehicle" + std::to_string(View->GetActorId()); - ROS2->AddActorRosName(static_cast(&Actor), VehicleName); + ROS2Interfaces->AddActorRosName(static_cast(&Actor), VehicleName); } else { size_t pos = RosName.find_last_of('.'); if (pos != std::string::npos) { std::string lastToken = RosName.substr(pos + 1) + "__"; - ROS2->AddActorRosName(static_cast(&Actor), lastToken); + ROS2Interfaces->AddActorRosName(static_cast(&Actor), lastToken); } } } else { - ROS2->AddActorRosName(static_cast(&Actor), RosName); + ROS2Interfaces->AddActorRosName(static_cast(&Actor), RosName); } // vehicle controller for hero @@ -209,14 +221,14 @@ FCarlaActor* UActorDispatcher::RegisterActor( { if (Attr.Key == "role_name" && (Attr.Value.Value == "hero" || Attr.Value.Value == "ego")) { - ROS2->AddActorCallback(static_cast(&Actor), RosName, [RosName](void *Actor, carla::ros2::ROS2CallbackData Data) -> void + ROS2Carla->AddActorCallback(static_cast(&Actor), RosName, [RosName](void *Actor, carla::ros2::ROS2CallbackData Data) -> void { AActor *UEActor = reinterpret_cast(Actor); ActorROS2Handler Handler(UEActor, RosName); std::visit(Handler, Data); }); #if defined(WITH_ROS2_DEMO) - ROS2->AddBasicSubscriberCallback(static_cast(&Actor), RosName, [RosName](void *Actor, carla::ros2::ROS2MessageCallbackData Data) -> void + ROS2Carla->AddBasicSubscriberCallback(static_cast(&Actor), RosName, [RosName](void *Actor, carla::ros2::ROS2MessageCallbackData Data) -> void { AActor *UEActor = reinterpret_cast(Actor); ActorROS2Handler Handler(UEActor, RosName); @@ -253,10 +265,7 @@ void UActorDispatcher::OnActorDestroyed(AActor *Actor) } #ifdef WITH_ROS2 - auto ROS2 = carla::ros2::ROS2::GetInstance(); - if (ROS2->IsEnabled()) - { - ROS2->RemoveActorRosName(reinterpret_cast(Actor)); - } + auto ROS2Interfaces = carla::ros2::ROS2Interfaces::GetInstance(); + ROS2Interfaces->RemoveActorRosName(reinterpret_cast(Actor)); #endif } diff --git a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Actor/ActorDispatcher.h b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Actor/ActorDispatcher.h index 8b3b238f7f9..08e4fdb0b2e 100644 --- a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Actor/ActorDispatcher.h +++ b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Actor/ActorDispatcher.h @@ -14,6 +14,11 @@ #include #include "Containers/Array.h" #include "Templates/Function.h" +#ifdef WITH_ROS2 + #include + #include + #include +#endif #include #include "ActorDispatcher.generated.h" @@ -31,6 +36,8 @@ class CARLA_API UActorDispatcher : public UObject using SpawnFunctionType = TFunction; + static std::shared_ptr GetInterfaces(); + /// Bind a definition to a spawn function. When SpawnActor is called with a /// matching description @a Functor is called. /// diff --git a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Actor/ActorRegistry.cpp b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Actor/ActorRegistry.cpp index 5d4409eac32..7936424833a 100644 --- a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Actor/ActorRegistry.cpp +++ b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Actor/ActorRegistry.cpp @@ -7,6 +7,7 @@ #include "Carla/Actor/ActorRegistry.h" #include "Carla.h" #include "Carla/Actor/ActorData.h" +#include "Carla/Actor/CarlaActorConstructorFactory.h" #include "Carla/Game/Tagger.h" #include "Carla/Traffic/TrafficLightBase.h" #include "Carla/Util/BoundingBoxCalculator.h" @@ -22,38 +23,6 @@ namespace crp = carla::rpc; FActorRegistry::IdType FActorRegistry::ID_COUNTER = 0u; -static FCarlaActor::ActorType FActorRegistry_GetActorType(const AActor *Actor) -{ - if (!Actor) - { - return FCarlaActor::ActorType::INVALID; - } - - if (nullptr != Cast(Actor)) - { - return FCarlaActor::ActorType::Vehicle; - } - else if (nullptr != Cast(Actor)) - { - return FCarlaActor::ActorType::Walker; - } - else if (nullptr != Cast(Actor)) - { - return FCarlaActor::ActorType::TrafficLight; - } - else if (nullptr != Cast(Actor)) - { - return FCarlaActor::ActorType::TrafficSign; - } - else if (nullptr != Cast(Actor)) - { - return FCarlaActor::ActorType::Sensor; - } - else - { - return FCarlaActor::ActorType::Other; - } -} FString CarlaGetRelevantTagAsString(const TSet &SemanticTags) { @@ -173,11 +142,15 @@ TSharedPtr FActorRegistry::MakeCarlaActor( std::begin(Token.data), std::end(Token.data)); } - auto Type = FActorRegistry_GetActorType(&Actor); + auto Type = CarlaActorConstructorFactory::Instance().GetActorType(&Actor); + FString CustomType; + if (Type == FCarlaActor::ActorType::Custom){ + CustomType = CarlaActorConstructorFactory::Instance().GetActorCustomType(&Actor); + } TSharedPtr CarlaActor = - FCarlaActor::ConstructCarlaActor( + CarlaActorConstructorFactory::Instance().ConstructCarlaActor( Id, &Actor, - std::move(Info), Type, + std::move(Info), Type, CustomType, InState, Actor.GetWorld()); return CarlaActor; } diff --git a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Actor/CarlaActor.cpp b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Actor/CarlaActor.cpp index 33d82f7e4e8..414b815346f 100644 --- a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Actor/CarlaActor.cpp +++ b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Actor/CarlaActor.cpp @@ -40,6 +40,8 @@ #include "GameFramework/CharacterMovementComponent.h" #include +const FString FCarlaActor::CustomType = ""; + FCarlaActor::FCarlaActor( IdType ActorId, AActor* Actor, @@ -120,37 +122,6 @@ FOtherActor::FOtherActor( ActorData = MakeShared(); } -TSharedPtr FCarlaActor::ConstructCarlaActor( - IdType ActorId, - AActor* Actor, - TSharedPtr Info, - ActorType Type, - carla::rpc::ActorState InState, - UWorld* World) -{ - switch(Type) - { - case ActorType::TrafficSign: - return MakeShared(ActorId, Actor, std::move(Info), InState, World); - break; - case ActorType::TrafficLight: - return MakeShared(ActorId, Actor, std::move(Info), InState, World); - break; - case ActorType::Vehicle: - return MakeShared(ActorId, Actor, std::move(Info), InState, World); - break; - case ActorType::Walker: - return MakeShared(ActorId, Actor, std::move(Info), InState, World); - break; - case ActorType::Sensor: - return MakeShared(ActorId, Actor, std::move(Info), InState, World); - break; - default: - return MakeShared(ActorId, Actor, std::move(Info), InState, World); - break; - } -} - // Base FCarlaActor functions --------------------- void FCarlaActor::PutActorToSleep(UCarlaEpisode* CarlaEpisode) diff --git a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Actor/CarlaActor.h b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Actor/CarlaActor.h index 62afba47167..0ffadd64a52 100644 --- a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Actor/CarlaActor.h +++ b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Actor/CarlaActor.h @@ -21,7 +21,7 @@ class AActor; /// A view over an actor and its properties. -class FCarlaActor +class CARLA_API FCarlaActor { public: @@ -35,6 +35,7 @@ class FCarlaActor TrafficLight, TrafficSign, Sensor, + Custom, INVALID }; @@ -88,6 +89,11 @@ class FCarlaActor return Type; } + FString GetCustomType() const + { + return CustomType; + } + AActor *GetActor() { return TheActor; @@ -431,16 +437,6 @@ class FCarlaActor return ECarlaServerResponse::ActorTypeMismatch; } - // Sensor functions - - static TSharedPtr ConstructCarlaActor( - IdType ActorId, - AActor* Actor, - TSharedPtr Info, - ActorType Type, - carla::rpc::ActorState InState, - UWorld* World); - private: friend class FActorRegistry; @@ -463,6 +459,8 @@ class FCarlaActor ActorType Type = ActorType::INVALID; + static const FString CustomType; + TSharedPtr ActorData = nullptr; UWorld *World = nullptr; diff --git a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Actor/CarlaActorConstructorFactory.cpp b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Actor/CarlaActorConstructorFactory.cpp new file mode 100644 index 00000000000..949a291c396 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Actor/CarlaActorConstructorFactory.cpp @@ -0,0 +1,112 @@ +// Copyright (c) 2024 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 . + +#include "CarlaActorConstructorFactory.h" +#include "CarlaActor.h" + + +void CarlaActorConstructorFactory::Register(const FString& ClassName, UClass* ClassType, CreatorFunc CreatorFunction) { + CreatorFunctions.Add(ClassName, CreatorFunction); + RegisteredClasses.Add(ClassName, ClassType); +} + +TSharedPtr CarlaActorConstructorFactory::Create(const FString& className, + FCarlaActor::IdType ActorId, + AActor* Actor, + TSharedPtr Info, + carla::rpc::ActorState InState, + UWorld* World) const { + if (CreatorFunctions.Contains(className)) + { + return (*CreatorFunctions.Find(className))(ActorId, Actor, std::move(Info), InState, World); + } + return nullptr; +} + +TSharedPtr CarlaActorConstructorFactory::ConstructCarlaActor( + FCarlaActor::IdType ActorId, + AActor* Actor, + TSharedPtr Info, + FCarlaActor::ActorType Type, + FString CustomType, + carla::rpc::ActorState InState, + UWorld* World) +{ + switch(Type) + { + case FCarlaActor::ActorType::TrafficSign: + return MakeShared(ActorId, Actor, std::move(Info), InState, World); + break; + case FCarlaActor::ActorType::TrafficLight: + return MakeShared(ActorId, Actor, std::move(Info), InState, World); + break; + case FCarlaActor::ActorType::Vehicle: + return MakeShared(ActorId, Actor, std::move(Info), InState, World); + break; + case FCarlaActor::ActorType::Walker: + return MakeShared(ActorId, Actor, std::move(Info), InState, World); + break; + case FCarlaActor::ActorType::Sensor: + return MakeShared(ActorId, Actor, std::move(Info), InState, World); + break; + case FCarlaActor::ActorType::Custom: + return Create(CustomType, ActorId, Actor, std::move(Info), InState, World); + break; + default: + return MakeShared(ActorId, Actor, std::move(Info), InState, World); + break; + } +} + +FCarlaActor::ActorType CarlaActorConstructorFactory::GetActorType(const AActor *Actor) +{ + if (!Actor) + { + return FCarlaActor::ActorType::INVALID; + } + + if (nullptr != Cast(Actor)) + { + return FCarlaActor::ActorType::Vehicle; + } + else if (nullptr != Cast(Actor)) + { + return FCarlaActor::ActorType::Walker; + } + else if (nullptr != Cast(Actor)) + { + return FCarlaActor::ActorType::TrafficLight; + } + else if (nullptr != Cast(Actor)) + { + return FCarlaActor::ActorType::TrafficSign; + } + else if (nullptr != Cast(Actor)) + { + return FCarlaActor::ActorType::Sensor; + } + else + { + for (const auto& Pair : RegisteredClasses) { + UClass* ClassType = Pair.Value; // UClass* from registration + if (Actor->IsA(ClassType)) { + return FCarlaActor::ActorType::Custom; + } + } + return FCarlaActor::ActorType::Other; + } +} + +FString CarlaActorConstructorFactory::GetActorCustomType(const AActor *Actor) +{ + for (const auto& Pair : RegisteredClasses) { + UClass* ClassType = Pair.Value; // UClass* from registration + if (Actor->IsA(ClassType)) { + return Pair.Key; + } + } + return ""; +} \ No newline at end of file diff --git a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Actor/CarlaActorConstructorFactory.h b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Actor/CarlaActorConstructorFactory.h new file mode 100644 index 00000000000..7b810b3f984 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Actor/CarlaActorConstructorFactory.h @@ -0,0 +1,70 @@ +// Copyright (c) 2024 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 . + +#pragma once + +#include "CarlaActor.h" + +/// A view over an actor and its properties. +class CARLA_API CarlaActorConstructorFactory +{ +public: + + using CreatorFunc = std::function(FCarlaActor::IdType ActorId, + AActor* Actor, + TSharedPtr Info, + carla::rpc::ActorState InState, + UWorld* World)>; + + static CarlaActorConstructorFactory& Instance() { + static CarlaActorConstructorFactory instance; + return instance; + } + + virtual ~CarlaActorConstructorFactory() {}; + + void Register(const FString& ClassName, UClass* ClassType, CreatorFunc Creator); + + TSharedPtr Create(const FString& className, + FCarlaActor::IdType ActorId, + AActor* Actor, + TSharedPtr Info, + carla::rpc::ActorState InState, + UWorld* World) const; + + FCarlaActor::ActorType GetActorType(const AActor *Actor); + FString GetActorCustomType(const AActor *Actor); + + TSharedPtr ConstructCarlaActor( + FCarlaActor::IdType ActorId, + AActor* Actor, + TSharedPtr Info, + FCarlaActor::ActorType Type, + FString CustomType, + carla::rpc::ActorState InState, + UWorld* World); + +protected: + TMap CreatorFunctions; + TMap RegisteredClasses; + + CarlaActorConstructorFactory() = default; // Private constructor for singleton +}; + +#define REGISTER_CLASS(className, classPointer) \ + namespace { \ + const bool registered_##className = []() { \ + CarlaActorConstructorFactory::Instance().Register(#className, #classPointer, []( \ + FCarlaActor::IdType ActorId, \ + AActor* Actor, \ + TSharedPtr Info, \ + carla::rpc::ActorState InState, \ + UWorld* World) -> std::unique_ptr { \ + return MakeShared(ActorId, Actor, std::move(Info), InState, World); \ + }); \ + return true; \ + }(); \ + } \ No newline at end of file diff --git a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Game/CarlaEngine.cpp b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Game/CarlaEngine.cpp index 78ce703e53f..eb30c5a398d 100644 --- a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Game/CarlaEngine.cpp +++ b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Game/CarlaEngine.cpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include #include @@ -65,9 +66,8 @@ FCarlaEngine::~FCarlaEngine() if (bIsRunning) { #if defined(WITH_ROS2) - auto ROS2 = carla::ros2::ROS2::GetInstance(); - if (ROS2->IsEnabled()) - ROS2->Shutdown(); + auto ROS2Interfaces = carla::ros2::ROS2Interfaces::GetInstance(); + ROS2Interfaces->Shutdown(); #endif FWorldDelegates::OnWorldTickStart.Remove(OnPreTickHandle); FWorldDelegates::OnWorldPostActorTick.Remove(OnPostTickHandle); @@ -222,9 +222,9 @@ void FCarlaEngine::NotifyInitGame(const UCarlaSettings &Settings) if (Settings.ROS2) { UE_LOG(LogCarla, Log, TEXT("ROS2: Creating ROS2 Instance...")); - auto ROS2 = carla::ros2::ROS2::GetInstance(); + auto ROS2Interfaces = carla::ros2::ROS2Interfaces::GetInstance(); UE_LOG(LogCarla, Log, TEXT("ROS2: Enabling ROS2...")); - ROS2->Enable(true); + ROS2Interfaces->Enable(true); UE_LOG(LogCarla, Log, TEXT("ROS2: ROS2 enabled...")); } else { UE_LOG(LogCarla, Log, TEXT("ROS2: ROS2 enabled...")); diff --git a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Game/CarlaEngine.h b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Game/CarlaEngine.h index 7c44a19bfdb..7b188d796b4 100644 --- a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Game/CarlaEngine.h +++ b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Game/CarlaEngine.h @@ -20,6 +20,7 @@ #include #if WITH_ROS2 #include + #include #endif #include @@ -75,9 +76,8 @@ class FCarlaEngine : private NonCopyable { FCarlaEngine::FrameCounter += 1; #if defined(WITH_ROS2) - auto ROS2 = carla::ros2::ROS2::GetInstance(); - if (ROS2->IsEnabled()) - ROS2->SetFrame(FCarlaEngine::FrameCounter); + auto ROS2Interfaces = carla::ros2::ROS2Interfaces::GetInstance(); + ROS2Interfaces->SetFrame(FCarlaEngine::FrameCounter); #endif return FCarlaEngine::FrameCounter; } @@ -86,9 +86,8 @@ class FCarlaEngine : private NonCopyable { FCarlaEngine::FrameCounter = Value; #if defined(WITH_ROS2) - auto ROS2 = carla::ros2::ROS2::GetInstance(); - if (ROS2->IsEnabled()) - ROS2->SetFrame(FCarlaEngine::FrameCounter); + auto ROS2Interfaces = carla::ros2::ROS2Interfaces::GetInstance(); + ROS2Interfaces->SetFrame(FCarlaEngine::FrameCounter); #endif } diff --git a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Game/CarlaEpisode.h b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Game/CarlaEpisode.h index 65d8a90fda4..b693732861a 100644 --- a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Game/CarlaEpisode.h +++ b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Game/CarlaEpisode.h @@ -21,6 +21,7 @@ #include #if WITH_ROS2 #include + #include #endif #include #include @@ -364,9 +365,8 @@ class CARLA_API UCarlaEpisode : public UObject ElapsedGameTime += DeltaSeconds; SetVisualGameTime(VisualGameTime + DeltaSeconds); #if defined(WITH_ROS2) - auto ROS2 = carla::ros2::ROS2::GetInstance(); - if (ROS2->IsEnabled()) - ROS2->SetTimestamp(GetElapsedGameTime()); + auto ROS2Interfaces = carla::ros2::ROS2Interfaces::GetInstance(); + ROS2Interfaces->SetTimestamp(GetElapsedGameTime()); #endif } diff --git a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Sensor/CollisionSensor.cpp b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Sensor/CollisionSensor.cpp index 38d4b24a536..50bc7df5935 100644 --- a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Sensor/CollisionSensor.cpp +++ b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Sensor/CollisionSensor.cpp @@ -93,8 +93,8 @@ void ACollisionSensor::OnCollisionEvent( // ROS2 #if defined(WITH_ROS2) - auto ROS2 = carla::ros2::ROS2::GetInstance(); - if (ROS2->IsEnabled()) + auto ROS2Carla = carla::ros2::ROS2Carla::GetInstance(); + if (ROS2Carla->IsEnabled()) { TRACE_CPUPROFILER_EVENT_SCOPE_STR("ROS2 Send"); auto StreamId = carla::streaming::detail::token_type(GetToken()).get_stream_id(); @@ -106,7 +106,7 @@ void ACollisionSensor::OnCollisionEvent( if (ParentActor) { FTransform LocalTransformRelativeToParent = GetActorTransform().GetRelativeTransform(ParentActor->GetActorTransform()); - ROS2->ProcessDataFromCollisionSensor( + ROS2Carla->ProcessDataFromCollisionSensor( 0, StreamId, LocalTransformRelativeToParent, OtherActor->GetUniqueID(), @@ -115,7 +115,7 @@ void ACollisionSensor::OnCollisionEvent( } else { - ROS2->ProcessDataFromCollisionSensor( + ROS2Carla->ProcessDataFromCollisionSensor( 0, StreamId, GetActorTransform(), OtherActor->GetUniqueID(), diff --git a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Sensor/DVSCamera.cpp b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Sensor/DVSCamera.cpp index 4a718b7c32d..40b15aab3ba 100644 --- a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Sensor/DVSCamera.cpp +++ b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Sensor/DVSCamera.cpp @@ -171,8 +171,8 @@ void ADVSCamera::PostPhysTick(UWorld *World, ELevelTick TickType, float DeltaTim // ROS2 #if defined(WITH_ROS2) - auto ROS2 = carla::ros2::ROS2::GetInstance(); - if (ROS2->IsEnabled()) + auto ROS2Carla = carla::ros2::ROS2Carla::GetInstance(); + if (ROS2Carla->IsEnabled()) { TRACE_CPUPROFILER_EVENT_SCOPE_STR("ROS2 Send"); auto StreamId = carla::streaming::detail::token_type(GetToken()).get_stream_id(); @@ -193,11 +193,11 @@ void ADVSCamera::PostPhysTick(UWorld *World, ELevelTick TickType, float DeltaTim if (ParentActor) { FTransform LocalTransformRelativeToParent = GetActorTransform().GetRelativeTransform(ParentActor->GetActorTransform()); - ROS2->ProcessDataFromDVS(DataStream.GetSensorType(), StreamId, LocalTransformRelativeToParent, BufView, W, H, Fov, this); + ROS2Carla->ProcessDataFromDVS(DataStream.GetSensorType(), StreamId, LocalTransformRelativeToParent, BufView, W, H, Fov, this); } else { - ROS2->ProcessDataFromDVS(DataStream.GetSensorType(), StreamId, DataStream.GetSensorTransform(), BufView, W, H, Fov, this); + ROS2Carla->ProcessDataFromDVS(DataStream.GetSensorType(), StreamId, DataStream.GetSensorTransform(), BufView, W, H, Fov, this); } } } diff --git a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Sensor/GnssSensor.cpp b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Sensor/GnssSensor.cpp index 23a73749134..c53e6bf00ed 100644 --- a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Sensor/GnssSensor.cpp +++ b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Sensor/GnssSensor.cpp @@ -61,8 +61,8 @@ void AGnssSensor::PostPhysTick(UWorld *World, ELevelTick TickType, float DeltaSe // ROS2 #if defined(WITH_ROS2) - auto ROS2 = carla::ros2::ROS2::GetInstance(); - if (ROS2->IsEnabled()) + auto ROS2Carla = carla::ros2::ROS2Carla::GetInstance(); + if (ROS2Carla->IsEnabled()) { TRACE_CPUPROFILER_EVENT_SCOPE_STR("ROS2 Send"); auto StreamId = carla::streaming::detail::token_type(GetToken()).get_stream_id(); @@ -70,11 +70,11 @@ void AGnssSensor::PostPhysTick(UWorld *World, ELevelTick TickType, float DeltaSe if (ParentActor) { FTransform LocalTransformRelativeToParent = GetActorTransform().GetRelativeTransform(ParentActor->GetActorTransform()); - ROS2->ProcessDataFromGNSS(DataStream.GetSensorType(), StreamId, LocalTransformRelativeToParent, carla::geom::GeoLocation{LatitudeValue, LongitudeValue, AltitudeValue}, this); + ROS2Carla->ProcessDataFromGNSS(DataStream.GetSensorType(), StreamId, LocalTransformRelativeToParent, carla::geom::GeoLocation{LatitudeValue, LongitudeValue, AltitudeValue}, this); } else { - ROS2->ProcessDataFromGNSS(DataStream.GetSensorType(), StreamId, DataStream.GetSensorTransform(), carla::geom::GeoLocation{LatitudeValue, LongitudeValue, AltitudeValue}, this); + ROS2Carla->ProcessDataFromGNSS(DataStream.GetSensorType(), StreamId, DataStream.GetSensorTransform(), carla::geom::GeoLocation{LatitudeValue, LongitudeValue, AltitudeValue}, this); } } #endif diff --git a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Sensor/GnssSensor.h b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Sensor/GnssSensor.h index 8b8eb8aa418..a6cfda3fede 100644 --- a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Sensor/GnssSensor.h +++ b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Sensor/GnssSensor.h @@ -51,8 +51,13 @@ class CARLA_API AGnssSensor : public ASensor float GetLongitudeBias() const; float GetAltitudeBias() const; + UFUNCTION(BlueprintCallable, Category = "GNSS") double GetLatitudeValue() const; + + UFUNCTION(BlueprintCallable, Category = "GNSS") double GetLongitudeValue() const; + + UFUNCTION(BlueprintCallable, Category = "GNSS") double GetAltitudeValue() const; protected: diff --git a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Sensor/ImageUtil.cpp b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Sensor/ImageUtil.cpp index 284b4fb79c6..03db511d69a 100644 --- a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Sensor/ImageUtil.cpp +++ b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Sensor/ImageUtil.cpp @@ -49,6 +49,7 @@ namespace ImageUtil SourcePitch, Out.GetData(), Flags); + return false; } diff --git a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Sensor/InertialMeasurementUnit.cpp b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Sensor/InertialMeasurementUnit.cpp index 975def4d529..8aaa2bac54d 100644 --- a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Sensor/InertialMeasurementUnit.cpp +++ b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Sensor/InertialMeasurementUnit.cpp @@ -194,8 +194,8 @@ void AInertialMeasurementUnit::PostPhysTick(UWorld *World, ELevelTick TickType, // ROS2 #if defined(WITH_ROS2) - auto ROS2 = carla::ros2::ROS2::GetInstance(); - if (ROS2->IsEnabled()) + auto ROS2Carla = carla::ros2::ROS2Carla::GetInstance(); + if (ROS2Carla->IsEnabled()) { TRACE_CPUPROFILER_EVENT_SCOPE_STR("ROS2 Send"); auto StreamId = carla::streaming::detail::token_type(GetToken()).get_stream_id(); @@ -203,11 +203,11 @@ void AInertialMeasurementUnit::PostPhysTick(UWorld *World, ELevelTick TickType, if (ParentActor) { FTransform LocalTransformRelativeToParent = GetActorTransform().GetRelativeTransform(ParentActor->GetActorTransform()); - ROS2->ProcessDataFromIMU(DataStream.GetSensorType(), StreamId, LocalTransformRelativeToParent, AccelerometerValue, GyroscopeValue, CompassValue, this); + ROS2Carla->ProcessDataFromIMU(DataStream.GetSensorType(), StreamId, LocalTransformRelativeToParent, AccelerometerValue, GyroscopeValue, CompassValue, this); } else { - ROS2->ProcessDataFromIMU(DataStream.GetSensorType(), StreamId, DataStream.GetSensorTransform(), AccelerometerValue, GyroscopeValue, CompassValue, this); + ROS2Carla->ProcessDataFromIMU(DataStream.GetSensorType(), StreamId, DataStream.GetSensorTransform(), AccelerometerValue, GyroscopeValue, CompassValue, this); } } #endif diff --git a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Sensor/ObstacleDetectionSensor.cpp b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Sensor/ObstacleDetectionSensor.cpp index 81566f43148..24c035a85d7 100644 --- a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Sensor/ObstacleDetectionSensor.cpp +++ b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Sensor/ObstacleDetectionSensor.cpp @@ -143,8 +143,8 @@ void AObstacleDetectionSensor::OnObstacleDetectionEvent( // ROS2 #if defined(WITH_ROS2) - auto ROS2 = carla::ros2::ROS2::GetInstance(); - if (ROS2->IsEnabled()) + auto ROS2Carla = carla::ros2::ROS2Carla::GetInstance(); + if (ROS2Carla->IsEnabled()) { TRACE_CPUPROFILER_EVENT_SCOPE_STR("ROS2 Send"); auto StreamId = carla::streaming::detail::token_type(GetToken()).get_stream_id(); @@ -152,11 +152,11 @@ void AObstacleDetectionSensor::OnObstacleDetectionEvent( if (ParentActor) { FTransform LocalTransformRelativeToParent = GetActorTransform().GetRelativeTransform(ParentActor->GetActorTransform()); - ROS2->ProcessDataFromObstacleDetection(DataStream.GetSensorType(), StreamId, LocalTransformRelativeToParent, Actor, OtherActor, HitDistance/100.0f, this); + ROS2Carla->ProcessDataFromObstacleDetection(DataStream.GetSensorType(), StreamId, LocalTransformRelativeToParent, Actor, OtherActor, HitDistance/100.0f, this); } else { - ROS2->ProcessDataFromObstacleDetection(DataStream.GetSensorType(), StreamId, DataStream.GetSensorTransform(), Actor, OtherActor, HitDistance/100.0f, this); + ROS2Carla->ProcessDataFromObstacleDetection(DataStream.GetSensorType(), StreamId, DataStream.GetSensorTransform(), Actor, OtherActor, HitDistance/100.0f, this); } } #endif diff --git a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Sensor/PixelReader.h b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Sensor/PixelReader.h index 7674c4cdb54..9e77330e8a3 100644 --- a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Sensor/PixelReader.h +++ b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Sensor/PixelReader.h @@ -188,12 +188,12 @@ void FPixelReader::SendPixelsInRenderThread(TSensor &Sensor, bool use16BitFormat // ROS2 #if defined(WITH_ROS2) - auto ROS2 = carla::ros2::ROS2::GetInstance(); - if (ROS2->IsEnabled()) + auto ROS2Carla = carla::ros2::ROS2Carla::GetInstance(); + if (ROS2Carla->IsEnabled()) { TRACE_CPUPROFILER_EVENT_SCOPE_STR("ROS2 Send PixelReader"); auto StreamId = carla::streaming::detail::token_type(Sensor.GetToken()).get_stream_id(); - auto Res = std::async(std::launch::async, [&Sensor, ROS2, &Stream, StreamId, BufView]() + auto Res = std::async(std::launch::async, [&Sensor, ROS2Carla, &Stream, StreamId, BufView]() { // get resolution of camera int W = -1, H = -1; @@ -206,17 +206,17 @@ void FPixelReader::SendPixelsInRenderThread(TSensor &Sensor, bool use16BitFormat H = FCString::Atoi(*HeightOpt->Value); auto FovOpt = Sensor.GetAttribute("fov"); if (FovOpt.has_value()) - Fov = FCString::Atof(*FovOpt->Value); + Fov = Sensor.GetFOVAngle(); // 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); + ROS2Carla->ProcessDataFromCamera(Stream.GetSensorType(), StreamId, LocalTransformRelativeToParent, W, H, Fov, BufView, &Sensor); } else { - ROS2->ProcessDataFromCamera(Stream.GetSensorType(), StreamId, Stream.GetSensorTransform(), W, H, Fov, BufView, &Sensor); + ROS2Carla->ProcessDataFromCamera(Stream.GetSensorType(), StreamId, Stream.GetSensorTransform(), W, H, Fov, BufView, &Sensor); } }); } diff --git a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Sensor/Radar.cpp b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Sensor/Radar.cpp index f44e21b7222..9d24951ae12 100644 --- a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Sensor/Radar.cpp +++ b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Sensor/Radar.cpp @@ -87,8 +87,8 @@ void ARadar::PostPhysTick(UWorld *World, ELevelTick TickType, float DeltaTime) // ROS2 #if defined(WITH_ROS2) - auto ROS2 = carla::ros2::ROS2::GetInstance(); - if (ROS2->IsEnabled()) + auto ROS2Carla = carla::ros2::ROS2Carla::GetInstance(); + if (ROS2Carla->IsEnabled()) { TRACE_CPUPROFILER_EVENT_SCOPE_STR("ARadar::PostPhysTick ROS2 Send"); auto StreamId = carla::streaming::detail::token_type(GetToken()).get_stream_id(); @@ -96,11 +96,11 @@ void ARadar::PostPhysTick(UWorld *World, ELevelTick TickType, float DeltaTime) if (ParentActor) { FTransform LocalTransformRelativeToParent = GetActorTransform().GetRelativeTransform(ParentActor->GetActorTransform()); - ROS2->ProcessDataFromRadar(DataStream.GetSensorType(), StreamId, LocalTransformRelativeToParent, RadarData, this); + ROS2Carla->ProcessDataFromRadar(DataStream.GetSensorType(), StreamId, LocalTransformRelativeToParent, RadarData, this); } else { - ROS2->ProcessDataFromRadar(DataStream.GetSensorType(), StreamId, DataStream.GetSensorTransform(), RadarData, this); + ROS2Carla->ProcessDataFromRadar(DataStream.GetSensorType(), StreamId, DataStream.GetSensorTransform(), RadarData, this); } } #endif diff --git a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Sensor/RayCastLidar.cpp b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Sensor/RayCastLidar.cpp index 148702e44cb..3554d68b750 100644 --- a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Sensor/RayCastLidar.cpp +++ b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Sensor/RayCastLidar.cpp @@ -70,8 +70,8 @@ void ARayCastLidar::PostPhysTick(UWorld *World, ELevelTick TickType, float Delta } // ROS2 #if defined(WITH_ROS2) - auto ROS2 = carla::ros2::ROS2::GetInstance(); - if (ROS2->IsEnabled()) + auto ROS2Carla = carla::ros2::ROS2Carla::GetInstance(); + if (ROS2Carla->IsEnabled()) { TRACE_CPUPROFILER_EVENT_SCOPE_STR("ROS2 Send"); auto StreamId = carla::streaming::detail::token_type(GetToken()).get_stream_id(); @@ -79,11 +79,11 @@ void ARayCastLidar::PostPhysTick(UWorld *World, ELevelTick TickType, float Delta if (ParentActor) { FTransform LocalTransformRelativeToParent = GetActorTransform().GetRelativeTransform(ParentActor->GetActorTransform()); - ROS2->ProcessDataFromLidar(DataStream.GetSensorType(), StreamId, LocalTransformRelativeToParent, LidarData, this); + ROS2Carla->ProcessDataFromLidar(DataStream.GetSensorType(), StreamId, LocalTransformRelativeToParent, LidarData, this); } else { - ROS2->ProcessDataFromLidar(DataStream.GetSensorType(), StreamId, SensorTransform, LidarData, this); + ROS2Carla->ProcessDataFromLidar(DataStream.GetSensorType(), StreamId, SensorTransform, LidarData, this); } } #endif diff --git a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Sensor/RayCastSemanticLidar.cpp b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Sensor/RayCastSemanticLidar.cpp index ba4e6fe2e50..7159dd4519a 100644 --- a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Sensor/RayCastSemanticLidar.cpp +++ b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Sensor/RayCastSemanticLidar.cpp @@ -83,8 +83,8 @@ void ARayCastSemanticLidar::PostPhysTick(UWorld *World, ELevelTick TickType, flo } // ROS2 #if defined(WITH_ROS2) - auto ROS2 = carla::ros2::ROS2::GetInstance(); - if (ROS2->IsEnabled()) + auto ROS2Carla = carla::ros2::ROS2Carla::GetInstance(); + if (ROS2Carla->IsEnabled()) { TRACE_CPUPROFILER_EVENT_SCOPE_STR("ROS2 Send"); auto StreamId = carla::streaming::detail::token_type(GetToken()).get_stream_id(); @@ -92,11 +92,11 @@ void ARayCastSemanticLidar::PostPhysTick(UWorld *World, ELevelTick TickType, flo if (ParentActor) { FTransform LocalTransformRelativeToParent = GetActorTransform().GetRelativeTransform(ParentActor->GetActorTransform()); - ROS2->ProcessDataFromSemanticLidar(DataStream.GetSensorType(), StreamId, LocalTransformRelativeToParent, SemanticLidarData, this); + ROS2Carla->ProcessDataFromSemanticLidar(DataStream.GetSensorType(), StreamId, LocalTransformRelativeToParent, SemanticLidarData, this); } else { - ROS2->ProcessDataFromSemanticLidar(DataStream.GetSensorType(), StreamId, SensorTransform, SemanticLidarData, this); + ROS2Carla->ProcessDataFromSemanticLidar(DataStream.GetSensorType(), StreamId, SensorTransform, SemanticLidarData, this); } } #endif diff --git a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Sensor/Sensor.h b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Sensor/Sensor.h index 9e395da5588..562ffa3c7da 100644 --- a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Sensor/Sensor.h +++ b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Sensor/Sensor.h @@ -204,12 +204,12 @@ class CARLA_API ASensor : public AActor auto BufferView = carla::BufferView::CreateFrom(std::move(SerializedBuffer)); #if defined(WITH_ROS2) - auto ROS2 = carla::ros2::ROS2::GetInstance(); - if (ROS2->IsEnabled()) + auto ROS2Carla = carla::ros2::ROS2Carla::GetInstance(); + if (ROS2Carla->IsEnabled()) { TRACE_CPUPROFILER_EVENT_SCOPE_STR("ROS2 SendDataToClient"); auto StreamId = carla::streaming::detail::token_type(Sensor.GetToken()).get_stream_id(); - auto Res = std::async(std::launch::async, [&Sensor, ROS2, &Stream, StreamId, BufferView]() + auto Res = std::async(std::launch::async, [&Sensor, ROS2Carla, &Stream, StreamId, BufferView]() { // get resolution of camera int W = -1, H = -1; @@ -222,14 +222,14 @@ class CARLA_API ASensor : public AActor H = FCString::Atoi(*HeightOpt->Value); auto FovOpt = Sensor.GetAttribute("fov"); if (FovOpt.has_value()) - Fov = FCString::Atof(*FovOpt->Value); + Fov = Sensor.GetFOVAngle(); // send data to ROS2 auto ParentActor = Sensor.GetAttachParentActor(); auto Transform = ParentActor ? Sensor.GetActorTransform().GetRelativeTransform(ParentActor->GetActorTransform()) : Stream.GetSensorTransform(); - ROS2->ProcessDataFromCamera( + ROS2Carla->ProcessDataFromCamera( Stream.GetSensorType(), StreamId, Transform, diff --git a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Server/CarlaServer.cpp b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Server/CarlaServer.cpp index 1af7c2667e4..e36cce46c51 100644 --- a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Server/CarlaServer.cpp +++ b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Server/CarlaServer.cpp @@ -765,6 +765,25 @@ void FCarlaServer::FPimpl::BindActions() LargeMap->OnActorSpawned(*Result.Value); } + #if defined(WITH_ROS2) + FCarlaActor* CarlaActor = Episode->FindCarlaActor(Result.Value->GetActorId()); + if (!CarlaActor) + { + RESPOND_ERROR("internal error: actor could not be spawned"); + } + auto ROS2Interfaces = carla::ros2::ROS2Interfaces::GetInstance(); + for (const auto &Attr : CarlaActor->GetActorInfo()->Description.Variations) + { + if (Attr.Key == "ros_name") + { + const std::string RosName = std::string(TCHAR_TO_UTF8(*Attr.Value.Value)); + // There may be a better way to convert from cr::ActorDescription to FActorDescription... + FActorDescription Des = Description; + ROS2Interfaces->RegisterActorWithInterfaces(Des, RosName, static_cast(CarlaActor->GetActor())); + } + } + #endif + return Episode->SerializeActor(Result.Value); }; @@ -800,21 +819,29 @@ void FCarlaServer::FPimpl::BindActions() ParentCarlaActor->AddChildren(CarlaActor->GetActorId()); #if defined(WITH_ROS2) - auto ROS2 = carla::ros2::ROS2::GetInstance(); - if (ROS2->IsEnabled()) + auto ROS2Interfaces = carla::ros2::ROS2Interfaces::GetInstance(); + FCarlaActor* CurrentActor = ParentCarlaActor; + while(CurrentActor) { - FCarlaActor* CurrentActor = ParentCarlaActor; - while(CurrentActor) + for (const auto &Attr : CurrentActor->GetActorInfo()->Description.Variations) { - for (const auto &Attr : CurrentActor->GetActorInfo()->Description.Variations) + if (Attr.Key == "ros_name") { - if (Attr.Key == "ros_name") - { - const std::string value = std::string(TCHAR_TO_UTF8(*Attr.Value.Value)); - ROS2->AddActorParentRosName(static_cast(CarlaActor->GetActor()), static_cast(CurrentActor->GetActor())); - } + const std::string value = std::string(TCHAR_TO_UTF8(*Attr.Value.Value)); + ROS2Interfaces->AddActorParentRosName(static_cast(CarlaActor->GetActor()), static_cast(CurrentActor->GetActor())); } - CurrentActor = Episode->FindCarlaActor(CurrentActor->GetParent()); + } + CurrentActor = Episode->FindCarlaActor(CurrentActor->GetParent()); + } + + for (const auto &Attr : CarlaActor->GetActorInfo()->Description.Variations) + { + if (Attr.Key == "ros_name") + { + const std::string RosName = std::string(TCHAR_TO_UTF8(*Attr.Value.Value)); + // There may be a better way to convert from cr::ActorDescription to FActorDescription... + FActorDescription Des = Description; + ROS2Interfaces->RegisterActorWithInterfaces(Des, RosName, static_cast(CarlaActor->GetActor())); } } #endif diff --git a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Server/CarlaServer.h b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Server/CarlaServer.h index 0b966a5625c..2d91596563e 100644 --- a/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Server/CarlaServer.h +++ b/Unreal/CarlaUnreal/Plugins/Carla/Source/Carla/Server/CarlaServer.h @@ -21,7 +21,7 @@ class UCarlaEpisode; -class FCarlaServer +class CARLA_API FCarlaServer { public: diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/.gitignore b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/.gitignore new file mode 100644 index 00000000000..069b713038e --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/.gitignore @@ -0,0 +1,3 @@ +*/Binaries/ +*/Intermediate/ +.vscode/ \ No newline at end of file diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/AudioSensor/AudioSensor.uplugin b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/AudioSensor/AudioSensor.uplugin new file mode 100644 index 00000000000..1664ac48767 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/AudioSensor/AudioSensor.uplugin @@ -0,0 +1,26 @@ +{ + "FileVersion" : 3, + "Version" : "0.0.0", + "VersionName": "0.0.0", + "FriendlyName": "AudioSensor", + "Description": "Audio Sensor Plugin", + "Category" : "Science", + "MarketplaceURL" : "", + "EnabledByDefault" : true, + "CanContainContent": true, + "IsBetaVersion" : true, + "Installed": false, + "Modules": [ + { + "Name": "AudioSensor", + "Type": "Runtime", + "LoadingPhase": "Default" + } + ], + "Plugins": [ + { + "Name": "Carla", + "Enabled": true + } + ] +} \ No newline at end of file diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/AudioSensor/Source/AudioSensor/AudioSensor.Build.cs b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/AudioSensor/Source/AudioSensor/AudioSensor.Build.cs new file mode 100644 index 00000000000..437a180c1a0 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/AudioSensor/Source/AudioSensor/AudioSensor.Build.cs @@ -0,0 +1,70 @@ +using System; +using System.IO; +using UnrealBuildTool; +using EpicGames.Core; + +public class AudioSensor : ModuleRules +{ + public AudioSensor(ReadOnlyTargetRules Target) : base(Target) + { + bEnableExceptions = true; + PrivatePCHHeaderFile = "AudioSensor.h"; + PCHUsage = ModuleRules.PCHUsageMode.UseExplicitOrSharedPCHs; + bUseRTTI = true; + + void AddDynamicLibrary(string library) + { + PublicAdditionalLibraries.Add(library); + RuntimeDependencies.Add(library); + PublicDelayLoadDLLs.Add(library); + System.Console.WriteLine("Dynamic Library Added: " + library); + } + + PublicDependencyModuleNames.AddRange(new string[] { + "AudioMixer", + "Carla", + "Chaos", + "ChaosVehicles", + "Core", + "CoreUObject", + "Engine", + "Foliage", + "HTTP", + "ImageWriteQueue", + "InputCore", + "Json", + "JsonUtilities", + "Landscape", + "MeshDescription", + "PhysicsCore", + "ProceduralMeshComponent", + "StaticMeshDescription", + "Slate", + "SlateCore", + "RHI", + "Renderer" + }); + + if (Target.Type == TargetType.Editor) + { + PublicDependencyModuleNames.AddRange(new string[] { "UnrealEd" }); + } + + PrivateDependencyModuleNames.AddRange(new string[] { }); + + string PluginSourcePath = Path.GetFullPath(ModuleDirectory); + string PluginBinariesBuildPath = Path.Combine(PluginSourcePath, "..", "..", "..", "..", "..", "..", ".."); + PublicIncludePaths.Add(Path.Combine(PluginBinariesBuildPath, "Build/Ros2Native/install/include")); + + string CarlaPluginBinariesLinuxPath = Path.Combine(PluginSourcePath, "..", "..", "..", "..", "Carla", "Binaries", "Linux"); + AddDynamicLibrary(Path.Combine(CarlaPluginBinariesLinuxPath, "libcarla-ros2-native.so")); + RuntimeDependencies.Add(Path.Combine(CarlaPluginBinariesLinuxPath, "libfoonathan_memory-0.7.3.so")); + RuntimeDependencies.Add(Path.Combine(CarlaPluginBinariesLinuxPath, "libfastcdr.so")); + RuntimeDependencies.Add(Path.Combine(CarlaPluginBinariesLinuxPath, "libfastcdr.so.1")); + RuntimeDependencies.Add(Path.Combine(CarlaPluginBinariesLinuxPath, "libfastcdr.so.1.1.0")); + RuntimeDependencies.Add(Path.Combine(CarlaPluginBinariesLinuxPath, "libfastrtps.so")); + RuntimeDependencies.Add(Path.Combine(CarlaPluginBinariesLinuxPath, "libfastrtps.so.2.11")); + RuntimeDependencies.Add(Path.Combine(CarlaPluginBinariesLinuxPath, "libfastrtps.so.2.11.2")); + + } +} \ No newline at end of file diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/AudioSensor/Source/AudioSensor/AudioSensor.cpp b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/AudioSensor/Source/AudioSensor/AudioSensor.cpp new file mode 100644 index 00000000000..6b3ebc37773 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/AudioSensor/Source/AudioSensor/AudioSensor.cpp @@ -0,0 +1,22 @@ +#include "AudioSensor.h" + +#include "AudioSensorActor.h" + + +#define LOCTEXT_NAMESPACE "FAudioSensor" + + +DEFINE_LOG_CATEGORY(LogAudioSensor) + +void FAudioSensor::StartupModule() +{ +} + +void FAudioSensor::ShutdownModule() +{ +} + +#undef LOCTEXT_NAMESPACE + +IMPLEMENT_MODULE(FAudioSensor, AudioSensor) + diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/AudioSensor/Source/AudioSensor/AudioSensor.h b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/AudioSensor/Source/AudioSensor/AudioSensor.h new file mode 100644 index 00000000000..fc09ce9c4b2 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/AudioSensor/Source/AudioSensor/AudioSensor.h @@ -0,0 +1,19 @@ + +#pragma once + +#include "Engine.h" +#include "Logging/LogMacros.h" +#include "CoreMinimal.h" +#include "Modules/ModuleManager.h" + +DECLARE_LOG_CATEGORY_EXTERN(LogAudioSensor, Log, All); + +class FAudioSensor : public IModuleInterface +{ +public: + + /** IModuleInterface implementation */ + virtual void StartupModule() override; + virtual void ShutdownModule() override; +}; + diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/AudioSensor/Source/AudioSensor/AudioSensorActor.cpp b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/AudioSensor/Source/AudioSensor/AudioSensorActor.cpp new file mode 100644 index 00000000000..8d0a75ac9e7 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/AudioSensor/Source/AudioSensor/AudioSensorActor.cpp @@ -0,0 +1,118 @@ +#include "AudioSensorActor.h" +#include "AudioMixerDevice.h" +#include "AudioDeviceManager.h" +#include "AudioSensorBlueprintLibrary.h" +#include "AudioSensor/ros2/ROS2Audio.h" + +AAudioSensorActor::AAudioSensorActor(const FObjectInitializer &ObjectInitializer) : Super(ObjectInitializer) +{ + // PrimaryActorTick.bCanEverTick = true; + this->bIsRecording = false; + this->Active = false; + this->ChunkDuration = 0.25; + this->AudioBuffer = Audio::TSampleBuffer(); +} + +void AAudioSensorActor::BeginPlay() +{ + Super::BeginPlay(); + + // Create the master submix + this->MasterSubmix = NewObject(); + Activate(); +} + +void AAudioSensorActor::SetChunkDuration(double Duration){ + this->ChunkDuration = Duration; +} + +FActorDefinition AAudioSensorActor::GetSensorDefinition() +{ + return UAudioSensorBlueprintLibrary::MakeAudioSensorDefinition(AAudioSensorActor::StaticClass()); +} + +void AAudioSensorActor::Set(const FActorDescription &ActorDescription) +{ + Super::Set(ActorDescription); + UAudioSensorBlueprintLibrary::SetAudioSensor(ActorDescription, this); +} + +void AAudioSensorActor::SetOwner(AActor* OwningActor) +{ + Super::SetOwner(OwningActor); +} + +void AAudioSensorActor::PrePhysTick(float DeltaSeconds){ + if (!Active){ + return; + } + + StopRecording(); // end the previous recording (if one is in progress) + StartRecording(); // start recording the next frame + + if (AudioBuffer.GetNumSamples() == 0) + { + return; + } + + auto ROS2Audio = carla::ros2::ROS2Audio::GetInstance(); + auto StreamId = carla::streaming::detail::token_type(GetToken()).get_stream_id(); + AActor* ParentActor = GetAttachParentActor(); + + if (ParentActor) + { + FTransform LocalTransformRelativeToParent = GetActorTransform().GetRelativeTransform(ParentActor->GetActorTransform()); + ROS2Audio->ProcessDataFromAudio(StreamId, LocalTransformRelativeToParent, AudioBuffer.GetNumChannels(), AudioBuffer.GetSampleRate(), AudioBuffer.GetNumFrames(), AudioBuffer.GetData(), this); + } + else + { + ROS2Audio->ProcessDataFromAudio(StreamId, GetActorTransform(), AudioBuffer.GetNumChannels(), AudioBuffer.GetSampleRate(), AudioBuffer.GetNumFrames(), AudioBuffer.GetData(), this); + } +} + +void AAudioSensorActor::StartRecording() +{ + if (MasterSubmix && !bIsRecording) + { + if(Audio::FMixerDevice* MixerDevice = FAudioDeviceManager::GetAudioMixerDeviceFromWorldContext(GetWorld())) + { + MixerDevice->StartRecording(MasterSubmix, ChunkDuration); + bIsRecording = true; + } + } +} + +void AAudioSensorActor::StopRecording() +{ + AudioBuffer.Reset(); + if (MasterSubmix && bIsRecording) + { + if(Audio::FMixerDevice* MixerDevice = FAudioDeviceManager::GetAudioMixerDeviceFromWorldContext(GetWorld())) + { + float Channels; + float SampleRate; + Audio::FAlignedFloatBuffer& Buffer = MixerDevice->StopRecording(MasterSubmix, Channels, SampleRate); + if (Buffer.Num() > 0) + { + Audio::FSampleBuffer SampleBuffer(Buffer, Channels, SampleRate); + + if (SampleBuffer.GetNumChannels() > 2) + { + SampleBuffer.MixBufferToChannels(2); + } + // This conveniently converts the data from float to int16 + AudioBuffer = SampleBuffer; + bIsRecording = false; + } + } + } +} + +void AAudioSensorActor::Activate(){ + Active = true; +} + +void AAudioSensorActor::Deactivate(){ + Active = false; + StopRecording(); +} \ No newline at end of file diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/AudioSensor/Source/AudioSensor/AudioSensorActor.h b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/AudioSensor/Source/AudioSensor/AudioSensorActor.h new file mode 100644 index 00000000000..46b30f03be7 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/AudioSensor/Source/AudioSensor/AudioSensorActor.h @@ -0,0 +1,67 @@ +#pragma once + +#include "CoreMinimal.h" +#include "GameFramework/Actor.h" +#include "Sound/SampleBufferIO.h" + +#include "Carla/Sensor/Sensor.h" +#include "Carla/Actor/ActorDefinition.h" +#include "Carla/Actor/ActorDescription.h" + +#include "AudioSensorActor.generated.h" + +UCLASS() +class AUDIOSENSOR_API AAudioSensorActor : public ASensor +{ + GENERATED_BODY() + +public: + + AAudioSensorActor(const FObjectInitializer &ObjectInitializer); + + static FActorDefinition GetSensorDefinition(); + + void SetChunkDuration(double Duration); + + void Set(const FActorDescription &ActorDescription) override; + + void SetOwner(AActor *Owner) override; + + void PrePhysTick(float DeltaSeconds) override; + +protected: + virtual void BeginPlay() override; + +public: + + UFUNCTION(BlueprintCallable) + void StartRecording(); + + UFUNCTION(BlueprintCallable) + void StopRecording(); + + UFUNCTION(BlueprintCallable) + void Activate(); + + UFUNCTION(BlueprintCallable) + void Deactivate(); + +protected: + UPROPERTY(BlueprintReadWrite) + class USoundSubmix* MasterSubmix; + + UPROPERTY(BlueprintReadOnly) + bool bIsRecording; + + UPROPERTY(BlueprintReadWrite) + float ChunkDuration; // in seconds + + UPROPERTY(BlueprintReadOnly) + bool Active; + + Audio::TSampleBuffer AudioBuffer; + +public: + UPROPERTY(BlueprintReadOnly) + FString OutputName; +}; diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/AudioSensor/Source/AudioSensor/AudioSensorBlueprintLibrary.cpp b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/AudioSensor/Source/AudioSensor/AudioSensorBlueprintLibrary.cpp new file mode 100644 index 00000000000..b9e5308686e --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/AudioSensor/Source/AudioSensor/AudioSensorBlueprintLibrary.cpp @@ -0,0 +1,225 @@ +#pragma once + +#include "AudioSensorBlueprintLibrary.h" +#include "AudioSensorActor.h" +#include "Carla/Actor/ActorBlueprintFunctionLibrary.h" +#include "Carla.h" +#include "Carla/Server/CarlaServer.h" +#include "Carla/Game/CarlaGameInstance.h" +#include "Carla/Game/CarlaStatics.h" +#include "Carla/Sensor/Sensor.h" + +struct FWavHeader +{ + char RIFF[4] = {'R', 'I', 'F', 'F'}; + uint32 FileSize; + char WAVE[4] = {'W', 'A', 'V', 'E'}; + char fmt[4] = {'f', 'm', 't', ' '}; + uint32 Subchunk1Size = 16; // PCM header size + uint16 AudioFormat = 1; // PCM = 1 + uint16 NumChannels; + uint32 SampleRate; + uint32 ByteRate; + uint16 BlockAlign; + uint16 BitsPerSample; + char data[4] = {'d', 'a', 't', 'a'}; + uint32 DataSize; +}; + +#if WITH_EDITOR +# define CARLA_ABFL_CHECK_ACTOR(ActorPtr) \ + if (!IsValid(ActorPtr)) \ + { \ + UE_LOG(LogAudioSensor, Error, TEXT("Cannot set empty actor!")); \ + return; \ + } +#else +# define CARLA_ABFL_CHECK_ACTOR(ActorPtr) \ + IsValid(ActorPtr); +#endif // WITH_EDITOR + +bool UAudioSensorBlueprintLibrary::SaveSoundWaveToWavFile(USoundWave* SoundWave, const FString& FilePath) +{ + if (!SoundWave or SoundWave->GetNumChunks() == 0) + { + UE_LOG(LogAudioSensor, Error, TEXT("Invalid SoundWave")); + return false; + } + + int32 RawDataSize = 0; + TArray RawPCMData; + for (uint32 i = 0; i < SoundWave->GetNumChunks(); i++){ + uint32 ChunkSize = SoundWave->GetSizeOfChunk(i); + RawDataSize += ChunkSize; + + uint8* data = new uint8[ChunkSize]; + SoundWave->GetChunkData(i, &data, false); + + for (uint32 j = 0; j < ChunkSize; j++){ + RawPCMData.Add(data[j]); + } + delete[] data; + } + + FWavHeader WavHeader; + WavHeader.NumChannels = SoundWave->NumChannels; + WavHeader.SampleRate = SoundWave->GetSampleRateForCurrentPlatform(); + WavHeader.BitsPerSample = 16; // Assuming 16-bit PCM + WavHeader.ByteRate = WavHeader.SampleRate * WavHeader.NumChannels * (WavHeader.BitsPerSample / 8); + WavHeader.BlockAlign = WavHeader.NumChannels * (WavHeader.BitsPerSample / 8); + WavHeader.DataSize = RawDataSize; + WavHeader.FileSize = sizeof(FWavHeader) - 8 + RawDataSize; + + // Combine header and PCM data + TArray WavData; + WavData.Append(reinterpret_cast(&WavHeader), sizeof(FWavHeader)); + WavData.Append(RawPCMData); + + if (FFileHelper::SaveArrayToFile(WavData, *FilePath)) + { + UE_LOG(LogAudioSensor, Log, TEXT("Saved WAV file to: %s"), *FilePath); + return true; + } + else + { + UE_LOG(LogAudioSensor, Error, TEXT("Failed to save WAV file.")); + return false; + } +} + +FActorDefinition UAudioSensorBlueprintLibrary::MakeAudioSensorDefinition(TSubclassOf Class) +{ + FActorDefinition Definition; + bool Success; + MakeAudioSensorDefinition(Success, Definition, Class); + check(Success); + return Definition; +} + +template +static FString JoinStrings(const FString &Separator, ARGS && ... Args) +{ + return FString::Join(TArray{std::forward(Args) ...}, *Separator); +} + +template +static void FillIdAndTags(FActorDefinition &Def, TStrs && ... Strings) +{ + Def.Id = JoinStrings(TEXT("."), std::forward(Strings) ...).ToLower(); + Def.Tags = JoinStrings(TEXT(","), std::forward(Strings) ...).ToLower(); + + // each actor gets an actor role name attribute (empty by default) + FActorVariation ActorRole; + ActorRole.Id = TEXT("role_name"); + ActorRole.Type = EActorAttributeType::String; + ActorRole.RecommendedValues = { TEXT("default") }; + ActorRole.bRestrictToRecommended = false; + Def.Variations.Emplace(ActorRole); + + // ROS2 + FActorVariation Var; + Var.Id = TEXT("ros_name"); + Var.Type = EActorAttributeType::String; + Var.RecommendedValues = { Def.Id }; + Var.bRestrictToRecommended = false; + Def.Variations.Emplace(Var); +} + +void UAudioSensorBlueprintLibrary::MakeAudioSensorDefinition( + bool &Success, + FActorDefinition &Definition, + TSubclassOf Class) +{ + FillIdAndTags(Definition, TEXT("sensor"), TEXT("other"), TEXT("audio")); + + Definition.Class = Class; + + FActorVariation Tick; + Tick.Id = TEXT("sensor_tick"); + Tick.Type = EActorAttributeType::Float; + Tick.RecommendedValues = { TEXT("0.0") }; + Tick.bRestrictToRecommended = false; + + Definition.Variations.Append({ + Tick}); + + Definition.Attributes.Emplace(FActorAttribute{ + TEXT("sensor_type"), + EActorAttributeType::String, + "audio"}); + + Success = UActorBlueprintFunctionLibrary::CheckActorDefinition(Definition); +} + +void UAudioSensorBlueprintLibrary::SetAudioSensor( + const FActorDescription &Description, + AAudioSensorActor *AudioSensor) +{ + CARLA_ABFL_CHECK_ACTOR(AudioSensor); + + AudioSensor->SetChunkDuration( + UActorBlueprintFunctionLibrary::RetrieveActorAttributeToFloat("sensor_tick", Description.Variations, 0.05f) + ); + + AudioSensor->OutputName = UActorBlueprintFunctionLibrary::RetrieveActorAttributeToString("ros_name", Description.Variations, "ABC"); + +} + +FActorSpawnResult UAudioSensorBlueprintLibrary::SpawnSensorActor( + const AActor* FactoryActor, + const FTransform &Transform, + const FActorDescription &Description) +{ + auto *World = FactoryActor->GetWorld(); + if (World == nullptr) + { + UE_LOG(LogAudioSensor, Error, TEXT("ACustomSensorFactory: cannot spawn sensor into an empty world.")); + return {}; + } + + UCarlaGameInstance *GameInstance = UCarlaStatics::GetGameInstance(World); + if (GameInstance == nullptr) + { + UE_LOG(LogAudioSensor, Error, TEXT("ACustomSensorFactory: cannot spawn sensor, incompatible game instance.")); + return {}; + } + + auto *Sensor = World->SpawnActorDeferred( + Description.Class, + Transform, + nullptr, + nullptr, + ESpawnActorCollisionHandlingMethod::AlwaysSpawn); + if (Sensor == nullptr) + { + UE_LOG(LogAudioSensor, Error, TEXT("ACustomSensorFactory: spawn sensor failed.")); + } + else + { + auto *Episode = GameInstance->GetCarlaEpisode(); + check(Episode != nullptr); + + Sensor->SetEpisode(*Episode); + Sensor->Set(Description); + Sensor->SetDataStream(GameInstance->GetServer().OpenStream()); + // ASceneCaptureSensor * SceneCaptureSensor = Cast(Sensor); + // if(SceneCaptureSensor) + // { + // SceneCaptureSensor->CameraGBuffers.SceneColor.SetDataStream(GameInstance->GetServer().OpenStream()); + // SceneCaptureSensor->CameraGBuffers.SceneDepth.SetDataStream(GameInstance->GetServer().OpenStream()); + // SceneCaptureSensor->CameraGBuffers.SceneStencil.SetDataStream(GameInstance->GetServer().OpenStream()); + // SceneCaptureSensor->CameraGBuffers.GBufferA.SetDataStream(GameInstance->GetServer().OpenStream()); + // SceneCaptureSensor->CameraGBuffers.GBufferB.SetDataStream(GameInstance->GetServer().OpenStream()); + // SceneCaptureSensor->CameraGBuffers.GBufferC.SetDataStream(GameInstance->GetServer().OpenStream()); + // SceneCaptureSensor->CameraGBuffers.GBufferD.SetDataStream(GameInstance->GetServer().OpenStream()); + // SceneCaptureSensor->CameraGBuffers.GBufferE.SetDataStream(GameInstance->GetServer().OpenStream()); + // SceneCaptureSensor->CameraGBuffers.GBufferF.SetDataStream(GameInstance->GetServer().OpenStream()); + // SceneCaptureSensor->CameraGBuffers.Velocity.SetDataStream(GameInstance->GetServer().OpenStream()); + // SceneCaptureSensor->CameraGBuffers.SSAO.SetDataStream(GameInstance->GetServer().OpenStream()); + // SceneCaptureSensor->CameraGBuffers.CustomDepth.SetDataStream(GameInstance->GetServer().OpenStream()); + // SceneCaptureSensor->CameraGBuffers.CustomStencil.SetDataStream(GameInstance->GetServer().OpenStream()); + // } + } + UGameplayStatics::FinishSpawningActor(Sensor, Transform); + return FActorSpawnResult{Sensor}; +} \ No newline at end of file diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/AudioSensor/Source/AudioSensor/AudioSensorBlueprintLibrary.h b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/AudioSensor/Source/AudioSensor/AudioSensorBlueprintLibrary.h new file mode 100644 index 00000000000..fb858e60997 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/AudioSensor/Source/AudioSensor/AudioSensorBlueprintLibrary.h @@ -0,0 +1,31 @@ +#pragma once + +#include +#include "Kismet/BlueprintFunctionLibrary.h" +#include + +#include "AudioSensorBlueprintLibrary.generated.h" + +class AAudioSensorActor; + +UCLASS() +class AUDIOSENSOR_API UAudioSensorBlueprintLibrary : public UBlueprintFunctionLibrary +{ + GENERATED_BODY() + + public: + UFUNCTION(BlueprintCallable) + static bool SaveSoundWaveToWavFile(USoundWave* SoundWave, const FString& FilePath); + + UFUNCTION(BlueprintCallable) + static FActorDefinition MakeAudioSensorDefinition(TSubclassOf Class); + + static void MakeAudioSensorDefinition(bool &Success, FActorDefinition &Definition, TSubclassOf Class); + + UFUNCTION(BlueprintCallable) + static void SetAudioSensor(const FActorDescription &Description, AAudioSensorActor *AudioSensor); + + // Note that this function should really be in a seperate plugin that all custom sensors depend on. + UFUNCTION(BlueprintCallable) + static FActorSpawnResult SpawnSensorActor(const AActor* FactoryActor, const FTransform &Transform, const FActorDescription &Description); +}; \ No newline at end of file diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/AudioSensor/Source/AudioSensor/AudioSensorGameSubsystem.cpp b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/AudioSensor/Source/AudioSensor/AudioSensorGameSubsystem.cpp new file mode 100644 index 00000000000..83f2c230940 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/AudioSensor/Source/AudioSensor/AudioSensorGameSubsystem.cpp @@ -0,0 +1,17 @@ + +#include "AudioSensorGameSubsystem.h" + +#include "AudioSensor/CarlaInterface/CarlaActor.h" + +#include "AudioSensor/ros2/ROS2Audio.h" + +void UAudioSensorSubsystem::Initialize(FSubsystemCollectionBase& Collection) +{ + FAudioSensorActor::RegisterClassWithFactory(); + carla::ros2::ROS2Audio::GetInstance(); +} + +void UAudioSensorSubsystem::Deinitialize() +{ + +} diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/AudioSensor/Source/AudioSensor/AudioSensorGameSubsystem.h b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/AudioSensor/Source/AudioSensor/AudioSensorGameSubsystem.h new file mode 100644 index 00000000000..e2c29bbd941 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/AudioSensor/Source/AudioSensor/AudioSensorGameSubsystem.h @@ -0,0 +1,19 @@ +#pragma once + +#include "Subsystems/GameInstanceSubsystem.h" + +#include "AudioSensorGameSubsystem.generated.h" + +UCLASS() +class UAudioSensorSubsystem : public UGameInstanceSubsystem +{ + GENERATED_BODY() +public: + // Begin USubsystem + virtual void Initialize(FSubsystemCollectionBase& Collection) override; + virtual void Deinitialize() override; + // End USubsystem + +private: + // All my variables +}; \ No newline at end of file diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/AudioSensor/Source/AudioSensor/CarlaInterface/AudioSensorActorData.cpp b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/AudioSensor/Source/AudioSensor/CarlaInterface/AudioSensorActorData.cpp new file mode 100644 index 00000000000..a7b46755c3c --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/AudioSensor/Source/AudioSensor/CarlaInterface/AudioSensorActorData.cpp @@ -0,0 +1,23 @@ +// Copyright (c) 2024 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 . + +#include "AudioSensorActorData.h" + + +void FAudioSensorData::RecordActorData(FCarlaActor* CarlaActor, UCarlaEpisode* CarlaEpisode) +{ + FActorData::RecordActorData(CarlaActor, CarlaEpisode); + // AActor* Actor = CarlaActor->GetActor(); + // AAudioSensorActor* Sensor = Cast(Actor); +} + +void FAudioSensorData::RestoreActorData(FCarlaActor* CarlaActor, UCarlaEpisode* CarlaEpisode) +{ + FActorData::RestoreActorData(CarlaActor, CarlaEpisode); + // AActor* Actor = CarlaActor->GetActor(); + // AAudioSensorActor* Sensor = Cast(Actor); + +} diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/AudioSensor/Source/AudioSensor/CarlaInterface/AudioSensorActorData.h b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/AudioSensor/Source/AudioSensor/CarlaInterface/AudioSensorActorData.h new file mode 100644 index 00000000000..08b880e2b44 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/AudioSensor/Source/AudioSensor/CarlaInterface/AudioSensorActorData.h @@ -0,0 +1,19 @@ +// Copyright (c) 2024 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 . + +#pragma once + +#include "Carla/Actor/ActorData.h" + + +class FAudioSensorData : public FActorData +{ +public: + + virtual void RecordActorData(FCarlaActor* CarlaActor, UCarlaEpisode* CarlaEpisode) override; + + virtual void RestoreActorData(FCarlaActor* CarlaActor, UCarlaEpisode* CarlaEpisode) override; +}; \ No newline at end of file diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/AudioSensor/Source/AudioSensor/CarlaInterface/CarlaActor.cpp b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/AudioSensor/Source/AudioSensor/CarlaInterface/CarlaActor.cpp new file mode 100644 index 00000000000..c4de1eb8e32 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/AudioSensor/Source/AudioSensor/CarlaInterface/CarlaActor.cpp @@ -0,0 +1,36 @@ +// Copyright (c) 2024 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 . + +#include "CarlaActor.h" +#include "AudioSensor/AudioSensorActor.h" + +const FString FAudioSensorActor::CustomType = "AudioSensor_AudioSensor"; + +FAudioSensorActor::FAudioSensorActor( + IdType ActorId, + AActor* Actor, + TSharedPtr Info, + carla::rpc::ActorState InState, + UWorld* World) + : FCarlaActor(ActorId, Actor, Info, InState, World) +{ + Type = ActorType::Custom; + ActorData = MakeShared(); +} + +TSharedPtr FAudioSensorActor::CreateInstance(FCarlaActor::IdType ActorId, + AActor* Actor, + TSharedPtr Info, + carla::rpc::ActorState InState, + UWorld* World) + { + return MakeShared(ActorId, Actor, std::move(Info), InState, World); + } + +void FAudioSensorActor::RegisterClassWithFactory() + { + CarlaActorConstructorFactory::Instance().Register(TEXT("FAudioSensorActor"), AAudioSensorActor::StaticClass(), CreateInstance); + } diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/AudioSensor/Source/AudioSensor/CarlaInterface/CarlaActor.h b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/AudioSensor/Source/AudioSensor/CarlaInterface/CarlaActor.h new file mode 100644 index 00000000000..23135584f21 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/AudioSensor/Source/AudioSensor/CarlaInterface/CarlaActor.h @@ -0,0 +1,35 @@ +// Copyright (c) 2024 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 . + +#pragma once + +#include "Carla/Actor/CarlaActor.h" +#include "Carla/Actor/CarlaActorConstructorFactory.h" + +class FAudioSensorActor : public FCarlaActor +{ +public: + FAudioSensorActor( + IdType ActorId, + AActor* Actor, + TSharedPtr Info, + carla::rpc::ActorState InState, + UWorld* World); + + static TSharedPtr CreateInstance(FCarlaActor::IdType ActorId, + AActor* Actor, + TSharedPtr Info, + carla::rpc::ActorState InState, + UWorld* World); + + static void RegisterClassWithFactory(); + + protected: + static const FString CustomType; + +}; + + diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/AudioSensor/Source/AudioSensor/ros2/ROS2Audio.cpp b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/AudioSensor/Source/AudioSensor/ros2/ROS2Audio.cpp new file mode 100644 index 00000000000..0dab77b8309 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/AudioSensor/Source/AudioSensor/ros2/ROS2Audio.cpp @@ -0,0 +1,135 @@ +// Copyright (c) 2024 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 . + +#include "ROS2Audio.h" +#include "carla/Logging.h" +#include "carla/geom/GeoLocation.h" +#include "carla/geom/Vector3D.h" +#include "carla/sensor/data/DVSEvent.h" +#include "carla/sensor/data/LidarData.h" +#include "carla/sensor/data/SemanticLidarData.h" +#include "carla/sensor/data/RadarData.h" +#include "carla/sensor/data/Image.h" +#include "carla/sensor/s11n/ImageSerializer.h" +#include "carla/sensor/s11n/SensorHeaderSerializer.h" + +#include "carla/ros2/plugin-utils/enable-fastdds-include.h" // start fastdds includes +#include "carla/ros2/publishers/CarlaPublisher.h" +#include "carla/ros2/publishers/CarlaClockPublisher.h" +#include "carla/ros2/publishers/CarlaTransformPublisher.h" +#include "carla/ros2/publishers/BasicPublisher.h" +#include "AudioSensor/ros2/publishers/CarlaAudioPublisher.h" +#include "carla/ros2/plugin-utils/disable-fastdds-include.h" // end fastdds includes + +#include + +namespace carla { +namespace ros2 { + +// static fields +std::shared_ptr ROS2Audio::_instance; + +void ROS2Audio::Enable(bool enable) { + _enabled = enable; + log_info("ROS2Audio enabled: ", _enabled); +} + +void ROS2Audio::SetFrame(uint64_t frame) { + _frame = frame; +} + +void ROS2Audio::SetTimestamp(double timestamp) { + if (IsEnabled()){ + double integral; + const double fractional = modf(timestamp, &integral); + const double multiplier = 1000000000.0; + _seconds = static_cast(integral); + _nanoseconds = static_cast(fractional * multiplier); + } +} + +void ROS2Audio::Shutdown() { + for (auto& element : _publishers) { + element.second.reset(); + } + for (auto& element : _transforms) { + element.second.reset(); + } + _enabled = false; +} + +void ROS2Audio::RemoveActorRosPublishers(void *actor) +{ + auto p_it = _publishers.find(actor); + if (p_it != _publishers.end()) { + _publishers.erase(actor); + } + + auto t_it = _transforms.find(actor); + if (t_it != _transforms.end()) { + _transforms.erase(actor); + } +} + +std::pair, std::shared_ptr> ROS2Audio::GetOrCreateSensor(carla::streaming::detail::stream_id_type id, void* actor) { + auto it_publishers = _publishers.find(actor); + auto it_transforms = _transforms.find(actor); + std::shared_ptr publisher {}; + std::shared_ptr transform {}; + auto ROS2Interfaces = UActorDispatcher::GetInterfaces(); + if (it_publishers != _publishers.end()) { + publisher = it_publishers->second; + if (it_transforms != _transforms.end()) { + transform = it_transforms->second; + } + } else { + //Sensor not found, creating one of the given type + const std::string string_id = std::to_string(id); + std::string ros_name = ROS2Interfaces->GetActorRosName(actor); + std::string parent_ros_name = ROS2Interfaces->GetActorParentRosName(actor); + if (ros_name == "audio__") { + ros_name.pop_back(); + ros_name.pop_back(); + ros_name += string_id; + ROS2Interfaces->UpdateActorRosName(actor, ros_name); + } + std::shared_ptr new_publisher = std::make_shared(ros_name.c_str(), parent_ros_name.c_str()); + if (new_publisher->Init()) { + _publishers.insert({actor, new_publisher}); + publisher = new_publisher; + } + std::shared_ptr new_transform = std::make_shared(ros_name.c_str(), parent_ros_name.c_str()); + if (new_transform->Init()) { + _transforms.insert({actor, new_transform}); + transform = new_transform; + } + } + return { publisher, transform }; +} + +void ROS2Audio::ProcessDataFromAudio( + carla::streaming::detail::stream_id_type stream_id, + const carla::geom::Transform sensor_transform, + uint32_t num_channels, uint32_t sample_rate, + uint32_t chunk_size, const int16_t *data, + void *actor) { + + log_info("Sensor AudioSensor to ROS data: frame.", _frame, "stream.", stream_id); + { + auto sensors = GetOrCreateSensor(stream_id, actor); + if (sensors.first) { + sensors.first->SetData(_seconds, _nanoseconds, num_channels, sample_rate, chunk_size, data); + sensors.first->Publish(); + } + if (sensors.second) { + sensors.second->SetData(_seconds, _nanoseconds, (const float*)&sensor_transform.location, (const float*)&sensor_transform.rotation); + sensors.second->Publish(); + } + } +} + +} // namespace ros2 +} // namespace carla diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/AudioSensor/Source/AudioSensor/ros2/ROS2Audio.h b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/AudioSensor/Source/AudioSensor/ros2/ROS2Audio.h new file mode 100644 index 00000000000..799e535fb4f --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/AudioSensor/Source/AudioSensor/ros2/ROS2Audio.h @@ -0,0 +1,79 @@ +// Copyright (c) 2024 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 . + +#pragma once + +#include "Carla/Actor/ActorDispatcher.h" +#include "carla/Buffer.h" +#include "carla/BufferView.h" +#include "carla/geom/Transform.h" +#include "carla/ros2/ROS2.h" +#include "carla/ros2/ROS2CallbackData.h" +#include "carla/streaming/detail/Types.h" + +#include +#include +#include +#include + +namespace carla { +namespace ros2 { + + class CarlaAudioPublisher; + +class ROS2Audio : public ROS2 +{ + public: + + // deleting copy constructor for singleton + ROS2Audio(const ROS2Audio& obj) = delete; + static std::shared_ptr GetInstance() { + if (!_instance) + { + _instance = std::shared_ptr(new ROS2Audio); + auto ROS2Interfaces = UActorDispatcher::GetInterfaces(); + ROS2Interfaces->RegisterInterface(_instance); + } + return _instance; + } + + virtual void Enable(bool enable) override; + virtual void Shutdown() override; + virtual bool IsEnabled() override { return _enabled; } + virtual void SetFrame(uint64_t frame) override; + virtual void SetTimestamp(double timestamp) override; + + // ros_name managing + virtual void RemoveActorRosPublishers(void *actor) override; + + // receiving data to publish + void ProcessDataFromAudio( + carla::streaming::detail::stream_id_type stream_id, + const carla::geom::Transform sensor_transform, + uint32_t num_channels, uint32_t sample_rate, + uint32_t chunk_size, const int16_t *data, + void *actor = nullptr); + + // enabling streams to publish + virtual void EnableStream(carla::streaming::detail::stream_id_type id) { _publish_stream.insert(id); } + virtual bool IsStreamEnabled(carla::streaming::detail::stream_id_type id) { return _publish_stream.count(id) > 0; } + virtual void ResetStreams() { _publish_stream.clear(); } + + static std::shared_ptr _instance; + + private: + std::pair, std::shared_ptr> GetOrCreateSensor(carla::streaming::detail::stream_id_type id, void* actor); + + // singleton + ROS2Audio() {}; + + std::unordered_map> _publishers; + std::unordered_map> _transforms; + std::unordered_set _publish_stream; +}; + +} // namespace ros2 +} // namespace carla diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/AudioSensor/Source/AudioSensor/ros2/publishers/CarlaAudioPublisher.cpp b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/AudioSensor/Source/AudioSensor/ros2/publishers/CarlaAudioPublisher.cpp new file mode 100644 index 00000000000..d21b122a513 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/AudioSensor/Source/AudioSensor/ros2/publishers/CarlaAudioPublisher.cpp @@ -0,0 +1,232 @@ +#include "CarlaAudioPublisher.h" + +#include + +#include "AudioSensor/ros2/types/CarlaAudioPubSubTypes.h" +#include "carla/ros2/listeners/CarlaListener.h" + +#include "carla/ros2/plugin-utils/enable-fastdds-include.h" // start fastdds includes +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include "carla/ros2/plugin-utils/disable-fastdds-include.h" // end fastdds includes + +namespace carla { +namespace ros2 { + + namespace efd = eprosima::fastdds::dds; + using erc = eprosima::fastrtps::types::ReturnCode_t; + + struct CarlaAudioPublisherImpl { + efd::DomainParticipant* _participant { nullptr }; + efd::Publisher* _publisher { nullptr }; + efd::Topic* _topic { nullptr }; + efd::DataWriter* _datawriter { nullptr }; + efd::TypeSupport _type { new carla_interfaces::msg::CarlaAudioPubSubType() }; + CarlaListener _listener {}; + carla_interfaces::msg::CarlaAudio _audio {}; + }; + + bool CarlaAudioPublisher::Init() { + if (_impl->_type == nullptr) { + std::cerr << "Invalid TypeSupport" << std::endl; + return false; + } + + efd::DomainParticipantQos pqos = efd::PARTICIPANT_QOS_DEFAULT; + pqos.name(_name); + auto factory = efd::DomainParticipantFactory::get_instance(); + _impl->_participant = factory->create_participant(0, pqos); + if (_impl->_participant == nullptr) { + std::cerr << "Failed to create DomainParticipant" << std::endl; + return false; + } + _impl->_type.register_type(_impl->_participant); + + efd::PublisherQos pubqos = efd::PUBLISHER_QOS_DEFAULT; + _impl->_publisher = _impl->_participant->create_publisher(pubqos, nullptr); + if (_impl->_publisher == nullptr) { + std::cerr << "Failed to create Publisher" << std::endl; + return false; + } + + efd::TopicQos tqos = efd::TOPIC_QOS_DEFAULT; + const std::string base { "rt/carla/" }; + std::string topic_name = base; + if (!_parent.empty()) + topic_name += _parent + "/"; + topic_name += _name; + _impl->_topic = _impl->_participant->create_topic(topic_name, _impl->_type->getName(), tqos); + if (_impl->_topic == nullptr) { + std::cerr << "Failed to create Topic" << std::endl; + return false; + } + efd::DataWriterQos wqos = efd::DATAWRITER_QOS_DEFAULT; + wqos.endpoint().history_memory_policy = eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + efd::DataWriterListener* listener = (efd::DataWriterListener*)_impl->_listener._impl.get(); + _impl->_datawriter = _impl->_publisher->create_datawriter(_impl->_topic, wqos, listener); + if (_impl->_datawriter == nullptr) { + std::cerr << "Failed to create DataWriter" << std::endl; + return false; + } + + _frame_id = _name; + return true; + } + + bool CarlaAudioPublisher::Publish() { + eprosima::fastrtps::rtps::InstanceHandle_t instance_handle; + eprosima::fastrtps::types::ReturnCode_t rcode = _impl->_datawriter->write(&_impl->_audio, instance_handle); + if (rcode == erc::ReturnCodeValue::RETCODE_OK) { + return true; + } + if (rcode == erc::ReturnCodeValue::RETCODE_ERROR) { + std::cerr << "RETCODE_ERROR" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_UNSUPPORTED) { + std::cerr << "RETCODE_UNSUPPORTED" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_BAD_PARAMETER) { + std::cerr << "RETCODE_BAD_PARAMETER" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_PRECONDITION_NOT_MET) { + std::cerr << "RETCODE_PRECONDITION_NOT_MET" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_OUT_OF_RESOURCES) { + std::cerr << "RETCODE_OUT_OF_RESOURCES" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ENABLED) { + std::cerr << "RETCODE_NOT_ENABLED" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_IMMUTABLE_POLICY) { + std::cerr << "RETCODE_IMMUTABLE_POLICY" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_INCONSISTENT_POLICY) { + std::cerr << "RETCODE_INCONSISTENT_POLICY" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_ALREADY_DELETED) { + std::cerr << "RETCODE_ALREADY_DELETED" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_TIMEOUT) { + std::cerr << "RETCODE_TIMEOUT" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_NO_DATA) { + std::cerr << "RETCODE_NO_DATA" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_ILLEGAL_OPERATION) { + std::cerr << "RETCODE_ILLEGAL_OPERATION" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ALLOWED_BY_SECURITY) { + std::cerr << "RETCODE_NOT_ALLOWED_BY_SECURITY" << std::endl; + return false; + } + std::cerr << "UNKNOWN" << std::endl; + return false; + } + +void CarlaAudioPublisher::SetData(int32_t seconds, uint32_t nanoseconds, uint32_t num_channels, uint32_t sample_rate, uint32_t chunk_size, const int16_t* data) { + std::vector vector_data; + const size_t size = chunk_size * num_channels; + vector_data.resize(size); + std::memcpy(&vector_data[0], &data[0], size * sizeof(data[0])); + uint32_t sample_format = carla_interfaces::msg::CarlaAudio_Constants::PA_INT16; + SetData(seconds, nanoseconds, sample_format, num_channels, sample_rate, chunk_size, std::move(vector_data)); + } + + void CarlaAudioPublisher::SetData(int32_t seconds, uint32_t nanoseconds, uint32_t sample_format, uint32_t num_channels, uint32_t sample_rate, uint32_t chunk_size, std::vector&& data) { + builtin_interfaces::msg::Time time; + time.sec(seconds); + time.nanosec(nanoseconds); + + std_msgs::msg::Header header; + header.stamp(std::move(time)); + header.frame_id(_frame_id); + _impl->_audio.header(header); + + _impl->_audio.sample_format(sample_format); + _impl->_audio.num_channels(num_channels); + _impl->_audio.sample_rate(sample_rate); + _impl->_audio.chunk_size(chunk_size); + _impl->_audio.data(std::move(data)); + } + + CarlaAudioPublisher::CarlaAudioPublisher(const char* ros_name, const char* parent) : + _impl(std::make_shared()) { + _name = ros_name; + _parent = parent; + } + + CarlaAudioPublisher::~CarlaAudioPublisher() { + if (!_impl) + return; + + if (_impl->_datawriter) + _impl->_publisher->delete_datawriter(_impl->_datawriter); + + if (_impl->_publisher) + _impl->_participant->delete_publisher(_impl->_publisher); + + if (_impl->_topic) + _impl->_participant->delete_topic(_impl->_topic); + + if (_impl->_participant) + efd::DomainParticipantFactory::get_instance()->delete_participant(_impl->_participant); + } + + CarlaAudioPublisher::CarlaAudioPublisher(const CarlaAudioPublisher& other) { + _frame_id = other._frame_id; + _name = other._name; + _parent = other._parent; + _impl = other._impl; + } + + CarlaAudioPublisher& CarlaAudioPublisher::operator=(const CarlaAudioPublisher& other) { + _frame_id = other._frame_id; + _name = other._name; + _parent = other._parent; + _impl = other._impl; + + return *this; + } + + CarlaAudioPublisher::CarlaAudioPublisher(CarlaAudioPublisher&& other) { + _frame_id = std::move(other._frame_id); + _name = std::move(other._name); + _parent = std::move(other._parent); + _impl = std::move(other._impl); + } + + CarlaAudioPublisher& CarlaAudioPublisher::operator=(CarlaAudioPublisher&& 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; + } +} +} diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/AudioSensor/Source/AudioSensor/ros2/publishers/CarlaAudioPublisher.h b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/AudioSensor/Source/AudioSensor/ros2/publishers/CarlaAudioPublisher.h new file mode 100644 index 00000000000..e6b9512803d --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/AudioSensor/Source/AudioSensor/ros2/publishers/CarlaAudioPublisher.h @@ -0,0 +1,38 @@ +// Copyright (c) 2024 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 . + +#pragma once + +#include +#include + +#include "carla/ros2/publishers/CarlaPublisher.h" + +namespace carla { +namespace ros2 { + + struct CarlaAudioPublisherImpl; + + class CarlaAudioPublisher : public CarlaPublisher { + public: + CarlaAudioPublisher(const char* ros_name = "", const char* parent = ""); + ~CarlaAudioPublisher(); + CarlaAudioPublisher(const CarlaAudioPublisher&); + CarlaAudioPublisher& operator=(const CarlaAudioPublisher&); + CarlaAudioPublisher(CarlaAudioPublisher&&); + CarlaAudioPublisher& operator=(CarlaAudioPublisher&&); + + bool Init(); + bool Publish(); + void SetData(int32_t seconds, uint32_t nanoseconds, uint32_t num_channels, uint32_t sample_rate, uint32_t chunk_size, const int16_t* data); + const char* type() const override { return "audio"; } + + private: + void SetData(int32_t seconds, uint32_t nanoseconds, uint32_t sample_format, uint32_t num_channels, uint32_t sample_rate, uint32_t chunk_size, std::vector&& data); + + private: + std::shared_ptr _impl; + }; +} +} diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/AudioSensor/Source/AudioSensor/ros2/types/CarlaAudio.cpp b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/AudioSensor/Source/AudioSensor/ros2/types/CarlaAudio.cpp new file mode 100644 index 00000000000..09edadce1c4 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/AudioSensor/Source/AudioSensor/ros2/types/CarlaAudio.cpp @@ -0,0 +1,406 @@ +// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// 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. + +/*! + * @file CarlaAudio.cpp + * This source file contains the definition of the described types in the IDL file. + * + * This file was generated by the tool gen. + */ + +#ifdef _WIN32 +// Remove linker warning LNK4221 on Visual Studio +namespace { +char dummy; +} // namespace +#endif // _WIN32 + +#include "CarlaAudio.h" + +#include "carla/ros2/plugin-utils/enable-fastdds-include.h" // start fastdds includes +#include +#include "carla/ros2/plugin-utils/disable-fastdds-include.h" // end fastdds includes + +#include +using namespace eprosima::fastcdr::exception; + +#include + +#define builtin_interfaces_msg_Time_max_cdr_typesize 8ULL; +#define carla_interfaces_msg_CarlaAudio_max_cdr_typesize 488ULL; +#define std_msgs_msg_Header_max_cdr_typesize 268ULL; +#define builtin_interfaces_msg_Time_max_key_cdr_typesize 0ULL; +#define carla_interfaces_msg_CarlaAudio_max_key_cdr_typesize 0ULL; +#define std_msgs_msg_Header_max_key_cdr_typesize 0ULL; + + +carla_interfaces::msg::CarlaAudio::CarlaAudio() +{ + // std_msgs::msg::Header m_header + + // unsigned long m_sample_format + m_sample_format = 0; + // unsigned long m_num_channels + m_num_channels = 0; + // unsigned long m_sample_rate + m_sample_rate = 0; + // unsigned long m_chunk_size + m_chunk_size = 0; + // sequence m_data + + +} + +carla_interfaces::msg::CarlaAudio::~CarlaAudio() +{ + +} + +carla_interfaces::msg::CarlaAudio::CarlaAudio( + const CarlaAudio& x) +{ + m_header = x.m_header; + m_sample_format = x.m_sample_format; + m_num_channels = x.m_num_channels; + m_sample_rate = x.m_sample_rate; + m_chunk_size = x.m_chunk_size; + m_data = x.m_data; +} + +carla_interfaces::msg::CarlaAudio::CarlaAudio( + CarlaAudio&& x) noexcept +{ + m_header = std::move(x.m_header); + m_sample_format = x.m_sample_format; + m_num_channels = x.m_num_channels; + m_sample_rate = x.m_sample_rate; + m_chunk_size = x.m_chunk_size; + m_data = std::move(x.m_data); +} + +carla_interfaces::msg::CarlaAudio& carla_interfaces::msg::CarlaAudio::operator =( + const CarlaAudio& x) +{ + + m_header = x.m_header; + m_sample_format = x.m_sample_format; + m_num_channels = x.m_num_channels; + m_sample_rate = x.m_sample_rate; + m_chunk_size = x.m_chunk_size; + m_data = x.m_data; + + return *this; +} + +carla_interfaces::msg::CarlaAudio& carla_interfaces::msg::CarlaAudio::operator =( + CarlaAudio&& x) noexcept +{ + + m_header = std::move(x.m_header); + m_sample_format = x.m_sample_format; + m_num_channels = x.m_num_channels; + m_sample_rate = x.m_sample_rate; + m_chunk_size = x.m_chunk_size; + m_data = std::move(x.m_data); + + return *this; +} + +bool carla_interfaces::msg::CarlaAudio::operator ==( + const CarlaAudio& x) const +{ + + return (m_header == x.m_header && m_sample_format == x.m_sample_format && m_num_channels == x.m_num_channels && m_sample_rate == x.m_sample_rate && m_chunk_size == x.m_chunk_size && m_data == x.m_data); +} + +bool carla_interfaces::msg::CarlaAudio::operator !=( + const CarlaAudio& x) const +{ + return !(*this == x); +} + +size_t carla_interfaces::msg::CarlaAudio::getMaxCdrSerializedSize( + size_t current_alignment) +{ + static_cast(current_alignment); + return carla_interfaces_msg_CarlaAudio_max_cdr_typesize; +} + +size_t carla_interfaces::msg::CarlaAudio::getCdrSerializedSize( + const carla_interfaces::msg::CarlaAudio& data, + size_t current_alignment) +{ + (void)data; + size_t initial_alignment = current_alignment; + + + current_alignment += std_msgs::msg::Header::getCdrSerializedSize(data.header(), current_alignment); + current_alignment += 4 + eprosima::fastcdr::Cdr::alignment(current_alignment, 4); + + + current_alignment += 4 + eprosima::fastcdr::Cdr::alignment(current_alignment, 4); + + + current_alignment += 4 + eprosima::fastcdr::Cdr::alignment(current_alignment, 4); + + + current_alignment += 4 + eprosima::fastcdr::Cdr::alignment(current_alignment, 4); + + + current_alignment += 4 + eprosima::fastcdr::Cdr::alignment(current_alignment, 4); + + if (data.data().size() > 0) + { + current_alignment += (data.data().size() * 2) + eprosima::fastcdr::Cdr::alignment(current_alignment, 2); + } + + + + + return current_alignment - initial_alignment; +} + +void carla_interfaces::msg::CarlaAudio::serialize( + eprosima::fastcdr::Cdr& scdr) const +{ + + scdr << m_header; + scdr << m_sample_format; + scdr << m_num_channels; + scdr << m_sample_rate; + scdr << m_chunk_size; + scdr << m_data; + +} + +void carla_interfaces::msg::CarlaAudio::deserialize( + eprosima::fastcdr::Cdr& dcdr) +{ + + dcdr >> m_header; + dcdr >> m_sample_format; + dcdr >> m_num_channels; + dcdr >> m_sample_rate; + dcdr >> m_chunk_size; + dcdr >> m_data; +} + +/*! + * @brief This function copies the value in member header + * @param _header New value to be copied in member header + */ +void carla_interfaces::msg::CarlaAudio::header( + const std_msgs::msg::Header& _header) +{ + m_header = _header; +} + +/*! + * @brief This function moves the value in member header + * @param _header New value to be moved in member header + */ +void carla_interfaces::msg::CarlaAudio::header( + std_msgs::msg::Header&& _header) +{ + m_header = std::move(_header); +} + +/*! + * @brief This function returns a constant reference to member header + * @return Constant reference to member header + */ +const std_msgs::msg::Header& carla_interfaces::msg::CarlaAudio::header() const +{ + return m_header; +} + +/*! + * @brief This function returns a reference to member header + * @return Reference to member header + */ +std_msgs::msg::Header& carla_interfaces::msg::CarlaAudio::header() +{ + return m_header; +} +/*! + * @brief This function sets a value in member sample_format + * @param _sample_format New value for member sample_format + */ +void carla_interfaces::msg::CarlaAudio::sample_format( + uint32_t _sample_format) +{ + m_sample_format = _sample_format; +} + +/*! + * @brief This function returns the value of member sample_format + * @return Value of member sample_format + */ +uint32_t carla_interfaces::msg::CarlaAudio::sample_format() const +{ + return m_sample_format; +} + +/*! + * @brief This function returns a reference to member sample_format + * @return Reference to member sample_format + */ +uint32_t& carla_interfaces::msg::CarlaAudio::sample_format() +{ + return m_sample_format; +} + +/*! + * @brief This function sets a value in member num_channels + * @param _num_channels New value for member num_channels + */ +void carla_interfaces::msg::CarlaAudio::num_channels( + uint32_t _num_channels) +{ + m_num_channels = _num_channels; +} + +/*! + * @brief This function returns the value of member num_channels + * @return Value of member num_channels + */ +uint32_t carla_interfaces::msg::CarlaAudio::num_channels() const +{ + return m_num_channels; +} + +/*! + * @brief This function returns a reference to member num_channels + * @return Reference to member num_channels + */ +uint32_t& carla_interfaces::msg::CarlaAudio::num_channels() +{ + return m_num_channels; +} + +/*! + * @brief This function sets a value in member sample_rate + * @param _sample_rate New value for member sample_rate + */ +void carla_interfaces::msg::CarlaAudio::sample_rate( + uint32_t _sample_rate) +{ + m_sample_rate = _sample_rate; +} + +/*! + * @brief This function returns the value of member sample_rate + * @return Value of member sample_rate + */ +uint32_t carla_interfaces::msg::CarlaAudio::sample_rate() const +{ + return m_sample_rate; +} + +/*! + * @brief This function returns a reference to member sample_rate + * @return Reference to member sample_rate + */ +uint32_t& carla_interfaces::msg::CarlaAudio::sample_rate() +{ + return m_sample_rate; +} + +/*! + * @brief This function sets a value in member chunk_size + * @param _chunk_size New value for member chunk_size + */ +void carla_interfaces::msg::CarlaAudio::chunk_size( + uint32_t _chunk_size) +{ + m_chunk_size = _chunk_size; +} + +/*! + * @brief This function returns the value of member chunk_size + * @return Value of member chunk_size + */ +uint32_t carla_interfaces::msg::CarlaAudio::chunk_size() const +{ + return m_chunk_size; +} + +/*! + * @brief This function returns a reference to member chunk_size + * @return Reference to member chunk_size + */ +uint32_t& carla_interfaces::msg::CarlaAudio::chunk_size() +{ + return m_chunk_size; +} + +/*! + * @brief This function copies the value in member data + * @param _data New value to be copied in member data + */ +void carla_interfaces::msg::CarlaAudio::data( + const std::vector& _data) +{ + m_data = _data; +} + +/*! + * @brief This function moves the value in member data + * @param _data New value to be moved in member data + */ +void carla_interfaces::msg::CarlaAudio::data( + std::vector&& _data) +{ + m_data = std::move(_data); +} + +/*! + * @brief This function returns a constant reference to member data + * @return Constant reference to member data + */ +const std::vector& carla_interfaces::msg::CarlaAudio::data() const +{ + return m_data; +} + +/*! + * @brief This function returns a reference to member data + * @return Reference to member data + */ +std::vector& carla_interfaces::msg::CarlaAudio::data() +{ + return m_data; +} + + +size_t carla_interfaces::msg::CarlaAudio::getKeyMaxCdrSerializedSize( + size_t current_alignment) +{ + static_cast(current_alignment); + return carla_interfaces_msg_CarlaAudio_max_key_cdr_typesize; +} + +bool carla_interfaces::msg::CarlaAudio::isKeyDefined() +{ + return false; +} + +void carla_interfaces::msg::CarlaAudio::serializeKey( + eprosima::fastcdr::Cdr& scdr) const +{ + (void) scdr; +} + + + diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/AudioSensor/Source/AudioSensor/ros2/types/CarlaAudio.h b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/AudioSensor/Source/AudioSensor/ros2/types/CarlaAudio.h new file mode 100644 index 00000000000..3d73b23219c --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/AudioSensor/Source/AudioSensor/ros2/types/CarlaAudio.h @@ -0,0 +1,331 @@ +// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// 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. + +/*! + * @file CarlaAudio.h + * This header file contains the declaration of the described types in the IDL file. + * + * This file was generated by the tool gen. + */ + +#ifndef _FAST_DDS_GENERATED_CARLA_INTERFACES_MSG_CARLAAUDIO_H_ +#define _FAST_DDS_GENERATED_CARLA_INTERFACES_MSG_CARLAAUDIO_H_ + +#include "carla/ros2/plugin-utils/enable-fastdds-include.h" // start fastdds includes +#include "carla/ros2/types/Header.h" +#include +#include "carla/ros2/plugin-utils/disable-fastdds-include.h" // end fastdds includes + +#include +#include +#include +#include +#include +#include + +#if defined(_WIN32) +#if defined(EPROSIMA_USER_DLL_EXPORT) +#define eProsima_user_DllExport __declspec( dllexport ) +#else +#define eProsima_user_DllExport +#endif // EPROSIMA_USER_DLL_EXPORT +#else +#define eProsima_user_DllExport +#endif // _WIN32 + +#if defined(_WIN32) +#if defined(EPROSIMA_USER_DLL_EXPORT) +#if defined(CARLAAUDIO_SOURCE) +#define CARLAAUDIO_DllAPI __declspec( dllexport ) +#else +#define CARLAAUDIO_DllAPI __declspec( dllimport ) +#endif // CARLAAUDIO_SOURCE +#else +#define CARLAAUDIO_DllAPI +#endif // EPROSIMA_USER_DLL_EXPORT +#else +#define CARLAAUDIO_DllAPI +#endif // _WIN32 + +namespace eprosima { +namespace fastcdr { +class Cdr; +} // namespace fastcdr +} // namespace eprosima + + +namespace carla_interfaces { + namespace msg { + namespace CarlaAudio_Constants { + const uint32_t PA_INT16 = 8; + } // namespace CarlaAudio_Constants + /*! + * @brief This class represents the structure CarlaAudio defined by the user in the IDL file. + * @ingroup CarlaAudio + */ + class CarlaAudio + { + public: + + /*! + * @brief Default constructor. + */ + eProsima_user_DllExport CarlaAudio(); + + /*! + * @brief Default destructor. + */ + eProsima_user_DllExport ~CarlaAudio(); + + /*! + * @brief Copy constructor. + * @param x Reference to the object carla_interfaces::msg::CarlaAudio that will be copied. + */ + eProsima_user_DllExport CarlaAudio( + const CarlaAudio& x); + + /*! + * @brief Move constructor. + * @param x Reference to the object carla_interfaces::msg::CarlaAudio that will be copied. + */ + eProsima_user_DllExport CarlaAudio( + CarlaAudio&& x) noexcept; + + /*! + * @brief Copy assignment. + * @param x Reference to the object carla_interfaces::msg::CarlaAudio that will be copied. + */ + eProsima_user_DllExport CarlaAudio& operator =( + const CarlaAudio& x); + + /*! + * @brief Move assignment. + * @param x Reference to the object carla_interfaces::msg::CarlaAudio that will be copied. + */ + eProsima_user_DllExport CarlaAudio& operator =( + CarlaAudio&& x) noexcept; + + /*! + * @brief Comparison operator. + * @param x carla_interfaces::msg::CarlaAudio object to compare. + */ + eProsima_user_DllExport bool operator ==( + const CarlaAudio& x) const; + + /*! + * @brief Comparison operator. + * @param x carla_interfaces::msg::CarlaAudio object to compare. + */ + eProsima_user_DllExport bool operator !=( + const CarlaAudio& x) const; + + /*! + * @brief This function copies the value in member header + * @param _header New value to be copied in member header + */ + eProsima_user_DllExport void header( + const std_msgs::msg::Header& _header); + + /*! + * @brief This function moves the value in member header + * @param _header New value to be moved in member header + */ + eProsima_user_DllExport void header( + std_msgs::msg::Header&& _header); + + /*! + * @brief This function returns a constant reference to member header + * @return Constant reference to member header + */ + eProsima_user_DllExport const std_msgs::msg::Header& header() const; + + /*! + * @brief This function returns a reference to member header + * @return Reference to member header + */ + eProsima_user_DllExport std_msgs::msg::Header& header(); + /*! + * @brief This function sets a value in member sample_format + * @param _sample_format New value for member sample_format + */ + eProsima_user_DllExport void sample_format( + uint32_t _sample_format); + + /*! + * @brief This function returns the value of member sample_format + * @return Value of member sample_format + */ + eProsima_user_DllExport uint32_t sample_format() const; + + /*! + * @brief This function returns a reference to member sample_format + * @return Reference to member sample_format + */ + eProsima_user_DllExport uint32_t& sample_format(); + + /*! + * @brief This function sets a value in member num_channels + * @param _num_channels New value for member num_channels + */ + eProsima_user_DllExport void num_channels( + uint32_t _num_channels); + + /*! + * @brief This function returns the value of member num_channels + * @return Value of member num_channels + */ + eProsima_user_DllExport uint32_t num_channels() const; + + /*! + * @brief This function returns a reference to member num_channels + * @return Reference to member num_channels + */ + eProsima_user_DllExport uint32_t& num_channels(); + + /*! + * @brief This function sets a value in member sample_rate + * @param _sample_rate New value for member sample_rate + */ + eProsima_user_DllExport void sample_rate( + uint32_t _sample_rate); + + /*! + * @brief This function returns the value of member sample_rate + * @return Value of member sample_rate + */ + eProsima_user_DllExport uint32_t sample_rate() const; + + /*! + * @brief This function returns a reference to member sample_rate + * @return Reference to member sample_rate + */ + eProsima_user_DllExport uint32_t& sample_rate(); + + /*! + * @brief This function sets a value in member chunk_size + * @param _chunk_size New value for member chunk_size + */ + eProsima_user_DllExport void chunk_size( + uint32_t _chunk_size); + + /*! + * @brief This function returns the value of member chunk_size + * @return Value of member chunk_size + */ + eProsima_user_DllExport uint32_t chunk_size() const; + + /*! + * @brief This function returns a reference to member chunk_size + * @return Reference to member chunk_size + */ + eProsima_user_DllExport uint32_t& chunk_size(); + + /*! + * @brief This function copies the value in member data + * @param _data New value to be copied in member data + */ + eProsima_user_DllExport void data( + const std::vector& _data); + + /*! + * @brief This function moves the value in member data + * @param _data New value to be moved in member data + */ + eProsima_user_DllExport void data( + std::vector&& _data); + + /*! + * @brief This function returns a constant reference to member data + * @return Constant reference to member data + */ + eProsima_user_DllExport const std::vector& data() const; + + /*! + * @brief This function returns a reference to member data + * @return Reference to member data + */ + eProsima_user_DllExport std::vector& data(); + + /*! + * @brief This function returns the maximum serialized size of an object + * depending on the buffer alignment. + * @param current_alignment Buffer alignment. + * @return Maximum serialized size. + */ + eProsima_user_DllExport static size_t getMaxCdrSerializedSize( + size_t current_alignment = 0); + + /*! + * @brief This function returns the serialized size of a data depending on the buffer alignment. + * @param data Data which is calculated its serialized size. + * @param current_alignment Buffer alignment. + * @return Serialized size. + */ + eProsima_user_DllExport static size_t getCdrSerializedSize( + const carla_interfaces::msg::CarlaAudio& data, + size_t current_alignment = 0); + + + /*! + * @brief This function serializes an object using CDR serialization. + * @param cdr CDR serialization object. + */ + eProsima_user_DllExport void serialize( + eprosima::fastcdr::Cdr& cdr) const; + + /*! + * @brief This function deserializes an object using CDR serialization. + * @param cdr CDR serialization object. + */ + eProsima_user_DllExport void deserialize( + eprosima::fastcdr::Cdr& cdr); + + + + /*! + * @brief This function returns the maximum serialized size of the Key of an object + * depending on the buffer alignment. + * @param current_alignment Buffer alignment. + * @return Maximum serialized size. + */ + eProsima_user_DllExport static size_t getKeyMaxCdrSerializedSize( + size_t current_alignment = 0); + + /*! + * @brief This function tells you if the Key has been defined for this type + */ + eProsima_user_DllExport static bool isKeyDefined(); + + /*! + * @brief This function serializes the key members of an object using CDR serialization. + * @param cdr CDR serialization object. + */ + eProsima_user_DllExport void serializeKey( + eprosima::fastcdr::Cdr& cdr) const; + + private: + + std_msgs::msg::Header m_header; + uint32_t m_sample_format; + uint32_t m_num_channels; + uint32_t m_sample_rate; + uint32_t m_chunk_size; + std::vector m_data; + + }; + } // namespace msg +} // namespace carla_interfaces + +#endif // _FAST_DDS_GENERATED_CARLA_INTERFACES_MSG_CARLAAUDIO_H_ + diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/AudioSensor/Source/AudioSensor/ros2/types/CarlaAudioPubSubTypes.cpp b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/AudioSensor/Source/AudioSensor/ros2/types/CarlaAudioPubSubTypes.cpp new file mode 100644 index 00000000000..9ca017c9882 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/AudioSensor/Source/AudioSensor/ros2/types/CarlaAudioPubSubTypes.cpp @@ -0,0 +1,182 @@ +// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// 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. + +/*! + * @file CarlaAudioPubSubTypes.cpp + * This header file contains the implementation of the serialization functions. + * + * This file was generated by the tool fastcdrgen. + */ + +#include "carla/ros2/plugin-utils/enable-fastdds-include.h" // start fastdds includes +#include +#include +#include "carla/ros2/plugin-utils/disable-fastdds-include.h" // end fastdds includes + +#include "CarlaAudioPubSubTypes.h" + +using SerializedPayload_t = eprosima::fastrtps::rtps::SerializedPayload_t; +using InstanceHandle_t = eprosima::fastrtps::rtps::InstanceHandle_t; + +namespace carla_interfaces { + namespace msg { + namespace CarlaAudio_Constants { + } //End of namespace CarlaAudio_Constants + + CarlaAudioPubSubType::CarlaAudioPubSubType() + { + setName("carla_interfaces::msg::dds_::CarlaAudio_"); + auto type_size = CarlaAudio::getMaxCdrSerializedSize(); + type_size += eprosima::fastcdr::Cdr::alignment(type_size, 4); /* possible submessage alignment */ + m_typeSize = static_cast(type_size) + 4; /*encapsulation*/ + m_isGetKeyDefined = CarlaAudio::isKeyDefined(); + size_t keyLength = CarlaAudio::getKeyMaxCdrSerializedSize() > 16 ? + CarlaAudio::getKeyMaxCdrSerializedSize() : 16; + m_keyBuffer = reinterpret_cast(malloc(keyLength)); + memset(m_keyBuffer, 0, keyLength); + } + + CarlaAudioPubSubType::~CarlaAudioPubSubType() + { + if (m_keyBuffer != nullptr) + { + free(m_keyBuffer); + } + } + + bool CarlaAudioPubSubType::serialize( + void* data, + SerializedPayload_t* payload) + { + CarlaAudio* p_type = static_cast(data); + + // Object that manages the raw buffer. + eprosima::fastcdr::FastBuffer fastbuffer(reinterpret_cast(payload->data), payload->max_size); + // Object that serializes the data. + eprosima::fastcdr::Cdr ser(fastbuffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, eprosima::fastcdr::Cdr::DDS_CDR); + payload->encapsulation = ser.endianness() == eprosima::fastcdr::Cdr::BIG_ENDIANNESS ? CDR_BE : CDR_LE; + + try + { + // Serialize encapsulation + ser.serialize_encapsulation(); + // Serialize the object. + p_type->serialize(ser); + } + catch (eprosima::fastcdr::exception::Exception& /*exception*/) + { + return false; + } + + // Get the serialized length + payload->length = static_cast(ser.getSerializedDataLength()); + return true; + } + + bool CarlaAudioPubSubType::deserialize( + SerializedPayload_t* payload, + void* data) + { + try + { + // Convert DATA to pointer of your type + CarlaAudio* p_type = static_cast(data); + + // Object that manages the raw buffer. + eprosima::fastcdr::FastBuffer fastbuffer(reinterpret_cast(payload->data), payload->length); + + // Object that deserializes the data. + eprosima::fastcdr::Cdr deser(fastbuffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, eprosima::fastcdr::Cdr::DDS_CDR); + + // Deserialize encapsulation. + deser.read_encapsulation(); + payload->encapsulation = deser.endianness() == eprosima::fastcdr::Cdr::BIG_ENDIANNESS ? CDR_BE : CDR_LE; + + // Deserialize the object. + p_type->deserialize(deser); + } + catch (eprosima::fastcdr::exception::Exception& /*exception*/) + { + return false; + } + + return true; + } + + std::function CarlaAudioPubSubType::getSerializedSizeProvider( + void* data) + { + return [data]() -> uint32_t + { + return static_cast(type::getCdrSerializedSize(*static_cast(data))) + + 4u /*encapsulation*/; + }; + } + + void* CarlaAudioPubSubType::createData() + { + return reinterpret_cast(new CarlaAudio()); + } + + void CarlaAudioPubSubType::deleteData( + void* data) + { + delete(reinterpret_cast(data)); + } + + bool CarlaAudioPubSubType::getKey( + void* data, + InstanceHandle_t* handle, + bool force_md5) + { + if (!m_isGetKeyDefined) + { + return false; + } + + CarlaAudio* p_type = static_cast(data); + + // Object that manages the raw buffer. + eprosima::fastcdr::FastBuffer fastbuffer(reinterpret_cast(m_keyBuffer), + CarlaAudio::getKeyMaxCdrSerializedSize()); + + // Object that serializes the data. + eprosima::fastcdr::Cdr ser(fastbuffer, eprosima::fastcdr::Cdr::BIG_ENDIANNESS); + p_type->serializeKey(ser); + if (force_md5 || CarlaAudio::getKeyMaxCdrSerializedSize() > 16) + { + m_md5.init(); + m_md5.update(m_keyBuffer, static_cast(ser.getSerializedDataLength())); + m_md5.finalize(); + for (uint8_t i = 0; i < 16; ++i) + { + handle->value[i] = m_md5.digest[i]; + } + } + else + { + for (uint8_t i = 0; i < 16; ++i) + { + handle->value[i] = m_keyBuffer[i]; + } + } + return true; + } + + + } //End of namespace msg + + +} //End of namespace carla_interfaces + diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/AudioSensor/Source/AudioSensor/ros2/types/CarlaAudioPubSubTypes.h b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/AudioSensor/Source/AudioSensor/ros2/types/CarlaAudioPubSubTypes.h new file mode 100644 index 00000000000..566513895c8 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/AudioSensor/Source/AudioSensor/ros2/types/CarlaAudioPubSubTypes.h @@ -0,0 +1,118 @@ +// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// 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. + +/*! + * @file CarlaAudioPubSubTypes.h + * This header file contains the declaration of the serialization functions. + * + * This file was generated by the tool fastcdrgen. + */ + + +#ifndef _FAST_DDS_GENERATED_CARLA_INTERFACES_MSG_CARLAAUDIO_PUBSUBTYPES_H_ +#define _FAST_DDS_GENERATED_CARLA_INTERFACES_MSG_CARLAAUDIO_PUBSUBTYPES_H_ + +#include "carla/ros2/plugin-utils/enable-fastdds-include.h" // start fastdds includes +#include +#include +#include "carla/ros2/types/HeaderPubSubTypes.h" +#include "carla/ros2/plugin-utils/disable-fastdds-include.h" // end fastdds includes + +#include "CarlaAudio.h" + + + +#if !defined(GEN_API_VER) || (GEN_API_VER != 1) +#error \ + Generated CarlaAudio is not compatible with current installed Fast DDS. Please, regenerate it with fastddsgen. +#endif // GEN_API_VER + +namespace carla_interfaces +{ + namespace msg + { + namespace CarlaAudio_Constants + { + } + + /*! + * @brief This class represents the TopicDataType of the type CarlaAudio defined by the user in the IDL file. + * @ingroup CarlaAudio + */ + class CarlaAudioPubSubType : public eprosima::fastdds::dds::TopicDataType + { + public: + + typedef CarlaAudio type; + + eProsima_user_DllExport CarlaAudioPubSubType(); + + eProsima_user_DllExport virtual ~CarlaAudioPubSubType() override; + + eProsima_user_DllExport virtual bool serialize( + void* data, + eprosima::fastrtps::rtps::SerializedPayload_t* payload) override; + + eProsima_user_DllExport virtual bool deserialize( + eprosima::fastrtps::rtps::SerializedPayload_t* payload, + void* data) override; + + eProsima_user_DllExport virtual std::function getSerializedSizeProvider( + void* data) override; + + eProsima_user_DllExport virtual bool getKey( + void* data, + eprosima::fastrtps::rtps::InstanceHandle_t* ihandle, + bool force_md5 = false) override; + + eProsima_user_DllExport virtual void* createData() override; + + eProsima_user_DllExport virtual void deleteData( + void* data) override; + + #ifdef TOPIC_DATA_TYPE_API_HAS_IS_BOUNDED + eProsima_user_DllExport inline bool is_bounded() const override + { + return false; + } + + #endif // TOPIC_DATA_TYPE_API_HAS_IS_BOUNDED + + #ifdef TOPIC_DATA_TYPE_API_HAS_IS_PLAIN + eProsima_user_DllExport inline bool is_plain() const override + { + return false; + } + + #endif // TOPIC_DATA_TYPE_API_HAS_IS_PLAIN + + #ifdef TOPIC_DATA_TYPE_API_HAS_CONSTRUCT_SAMPLE + eProsima_user_DllExport inline bool construct_sample( + void* memory) const override + { + (void)memory; + return false; + } + + #endif // TOPIC_DATA_TYPE_API_HAS_CONSTRUCT_SAMPLE + + MD5 m_md5; + unsigned char* m_keyBuffer; + + }; + } +} + +#endif // _FAST_DDS_GENERATED_CARLA_INTERFACES_MSG_CARLAAUDIO_PUBSUBTYPES_H_ + diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/CameraController.uplugin b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/CameraController.uplugin new file mode 100644 index 00000000000..8e5da7817e9 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/CameraController.uplugin @@ -0,0 +1,31 @@ +{ + "FileVersion": 3, + "Version": 1, + "VersionName": "1.0", + "FriendlyName": "CameraController", + "Description": "Camera controller for a given camera", + "Category": "Other", + "CreatedBy": "Solomon Wiznitzer", + "CreatedByURL": "", + "DocsURL": "", + "MarketplaceURL": "", + "EnabledByDefault" : true, + "SupportURL": "", + "CanContainContent": true, + "IsBetaVersion": true, + "IsExperimentalVersion": false, + "Installed": false, + "Modules": [ + { + "Name": "CameraController", + "Type": "Runtime", + "LoadingPhase": "Default" + } + ], + "Plugins": [ + { + "Name": "Carla", + "Enabled": true + } + ] +} \ No newline at end of file diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Resources/Icon128.png b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Resources/Icon128.png new file mode 100644 index 00000000000..1231d4aad4d Binary files /dev/null and b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Resources/Icon128.png differ diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/CameraControl.h b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/CameraControl.h new file mode 100644 index 00000000000..d914e6d0f76 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/CameraControl.h @@ -0,0 +1,24 @@ +// Copyright (c) 2024 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 . + +#pragma once + +#include "CameraControl.generated.h" + +USTRUCT(BlueprintType) +struct CARLA_API FCameraControl +{ + GENERATED_BODY() + + UPROPERTY(Category = "Camera Control", EditAnywhere, BlueprintReadWrite) + float Pan = 0.0f; + + UPROPERTY(Category = "Camera Control", EditAnywhere, BlueprintReadWrite) + float Tilt = 0.0f; + + UPROPERTY(Category = "Camera Control", EditAnywhere, BlueprintReadWrite) + float Zoom = 0.0f; +}; diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/CameraController.Build.cs b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/CameraController.Build.cs new file mode 100644 index 00000000000..c32ec34cb7a --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/CameraController.Build.cs @@ -0,0 +1,56 @@ +// Copyright Epic Games, Inc. All Rights Reserved. + +using System.IO; +using UnrealBuildTool; + +public class CameraController : ModuleRules +{ + public CameraController(ReadOnlyTargetRules Target) : base(Target) + { + bUseRTTI = true; + bEnableExceptions = true; + PrivatePCHHeaderFile = "CameraController.h"; + PCHUsage = ModuleRules.PCHUsageMode.UseExplicitOrSharedPCHs; + + void AddDynamicLibrary(string library) + { + PublicAdditionalLibraries.Add(library); + RuntimeDependencies.Add(library); + PublicDelayLoadDLLs.Add(library); + System.Console.WriteLine("Dynamic Library Added: " + library); + } + + PublicDependencyModuleNames.AddRange( + new string[] + { + "Carla", + "Chaos", + "ChaosVehicles", + "Core", + "CoreUObject", + "Engine", + "Foliage" + } + ); + + if (Target.Type == TargetType.Editor) + { + PublicDependencyModuleNames.AddRange(new string[] { "UnrealEd" }); + } + + string PluginSourcePath = Path.GetFullPath(ModuleDirectory); + string PluginBinariesBuildPath = Path.Combine(PluginSourcePath, "..", "..", "..", "..", "..", "..", ".."); + PublicIncludePaths.Add(Path.Combine(PluginBinariesBuildPath, "Build/Ros2Native/install/include")); + + string CarlaPluginBinariesLinuxPath = Path.Combine(PluginSourcePath, "..", "..", "..", "..", "Carla", "Binaries", "Linux"); + AddDynamicLibrary(Path.Combine(CarlaPluginBinariesLinuxPath, "libcarla-ros2-native.so")); + RuntimeDependencies.Add(Path.Combine(CarlaPluginBinariesLinuxPath, "libfoonathan_memory-0.7.3.so")); + RuntimeDependencies.Add(Path.Combine(CarlaPluginBinariesLinuxPath, "libfastcdr.so")); + RuntimeDependencies.Add(Path.Combine(CarlaPluginBinariesLinuxPath, "libfastcdr.so.1")); + RuntimeDependencies.Add(Path.Combine(CarlaPluginBinariesLinuxPath, "libfastcdr.so.1.1.0")); + RuntimeDependencies.Add(Path.Combine(CarlaPluginBinariesLinuxPath, "libfastrtps.so")); + RuntimeDependencies.Add(Path.Combine(CarlaPluginBinariesLinuxPath, "libfastrtps.so.2.11")); + RuntimeDependencies.Add(Path.Combine(CarlaPluginBinariesLinuxPath, "libfastrtps.so.2.11.2")); + + } +} diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/CameraController.cpp b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/CameraController.cpp new file mode 100644 index 00000000000..b41d5d30251 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/CameraController.cpp @@ -0,0 +1,22 @@ +// Copyright Epic Games, Inc. All Rights Reserved. + +#include "CameraController.h" + +#define LOCTEXT_NAMESPACE "FCameraControllerModule" + +DEFINE_LOG_CATEGORY(LogCameraController) + +void FCameraControllerModule::StartupModule() +{ + // This code will execute after your module is loaded into memory; the exact timing is specified in the .uplugin file per-module +} + +void FCameraControllerModule::ShutdownModule() +{ + // This function may be called during shutdown to clean up your module. For modules that support dynamic reloading, + // we call this function before unloading the module. +} + +#undef LOCTEXT_NAMESPACE + +IMPLEMENT_MODULE(FCameraControllerModule, CameraController) \ No newline at end of file diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/CameraController.h b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/CameraController.h new file mode 100644 index 00000000000..be17ef1df60 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/CameraController.h @@ -0,0 +1,16 @@ +// Copyright Epic Games, Inc. All Rights Reserved. + +#pragma once + +#include "Modules/ModuleManager.h" + +DECLARE_LOG_CATEGORY_EXTERN(LogCameraController, Log, All); + +class FCameraControllerModule : public IModuleInterface +{ +public: + + /** IModuleInterface implementation */ + virtual void StartupModule() override; + virtual void ShutdownModule() override; +}; diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/CameraControllerActor.cpp b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/CameraControllerActor.cpp new file mode 100644 index 00000000000..51136801cc7 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/CameraControllerActor.cpp @@ -0,0 +1,151 @@ +#include "CameraControllerActor.h" +#include "CameraControllerBlueprintLibrary.h" +#include "CameraController/ros2/ROS2CameraControl.h" + +#include "Kismet/KismetStringLibrary.h" +#include "Kismet/KismetMathLibrary.h" + +ACameraControllerActor::ACameraControllerActor(const FObjectInitializer &ObjectInitializer) : Super(ObjectInitializer) +{ +// Add code that is required on startup + PrimaryActorTick.bCanEverTick = true; +} + +void ACameraControllerActor::BeginPlay() +{ + Super::BeginPlay(); +} + +void ACameraControllerActor::Set(const FActorDescription &ActorDescription) +{ + Super::Set(ActorDescription); + UCameraControllerBlueprintLibrary::SetCameraController(ActorDescription, this); +} + +void ACameraControllerActor::SetOwner(AActor* OwningActor) +{ + Super::SetOwner(OwningActor); +} + +// ROS Callback function to control the camera +void ACameraControllerActor::ApplyCameraControl(const FCameraControl& Control) +{ + // Invert yaw (pan) and pitch (tilt) since Unreal uses the left-handed rule with Z-up convention. + // Then convert to degrees. + float Pan = -Control.Pan * 180.0 / PI; + float Tilt = -Control.Tilt * 180.0 / PI; + + if (Pan >= MinPanAngle && Pan <= MaxPanAngle) + { + TargetRotation.Yaw = Pan; + ReachedTargetRotation = false; + UE_LOG(LogCameraController, Log, TEXT("Pan Command '%f' deg has been set!"), Pan); + } + + if (Tilt >= MinTiltAngle && Tilt <= MaxTiltAngle) + { + TargetRotation.Pitch = Tilt; + ReachedTargetRotation = false; + UE_LOG(LogCameraController, Log, TEXT("Tilt Command '%f' deg has been set!"), Tilt); + } + + if (ZoomValues.Contains(Control.Zoom)) + { + TargetFOV = OriginalFOV / Control.Zoom; + ReachedTargetFOV = false; + UE_LOG(LogCameraController, Log, TEXT("Zoom Command '%f' deg has been set!"), Control.Zoom); + } +} + +// Takes a string (ex."1,2,5") containing zoom values and creates an array of floats (ex. [1.0, 2.0, 5.0]) +void ACameraControllerActor::SetZoomValues(const FString& ZoomValuesString) +{ + ZoomValues.Empty(); + + TArray stringParts; + ZoomValuesString.ParseIntoArray(stringParts, TEXT(",")); + + for (const FString& part : stringParts) + { + float value = FCString::Atof(*part); + ZoomValues.Add(value); + } +} + +void ACameraControllerActor::PrePhysTick(float DeltaSeconds){ + + ASceneCaptureSensor* CameraActor = Cast(GetAttachParentActor()); + static int throttle_cntr = 0; + if (CameraActor == nullptr) + { + // Only print the warning the first 10 times so it doesn't spam the terminal + if (throttle_cntr < 10) + { + UE_LOG(LogCameraController, Warning, TEXT("No compatible camera was found for the '%s' actor to control."), *RosName); + throttle_cntr++; + } + return; + } + + float CurrentFOV = CameraActor->GetFOVAngle(); + + if (FirstTick) + { + OriginalFOV = CurrentFOV; + FTransform CameraParentToCameraTransform; + AActor* CameraParentActor = CameraActor->GetAttachParentActor(); + + if (CameraParentActor) + { + UE_LOG(LogCameraController, Log, TEXT("Camera has an attached parent actor. Getting relative transform...")); + CameraParentToCameraTransform = CameraActor->GetActorTransform().GetRelativeTransform(CameraParentActor->GetActorTransform()); + } + else + { + UE_LOG(LogCameraController, Log, TEXT("Camera does not have an attached parent actor. Using its own transform relative to world...")); + CameraParentToCameraTransform = CameraActor->GetActorTransform(); + } + + CameraParentToCameraQuat = CameraParentToCameraTransform.GetRotation(); + + FirstTick = false; + } + + float CurrentZoom = OriginalFOV / CurrentFOV; + + if (!ReachedTargetRotation) + { + if (!CurrentRotation.Equals(TargetRotation, 0.01)) + { + FRotator NewRotation = FMath::RInterpConstantTo(CurrentRotation, TargetRotation, DeltaSeconds, PanTiltSpeed); + FQuat NewQuat = NewRotation.Quaternion(); + CameraActor->SetActorRelativeRotation(CameraParentToCameraQuat * NewQuat); + CurrentRotation = NewRotation; + } + else + { + ReachedTargetRotation = true; + UE_LOG(LogCameraController, Log, TEXT("Reached Target Rotation!")); + } + } + + if (!ReachedTargetFOV) + { + if (CurrentFOV != TargetFOV) + { + float NewFOV = FMath::FInterpConstantTo(CurrentFOV, TargetFOV, DeltaSeconds, ZoomSpeed); + CameraActor->SetFOVAngle(NewFOV); + } + else + { + ReachedTargetFOV = true; + UE_LOG(LogCameraController, Log, TEXT("Reached Target Zoom")); + } + } + + auto ROS2CameraControl = carla::ros2::ROS2CameraControl::GetInstance(); + auto StreamId = carla::streaming::detail::token_type(GetToken()).get_stream_id(); + + FTransform LocalTransformRelativeToParent = GetActorTransform().GetRelativeTransform(CameraActor->GetActorTransform()); + ROS2CameraControl->ProcessDataFromCameraControl(StreamId, LocalTransformRelativeToParent, CurrentRotation.Yaw, CurrentRotation.Pitch, CurrentZoom, this); +} diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/CameraControllerActor.h b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/CameraControllerActor.h new file mode 100644 index 00000000000..565e2474d8e --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/CameraControllerActor.h @@ -0,0 +1,85 @@ +#pragma once + +#include "CoreMinimal.h" +#include "GameFramework/Actor.h" + +#include "Carla/Sensor/Sensor.h" +#include "Carla/Sensor/SceneCaptureSensor.h" +#include "Carla/Actor/ActorDefinition.h" +#include "Carla/Actor/ActorDescription.h" +#include "CameraControl.h" + +#include "CameraControllerActor.generated.h" + +UCLASS() +class CAMERACONTROLLER_API ACameraControllerActor : public ASensor +{ + GENERATED_BODY() + +public: + + ACameraControllerActor(const FObjectInitializer &ObjectInitializer); + + void Set(const FActorDescription &ActorDescription) override; + + void SetOwner(AActor *Owner) override; + + void PrePhysTick(float DeltaSeconds) override; + + void ApplyCameraControl(const FCameraControl& Control); + + void SetZoomValues(const FString& ZoomValuesString); + +protected: + virtual void BeginPlay() override; + + UPROPERTY(BlueprintReadOnly) + bool FirstTick = true; + + UPROPERTY(BlueprintReadOnly) + FQuat CameraParentToCameraQuat; + + UPROPERTY(BlueprintReadOnly) + FRotator CurrentRotation = FRotator::ZeroRotator; + + UPROPERTY(BlueprintReadOnly) + FRotator TargetRotation = FRotator::ZeroRotator; + + UPROPERTY(BlueprintReadOnly) + bool ReachedTargetRotation = true; + + UPROPERTY(BlueprintReadOnly) + float OriginalFOV; + + UPROPERTY(BlueprintReadOnly) + float TargetFOV; + + UPROPERTY(BlueprintReadOnly) + bool ReachedTargetFOV = true; + +public: + + UPROPERTY(BlueprintReadWrite) + FString RosName; + + UPROPERTY(BlueprintReadWrite) + float MinPanAngle = 0; + + UPROPERTY(BlueprintReadWrite) + float MaxPanAngle = 0; + + UPROPERTY(BlueprintReadWrite) + float MinTiltAngle = 0; + + UPROPERTY(BlueprintReadWrite) + float MaxTiltAngle = 0; + + UPROPERTY(BlueprintReadWrite) + float PanTiltSpeed = 0; + + UPROPERTY(BlueprintReadWrite) + float ZoomSpeed = 0; + + UPROPERTY(BlueprintReadWrite) + TArray ZoomValues; +}; diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/CameraControllerBlueprintLibrary.cpp b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/CameraControllerBlueprintLibrary.cpp new file mode 100644 index 00000000000..d5e25308160 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/CameraControllerBlueprintLibrary.cpp @@ -0,0 +1,154 @@ +#pragma once + +#include "CameraControllerBlueprintLibrary.h" +#include "CameraControllerActor.h" +#include "Carla/Actor/ActorBlueprintFunctionLibrary.h" +#include "Carla.h" +#include "Carla/Server/CarlaServer.h" +#include "Carla/Game/CarlaGameInstance.h" +#include "Carla/Game/CarlaStatics.h" +#include "Carla/Sensor/Sensor.h" + +#if WITH_EDITOR +# define CARLA_ABFL_CHECK_ACTOR(ActorPtr) \ + if (!IsValid(ActorPtr)) \ + { \ + UE_LOG(LogCameraController, Error, TEXT("Cannot set empty actor!")); \ + return; \ + } +#else +# define CARLA_ABFL_CHECK_ACTOR(ActorPtr) \ + IsValid(ActorPtr); +#endif // WITH_EDITOR + +FActorDefinition UCameraControllerBlueprintLibrary::MakeCameraControllerDefinition(TSubclassOf Class) +{ + FActorDefinition Definition; + bool Success; + MakeCameraControllerDefinition(Success, Definition, Class); + check(Success); + return Definition; +} + +template +static FString JoinStrings(const FString &Separator, ARGS && ... Args) +{ + return FString::Join(TArray{std::forward(Args) ...}, *Separator); +} + +template +static void FillIdAndTags(FActorDefinition &Def, TStrs && ... Strings) +{ + Def.Id = JoinStrings(TEXT("."), std::forward(Strings) ...).ToLower(); + Def.Tags = JoinStrings(TEXT(","), std::forward(Strings) ...).ToLower(); + + // each actor gets an actor role name attribute (empty by default) + FActorVariation ActorRole; + ActorRole.Id = TEXT("role_name"); + ActorRole.Type = EActorAttributeType::String; + ActorRole.RecommendedValues = { TEXT("default") }; + ActorRole.bRestrictToRecommended = false; + Def.Variations.Emplace(ActorRole); + + // ROS2 + FActorVariation Var; + Var.Id = TEXT("ros_name"); + Var.Type = EActorAttributeType::String; + Var.RecommendedValues = { Def.Id }; + Var.bRestrictToRecommended = false; + Def.Variations.Emplace(Var); +} + +void UCameraControllerBlueprintLibrary::MakeCameraControllerDefinition( + bool &Success, + FActorDefinition &Definition, + TSubclassOf Class) +{ + FillIdAndTags(Definition, TEXT("sensor"), TEXT("other"), TEXT("camera_controller")); + + Definition.Class = Class; + + FActorVariation Tick; + Tick.Id = TEXT("sensor_tick"); + Tick.Type = EActorAttributeType::Float; + Tick.RecommendedValues = { TEXT("0.05") }; + Tick.bRestrictToRecommended = false; + + FActorVariation MinPanAngle; + MinPanAngle.Id = TEXT("min_pan_angle"); + MinPanAngle.Type = EActorAttributeType::Float; + MinPanAngle.RecommendedValues = { TEXT("-60") }; + MinPanAngle.bRestrictToRecommended = false; + + FActorVariation MaxPanAngle; + MaxPanAngle.Id = TEXT("max_pan_angle"); + MaxPanAngle.Type = EActorAttributeType::Float; + MaxPanAngle.RecommendedValues = { TEXT("60") }; + MaxPanAngle.bRestrictToRecommended = false; + + FActorVariation MinTiltAngle; + MinTiltAngle.Id = TEXT("min_tilt_angle"); + MinTiltAngle.Type = EActorAttributeType::Float; + MinTiltAngle.RecommendedValues = { TEXT("-60") }; + MinTiltAngle.bRestrictToRecommended = false; + + FActorVariation MaxTiltAngle; + MaxTiltAngle.Id = TEXT("max_tilt_angle"); + MaxTiltAngle.Type = EActorAttributeType::Float; + MaxTiltAngle.RecommendedValues = { TEXT("60") }; + MaxTiltAngle.bRestrictToRecommended = false; + + FActorVariation PanTiltSpeed; + PanTiltSpeed.Id = TEXT("pan_tilt_speed"); // in deg/s + PanTiltSpeed.Type = EActorAttributeType::Float; + PanTiltSpeed.RecommendedValues = { TEXT("180.0") }; + PanTiltSpeed.bRestrictToRecommended = false; + + FActorVariation ZoomSpeed; + ZoomSpeed.Id = TEXT("zoom_speed"); // in fov deg/sec + ZoomSpeed.Type = EActorAttributeType::Float; + ZoomSpeed.RecommendedValues = { TEXT("80.0") }; + ZoomSpeed.bRestrictToRecommended = false; + + FActorVariation ZoomValues; + ZoomValues.Id = TEXT("zoom_values"); + ZoomValues.Type = EActorAttributeType::String; + ZoomValues.RecommendedValues = { TEXT("1,2,5") }; + ZoomValues.bRestrictToRecommended = false; + + Definition.Variations.Append({ + Tick, + MinPanAngle, + MaxPanAngle, + MinTiltAngle, + MaxTiltAngle, + PanTiltSpeed, + ZoomSpeed, + ZoomValues + }); + + Definition.Attributes.Emplace(FActorAttribute{ + TEXT("sensor_type"), + EActorAttributeType::String, + "camera_control"}); + + Success = UActorBlueprintFunctionLibrary::CheckActorDefinition(Definition); +} + +void UCameraControllerBlueprintLibrary::SetCameraController( + const FActorDescription &Description, + ACameraControllerActor *CameraController) +{ + CARLA_ABFL_CHECK_ACTOR(CameraController); + + CameraController->RosName = UActorBlueprintFunctionLibrary::RetrieveActorAttributeToString("ros_name", Description.Variations, ""); + CameraController->MinPanAngle = UActorBlueprintFunctionLibrary::RetrieveActorAttributeToFloat("min_pan_angle", Description.Variations, -60.0f); + CameraController->MaxPanAngle = UActorBlueprintFunctionLibrary::RetrieveActorAttributeToFloat("max_pan_angle", Description.Variations, 60.0f); + CameraController->MinTiltAngle = UActorBlueprintFunctionLibrary::RetrieveActorAttributeToFloat("min_tilt_angle", Description.Variations, -60.0f); + CameraController->MaxTiltAngle = UActorBlueprintFunctionLibrary::RetrieveActorAttributeToFloat("max_tilt_angle", Description.Variations, 60.0f); + CameraController->PanTiltSpeed = UActorBlueprintFunctionLibrary::RetrieveActorAttributeToFloat("pan_tilt_speed", Description.Variations, 180.0f); + CameraController->ZoomSpeed = UActorBlueprintFunctionLibrary::RetrieveActorAttributeToFloat("zoom_speed", Description.Variations, 80.0f); + + FString ZoomValuesString = UActorBlueprintFunctionLibrary::RetrieveActorAttributeToString("zoom_values", Description.Variations, ""); + CameraController->SetZoomValues(ZoomValuesString); +} \ No newline at end of file diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/CameraControllerBlueprintLibrary.h b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/CameraControllerBlueprintLibrary.h new file mode 100644 index 00000000000..bc05777eda3 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/CameraControllerBlueprintLibrary.h @@ -0,0 +1,24 @@ +#pragma once + +#include +#include "Kismet/BlueprintFunctionLibrary.h" +#include + +#include "CameraControllerBlueprintLibrary.generated.h" + +class ACameraControllerActor; + +UCLASS() +class CAMERACONTROLLER_API UCameraControllerBlueprintLibrary : public UBlueprintFunctionLibrary +{ + GENERATED_BODY() + + public: + UFUNCTION(BlueprintCallable) + static FActorDefinition MakeCameraControllerDefinition(TSubclassOf Class); + + static void MakeCameraControllerDefinition(bool &Success, FActorDefinition &Definition, TSubclassOf Class); + + UFUNCTION(BlueprintCallable) + static void SetCameraController(const FActorDescription &Description, ACameraControllerActor *CameraController); +}; \ No newline at end of file diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/CameraControllerGameSubsystem.cpp b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/CameraControllerGameSubsystem.cpp new file mode 100644 index 00000000000..92bb411dbfe --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/CameraControllerGameSubsystem.cpp @@ -0,0 +1,15 @@ + +#include "CameraControllerGameSubsystem.h" +#include "CarlaInterface/CarlaActor.h" +#include "ros2/ROS2CameraControl.h" + +void UCameraControllerSubsystem::Initialize(FSubsystemCollectionBase& Collection) +{ + FCameraControllerActor::RegisterClassWithFactory(); + carla::ros2::ROS2CameraControl::GetInstance(); +} + +void UCameraControllerSubsystem::Deinitialize() +{ + +} diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/CameraControllerGameSubsystem.h b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/CameraControllerGameSubsystem.h new file mode 100644 index 00000000000..f5ee1432fb7 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/CameraControllerGameSubsystem.h @@ -0,0 +1,19 @@ +#pragma once + +#include "Subsystems/GameInstanceSubsystem.h" + +#include "CameraControllerGameSubsystem.generated.h" + +UCLASS() +class UCameraControllerSubsystem : public UGameInstanceSubsystem +{ + GENERATED_BODY() +public: + // Begin USubsystem + virtual void Initialize(FSubsystemCollectionBase& Collection) override; + virtual void Deinitialize() override; + // End USubsystem + +private: + // All my variables +}; \ No newline at end of file diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/CarlaInterface/CameraControllerActorData.cpp b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/CarlaInterface/CameraControllerActorData.cpp new file mode 100644 index 00000000000..c2a0be81585 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/CarlaInterface/CameraControllerActorData.cpp @@ -0,0 +1,17 @@ +// Copyright (c) 2024 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 . + +#include "CameraControllerActorData.h" + +void FCameraControllerData::RecordActorData(FCarlaActor* CarlaActor, UCarlaEpisode* CarlaEpisode) +{ + FActorData::RecordActorData(CarlaActor, CarlaEpisode); +} + +void FCameraControllerData::RestoreActorData(FCarlaActor* CarlaActor, UCarlaEpisode* CarlaEpisode) +{ + FActorData::RestoreActorData(CarlaActor, CarlaEpisode); +} diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/CarlaInterface/CameraControllerActorData.h b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/CarlaInterface/CameraControllerActorData.h new file mode 100644 index 00000000000..0df756493b2 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/CarlaInterface/CameraControllerActorData.h @@ -0,0 +1,18 @@ +// Copyright (c) 2024 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 . + +#pragma once + +#include "Carla/Actor/ActorData.h" + +class FCameraControllerData : public FActorData +{ +public: + + virtual void RecordActorData(FCarlaActor* CarlaActor, UCarlaEpisode* CarlaEpisode) override; + + virtual void RestoreActorData(FCarlaActor* CarlaActor, UCarlaEpisode* CarlaEpisode) override; +}; diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/CarlaInterface/CarlaActor.cpp b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/CarlaInterface/CarlaActor.cpp new file mode 100644 index 00000000000..7b01f28abbf --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/CarlaInterface/CarlaActor.cpp @@ -0,0 +1,37 @@ +// Copyright (c) 2024 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 . + +#include "CarlaActor.h" +#include "CameraControllerActorData.h" + +const FString FCameraControllerActor::CustomType = "CameraController_CameraController"; + +FCameraControllerActor::FCameraControllerActor( + IdType ActorId, + AActor* Actor, + TSharedPtr Info, + carla::rpc::ActorState InState, + UWorld* World) + : FCarlaActor(ActorId, Actor, Info, InState, World) +{ + Type = ActorType::Custom; + ActorData = MakeShared(); +} + +TSharedPtr FCameraControllerActor::CreateInstance( + FCarlaActor::IdType ActorId, + AActor* Actor, + TSharedPtr Info, + carla::rpc::ActorState InState, + UWorld* World) + { + return MakeShared(ActorId, Actor, std::move(Info), InState, World); + } + +void FCameraControllerActor::RegisterClassWithFactory() + { + CarlaActorConstructorFactory::Instance().Register(TEXT("FCameraControllerActor"), ACameraControllerActor::StaticClass(), CreateInstance); + } diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/CarlaInterface/CarlaActor.h b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/CarlaInterface/CarlaActor.h new file mode 100644 index 00000000000..14a5444632e --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/CarlaInterface/CarlaActor.h @@ -0,0 +1,33 @@ +// Copyright (c) 2024 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 . + +#pragma once + +#include "Carla/Actor/CarlaActor.h" +#include "Carla/Actor/CarlaActorConstructorFactory.h" + +class FCameraControllerActor : public FCarlaActor +{ +public: + FCameraControllerActor( + IdType ActorId, + AActor* Actor, + TSharedPtr Info, + carla::rpc::ActorState InState, + UWorld* World); + + static TSharedPtr CreateInstance(FCarlaActor::IdType ActorId, + AActor* Actor, + TSharedPtr Info, + carla::rpc::ActorState InState, + UWorld* World); + + static void RegisterClassWithFactory(); + + protected: + static const FString CustomType; + +}; diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/ros2/CameraControlROS2Handler.cpp b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/ros2/CameraControlROS2Handler.cpp new file mode 100644 index 00000000000..5b6ee1f58b9 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/ros2/CameraControlROS2Handler.cpp @@ -0,0 +1,24 @@ +// Copyright (c) 2024 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 . + +#include "CameraControlROS2Handler.h" +#include "CameraController/CameraControl.h" + +void CameraControlROS2Handler::operator()(carla::ros2::CameraControl &Source) +{ + if (!_Actor) return; + + ACameraControllerActor *CameraControl = Cast(_Actor); + if (!CameraControl) return; + + // setup control values + FCameraControl NewControl; + NewControl.Pan = Source.pan; + NewControl.Tilt = Source.tilt; + NewControl.Zoom = Source.zoom; + + CameraControl->ApplyCameraControl(NewControl); +} diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/ros2/CameraControlROS2Handler.h b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/ros2/CameraControlROS2Handler.h new file mode 100644 index 00000000000..3dd7d6a11ad --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/ros2/CameraControlROS2Handler.h @@ -0,0 +1,25 @@ +// Copyright (c) 2024 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 . + +#pragma once + +#include +#include "carla/ros2/ROS2.h" +#include + +/// visitor class +class CameraControlROS2Handler +{ + public: + CameraControlROS2Handler() = delete; + CameraControlROS2Handler(AActor *Actor, std::string RosName) : _Actor(Actor), _RosName(RosName) {}; + + void operator()(carla::ros2::CameraControl &Source); + + private: + AActor *_Actor {nullptr}; + std::string _RosName; +}; diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/ros2/ROS2CameraControl.cpp b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/ros2/ROS2CameraControl.cpp new file mode 100644 index 00000000000..a41f8f7ee3e --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/ros2/ROS2CameraControl.cpp @@ -0,0 +1,176 @@ +// Copyright (c) 2024 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 . + +#include "ROS2CameraControl.h" +#include "carla/Logging.h" + +#include "carla/ros2/plugin-utils/enable-fastdds-include.h" // start fastdds includes +#include "carla/ros2/publishers/CarlaTransformPublisher.h" +#include "carla/ros2/subscribers/CarlaSubscriber.h" +#include "carla/ros2/plugin-utils/disable-fastdds-include.h" // end fastdds includes + +#include + +namespace carla { +namespace ros2 { + +// static fields +std::shared_ptr ROS2CameraControl::_instance; + +void ROS2CameraControl::Enable(bool enable) { + _enabled = enable; + log_info("ROS2CameraControl enabled: ", _enabled); +} + +void ROS2CameraControl::SetFrame(uint64_t frame) { + _frame = frame; + //log_info("ROS2CameraControl new frame: ", _frame); + for (auto controllerPair : _controllers) { + std::shared_ptr controller = controllerPair.second; + void* actor = controller->GetCameraControl(); + if (controller->IsAlive()) { + if (controller->HasNewMessage()) { + auto it = _actor_callbacks.find(actor); + if (it != _actor_callbacks.end()) { + CameraControl control = controller->GetMessage(); + it->second(actor, control); + } + } + } else { + RemoveActorCallback(actor); + } + } +} + +void ROS2CameraControl::SetTimestamp(double timestamp) { + if (IsEnabled()){ + double integral; + const double fractional = modf(timestamp, &integral); + const double multiplier = 1000000000.0; + _seconds = static_cast(integral); + _nanoseconds = static_cast(fractional * multiplier); + } + +} + +void ROS2CameraControl::Shutdown() { + for (auto& element : _publishers) { + element.second.reset(); + } + for (auto& element : _transforms) { + element.second.reset(); + } + _controllers.clear(); + _enabled = false; +} + +void ROS2CameraControl::RegisterActor(FActorDescription& Description, std::string RosName, void *Actor) +{ + for (auto &&Attr : Description.Variations) + { + if (Attr.Key == "sensor_type" && (Attr.Value.Value == "camera_control")) + { + AddActorCallback(Actor, RosName, [RosName](void *Actor, carla::ros2::ROS2CameraControlCallbackData Data) -> void + { + AActor *UEActor = static_cast(Actor); + CameraControlROS2Handler Handler(UEActor, RosName); + std::visit(Handler, Data); + }); + } + } +} + +void ROS2CameraControl::RemoveActor(void* Actor) +{ + RemoveActorCallback(Actor); +} + +void ROS2CameraControl::RemoveActorRosPublishers(void *actor) +{ + auto p_it = _publishers.find(actor); + if (p_it != _publishers.end()) { + _publishers.erase(actor); + } + + auto t_it = _transforms.find(actor); + if (t_it != _transforms.end()) { + _transforms.erase(actor); + } +} + +void ROS2CameraControl::AddActorCallback(void* actor, std::string ros_name, CameraControlActorCallback callback) { + _actor_callbacks.insert({actor, std::move(callback)}); + auto ROS2Interfaces = UActorDispatcher::GetInterfaces(); + std::string parent_ros_name = ROS2Interfaces->GetActorParentRosName(actor); + auto controller = std::make_shared(actor, ros_name.c_str(), parent_ros_name.c_str()); + _controllers.insert({actor, controller}); + controller->Init(); +} + +void ROS2CameraControl::RemoveActorCallback(void* actor) { + auto it = _actor_callbacks.find(actor); + if (it != _actor_callbacks.end()) { + _controllers.erase(actor); + _actor_callbacks.erase(actor); + } +} + +std::pair, std::shared_ptr> ROS2CameraControl::GetOrCreateSensor(carla::streaming::detail::stream_id_type id, void* actor) { + auto it_publishers = _publishers.find(actor); + auto it_transforms = _transforms.find(actor); + std::shared_ptr publisher {}; + std::shared_ptr transform {}; + auto ROS2Interfaces = UActorDispatcher::GetInterfaces(); + if (it_publishers != _publishers.end()) { + publisher = it_publishers->second; + if (it_transforms != _transforms.end()) { + transform = it_transforms->second; + } + } else { + //Sensor not found, creating one of the given type + const std::string string_id = std::to_string(id); + std::string ros_name = ROS2Interfaces->GetActorRosName(actor); + std::string parent_ros_name = ROS2Interfaces->GetActorParentRosName(actor); + std::string parent_ros_name_standalone = ROS2Interfaces->GetActorParentRosName(actor, false); + if (ros_name == "camera_control__") { + ros_name.pop_back(); + ros_name.pop_back(); + ros_name += string_id; + ROS2Interfaces->UpdateActorRosName(actor, ros_name); + } + std::shared_ptr new_publisher = std::make_shared(ros_name.c_str(), parent_ros_name.c_str()); + if (new_publisher->Init()) { + _publishers.insert({actor, new_publisher}); + publisher = new_publisher; + } + std::shared_ptr new_transform = std::make_shared(ros_name.c_str(), parent_ros_name.c_str(), parent_ros_name_standalone.c_str()); + if (new_transform->Init()) { + _transforms.insert({actor, new_transform}); + transform = new_transform; + } + } + return { publisher, transform }; +} + +void ROS2CameraControl::ProcessDataFromCameraControl( + carla::streaming::detail::stream_id_type stream_id, + const carla::geom::Transform sensor_transform, + float pan, float tilt, float zoom, + void *actor) { + log_info("Sensor CameraControl to ROS data: frame.", _frame, "stream.", stream_id, "camera_control.", pan, tilt, zoom); + auto sensors = GetOrCreateSensor(stream_id, actor); + if (sensors.first) { + sensors.first->SetData(_seconds, _nanoseconds, pan, tilt, zoom); + sensors.first->Publish(); + } + if (sensors.second) { + sensors.second->SetData(_seconds, _nanoseconds, (const float*)&sensor_transform.location, (const float*)&sensor_transform.rotation); + sensors.second->Publish(); + } +} + +} // namespace ros2 +} // namespace carla diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/ros2/ROS2CameraControl.h b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/ros2/ROS2CameraControl.h new file mode 100644 index 00000000000..3b0100f123b --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/ros2/ROS2CameraControl.h @@ -0,0 +1,88 @@ +// Copyright (c) 2024 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 . + +#pragma once + +#include "Carla/Actor/ActorDispatcher.h" +#include "carla/ros2/ROS2Interfaces.h" +#include "carla/geom/Transform.h" +#include "carla/ros2/ROS2.h" +#include "carla/streaming/detail/Types.h" +#include "ROS2CameraControlCallbackData.h" + +#include "CameraController/ros2/publishers/CarlaCameraControlPublisher.h" +#include "CameraController/ros2/subscribers/CarlaCameraControlSubscriber.h" + +#include +#include +#include +#include + +namespace carla { +namespace ros2 { + + class CarlaTransformPublisher; + class CarlaCameraControlSubscriber; + +class ROS2CameraControl : public ROS2 +{ + public: + + // deleting copy constructor for singleton + ROS2CameraControl(const ROS2CameraControl& obj) = delete; + static std::shared_ptr GetInstance() { + if (!_instance) + { + _instance = std::shared_ptr(new ROS2CameraControl); + auto ROS2Interfaces = UActorDispatcher::GetInterfaces(); + ROS2Interfaces->RegisterInterface(_instance); + } + return _instance; + } + + virtual void Enable(bool enable) override; + virtual void Shutdown() override; + virtual bool IsEnabled() override { return _enabled; } + virtual void SetFrame(uint64_t frame) override; + virtual void SetTimestamp(double timestamp) override; + virtual void RegisterActor(FActorDescription& Description, std::string RosName, void* Actor) override; + virtual void RemoveActor(void* Actor) override; + // ros_name managing + virtual void RemoveActorRosPublishers(void *actor) override; + + // callbacks + void AddActorCallback(void* actor, std::string ros_name, CameraControlActorCallback callback); + void RemoveActorCallback(void* actor); + + // receiving data to publish + void ProcessDataFromCameraControl( + carla::streaming::detail::stream_id_type stream_id, + const carla::geom::Transform sensor_transform, + float pan, float tilt, float zoom, + void *actor = nullptr); + + // enabling streams to publish + virtual void EnableStream(carla::streaming::detail::stream_id_type id) { _publish_stream.insert(id); } + virtual bool IsStreamEnabled(carla::streaming::detail::stream_id_type id) { return _publish_stream.count(id) > 0; } + virtual void ResetStreams() { _publish_stream.clear(); } + + static std::shared_ptr _instance; + + private: + std::pair, std::shared_ptr> GetOrCreateSensor(carla::streaming::detail::stream_id_type id, void* actor); + + // singleton + ROS2CameraControl() {}; + + std::unordered_map> _controllers; + std::unordered_map> _publishers; + std::unordered_map> _transforms; + std::unordered_set _publish_stream; + std::unordered_map _actor_callbacks; +}; + +} // namespace ros2 +} // namespace carla diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/ros2/ROS2CameraControlCallbackData.h b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/ros2/ROS2CameraControlCallbackData.h new file mode 100644 index 00000000000..ee567961742 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/ros2/ROS2CameraControlCallbackData.h @@ -0,0 +1,27 @@ +// Copyright (c) 2024 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 . + +#pragma once + +#include +#include + +namespace carla { +namespace ros2 { + + struct CameraControl + { + float pan; + float tilt; + float zoom; + }; + + using ROS2CameraControlCallbackData = std::variant; + + using CameraControlActorCallback = std::function; + +} // namespace ros2 +} // namespace carla diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/ros2/listeners/CarlaCameraControlSubscriberListener.cpp b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/ros2/listeners/CarlaCameraControlSubscriberListener.cpp new file mode 100644 index 00000000000..974f524813d --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/ros2/listeners/CarlaCameraControlSubscriberListener.cpp @@ -0,0 +1,113 @@ +#include "CarlaCameraControlSubscriberListener.h" +#include + +#include "carla/ros2/plugin-utils/enable-fastdds-include.h" // start fastdds includes +#include +#include +#include +#include +#include "carla/ros2/plugin-utils/disable-fastdds-include.h" // end fastdds includes + +#include "CameraController/ros2/types/CarlaCameraControl.h" +#include "CameraController/ros2/subscribers/CarlaCameraControlSubscriber.h" +#include "CameraController/ros2/ROS2CameraControlCallbackData.h" + +namespace carla { +namespace ros2 { + + namespace efd = eprosima::fastdds::dds; + using erc = eprosima::fastrtps::types::ReturnCode_t; + + class CarlaCameraControlSubscriberListenerImpl : public efd::DataReaderListener { + public: + void on_subscription_matched( + efd::DataReader* reader, + const efd::SubscriptionMatchedStatus& info) override; + void on_data_available(efd::DataReader* reader) override; + + int _matched {0}; + bool _first_connected {false}; + CarlaCameraControlSubscriber* _owner {nullptr}; + carla_interfaces::msg::CarlaCameraControl _message {}; + }; + + void CarlaCameraControlSubscriberListenerImpl::on_subscription_matched(efd::DataReader* reader, const efd::SubscriptionMatchedStatus& info) + { + if (info.current_count_change == 1) { + _matched = info.total_count; + _first_connected = true; + } else if (info.current_count_change == -1) { + _matched = info.total_count; + if (_matched == 0) { + _owner->DestroySubscriber(); + } + } else { + std::cerr << info.current_count_change + << " is not a valid value for PublicationMatchedStatus current count change" << std::endl; + } + } + + void CarlaCameraControlSubscriberListenerImpl::on_data_available(efd::DataReader* reader) + { + efd::SampleInfo info; + eprosima::fastrtps::types::ReturnCode_t rcode = reader->take_next_sample(&_message, &info); + if (rcode == erc::ReturnCodeValue::RETCODE_OK) { + CameraControl control; + control.pan = _message.pan(); + control.tilt = _message.tilt(); + control.zoom = _message.zoom(); + _owner->ForwardMessage(control); + } + if (rcode == erc::ReturnCodeValue::RETCODE_ERROR) { + std::cerr << "RETCODE_ERROR" << std::endl; + } + if (rcode == erc::ReturnCodeValue::RETCODE_UNSUPPORTED) { + std::cerr << "RETCODE_UNSUPPORTED" << std::endl; + } + if (rcode == erc::ReturnCodeValue::RETCODE_BAD_PARAMETER) { + std::cerr << "RETCODE_BAD_PARAMETER" << std::endl; + } + if (rcode == erc::ReturnCodeValue::RETCODE_PRECONDITION_NOT_MET) { + std::cerr << "RETCODE_PRECONDITION_NOT_MET" << std::endl; + } + if (rcode == erc::ReturnCodeValue::RETCODE_OUT_OF_RESOURCES) { + std::cerr << "RETCODE_OUT_OF_RESOURCES" << std::endl; + } + if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ENABLED) { + std::cerr << "RETCODE_NOT_ENABLED" << std::endl; + } + if (rcode == erc::ReturnCodeValue::RETCODE_IMMUTABLE_POLICY) { + std::cerr << "RETCODE_IMMUTABLE_POLICY" << std::endl; + } + if (rcode == erc::ReturnCodeValue::RETCODE_INCONSISTENT_POLICY) { + std::cerr << "RETCODE_INCONSISTENT_POLICY" << std::endl; + } + if (rcode == erc::ReturnCodeValue::RETCODE_ALREADY_DELETED) { + std::cerr << "RETCODE_ALREADY_DELETED" << std::endl; + } + if (rcode == erc::ReturnCodeValue::RETCODE_TIMEOUT) { + std::cerr << "RETCODE_TIMEOUT" << std::endl; + } + if (rcode == erc::ReturnCodeValue::RETCODE_NO_DATA) { + std::cerr << "RETCODE_NO_DATA" << std::endl; + } + if (rcode == erc::ReturnCodeValue::RETCODE_ILLEGAL_OPERATION) { + std::cerr << "RETCODE_ILLEGAL_OPERATION" << std::endl; + } + if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ALLOWED_BY_SECURITY) { + std::cerr << "RETCODE_NOT_ALLOWED_BY_SECURITY" << std::endl; + } + } + + void CarlaCameraControlSubscriberListener::SetOwner(CarlaCameraControlSubscriber* owner) { + _impl->_owner = owner; + } + + CarlaCameraControlSubscriberListener::CarlaCameraControlSubscriberListener(CarlaCameraControlSubscriber* owner) : + _impl(std::make_unique()) { + _impl->_owner = owner; + } + + CarlaCameraControlSubscriberListener::~CarlaCameraControlSubscriberListener() {} + +}} diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/ros2/listeners/CarlaCameraControlSubscriberListener.h b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/ros2/listeners/CarlaCameraControlSubscriberListener.h new file mode 100644 index 00000000000..638c8ccc655 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/ros2/listeners/CarlaCameraControlSubscriberListener.h @@ -0,0 +1,30 @@ +// Copyright (c) 2024 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 . + +#pragma once +#define _GLIBCXX_USE_CXX11_ABI 0 + +#include + +namespace carla { +namespace ros2 { + + class CarlaCameraControlSubscriberListenerImpl; + class CarlaCameraControlSubscriber; + + class CarlaCameraControlSubscriberListener { + public: + CarlaCameraControlSubscriberListener(CarlaCameraControlSubscriber* owner); + ~CarlaCameraControlSubscriberListener(); + CarlaCameraControlSubscriberListener(const CarlaCameraControlSubscriberListener&) = delete; + CarlaCameraControlSubscriberListener& operator=(const CarlaCameraControlSubscriberListener&) = delete; + CarlaCameraControlSubscriberListener(CarlaCameraControlSubscriberListener&&) = delete; + CarlaCameraControlSubscriberListener& operator=(CarlaCameraControlSubscriberListener&&) = delete; + + void SetOwner(CarlaCameraControlSubscriber* owner); + + std::unique_ptr _impl; + }; +} +} diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/ros2/publishers/CarlaCameraControlPublisher.cpp b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/ros2/publishers/CarlaCameraControlPublisher.cpp new file mode 100644 index 00000000000..79bbc274bf9 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/ros2/publishers/CarlaCameraControlPublisher.cpp @@ -0,0 +1,223 @@ +#include "CarlaCameraControlPublisher.h" + +#include + +#include "CameraController/ros2/types/CarlaCameraControlPubSubTypes.h" +#include "carla/ros2/listeners/CarlaListener.h" + +#include "carla/ros2/plugin-utils/enable-fastdds-include.h" // start fastdds includes +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include "carla/ros2/plugin-utils/disable-fastdds-include.h" // end fastdds includes + + +namespace carla { +namespace ros2 { + namespace efd = eprosima::fastdds::dds; + using erc = eprosima::fastrtps::types::ReturnCode_t; + + struct CarlaCameraControlPublisherImpl { + efd::DomainParticipant* _participant { nullptr }; + efd::Publisher* _publisher { nullptr }; + efd::Topic* _topic { nullptr }; + efd::DataWriter* _datawriter { nullptr }; + efd::TypeSupport _type { new carla_interfaces::msg::CarlaCameraControlPubSubType() }; + CarlaListener _listener {}; + carla_interfaces::msg::CarlaCameraControl _camera_control {}; + }; + + bool CarlaCameraControlPublisher::Init() { + if (_impl->_type == nullptr) { + std::cerr << "Invalid TypeSupport" << std::endl; + return false; + } + + efd::DomainParticipantQos pqos = efd::PARTICIPANT_QOS_DEFAULT; + pqos.name(_name); + auto factory = efd::DomainParticipantFactory::get_instance(); + _impl->_participant = factory->create_participant(0, pqos); + if (_impl->_participant == nullptr) { + std::cerr << "Failed to create DomainParticipant" << std::endl; + return false; + } + _impl->_type.register_type(_impl->_participant); + + efd::PublisherQos pubqos = efd::PUBLISHER_QOS_DEFAULT; + _impl->_publisher = _impl->_participant->create_publisher(pubqos, nullptr); + if (_impl->_publisher == nullptr) { + std::cerr << "Failed to create Publisher" << std::endl; + return false; + } + + efd::TopicQos tqos = efd::TOPIC_QOS_DEFAULT; + const std::string base { "rt/carla/" }; + std::string topic_name = base; + if (!_parent.empty()) + topic_name += _parent + "/"; + topic_name += _name + "/status"; + _impl->_topic = _impl->_participant->create_topic(topic_name, _impl->_type->getName(), tqos); + if (_impl->_topic == nullptr) { + std::cerr << "Failed to create Topic" << std::endl; + return false; + } + + efd::DataWriterQos wqos = efd::DATAWRITER_QOS_DEFAULT; + wqos.endpoint().history_memory_policy = eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + efd::DataWriterListener* listener = (efd::DataWriterListener*)_impl->_listener._impl.get(); + _impl->_datawriter = _impl->_publisher->create_datawriter(_impl->_topic, wqos, listener); + if (_impl->_datawriter == nullptr) { + std::cerr << "Failed to create DataWriter" << std::endl; + return false; + } + _frame_id = _name; + return true; + } + + bool CarlaCameraControlPublisher::Publish() { + eprosima::fastrtps::rtps::InstanceHandle_t instance_handle; + eprosima::fastrtps::types::ReturnCode_t rcode = _impl->_datawriter->write(&_impl->_camera_control, instance_handle); + if (rcode == erc::ReturnCodeValue::RETCODE_OK) { + return true; + } + if (rcode == erc::ReturnCodeValue::RETCODE_ERROR) { + std::cerr << "RETCODE_ERROR" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_UNSUPPORTED) { + std::cerr << "RETCODE_UNSUPPORTED" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_BAD_PARAMETER) { + std::cerr << "RETCODE_BAD_PARAMETER" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_PRECONDITION_NOT_MET) { + std::cerr << "RETCODE_PRECONDITION_NOT_MET" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_OUT_OF_RESOURCES) { + std::cerr << "RETCODE_OUT_OF_RESOURCES" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ENABLED) { + std::cerr << "RETCODE_NOT_ENABLED" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_IMMUTABLE_POLICY) { + std::cerr << "RETCODE_IMMUTABLE_POLICY" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_INCONSISTENT_POLICY) { + std::cerr << "RETCODE_INCONSISTENT_POLICY" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_ALREADY_DELETED) { + std::cerr << "RETCODE_ALREADY_DELETED" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_TIMEOUT) { + std::cerr << "RETCODE_TIMEOUT" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_NO_DATA) { + std::cerr << "RETCODE_NO_DATA" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_ILLEGAL_OPERATION) { + std::cerr << "RETCODE_ILLEGAL_OPERATION" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ALLOWED_BY_SECURITY) { + std::cerr << "RETCODE_NOT_ALLOWED_BY_SECURITY" << std::endl; + return false; + } + std::cerr << "UNKNOWN" << std::endl; + return false; + } + + void CarlaCameraControlPublisher::SetData(int32_t seconds, uint32_t nanoseconds, float pan, float tilt, float zoom) { + builtin_interfaces::msg::Time time; + time.sec(seconds); + time.nanosec(nanoseconds); + + std_msgs::msg::Header header; + header.stamp(std::move(time)); + header.frame_id(_frame_id); + + _impl->_camera_control.header(std::move(header)); + // Invert yaw (pan) and pitch (tilt) since ROS uses the right handed rule with Z-up convention. + // Then convert to radians. + _impl->_camera_control.pan(-pan * M_PIf32 / 180.0f); + _impl->_camera_control.tilt(-tilt * M_PIf32 / 180.0f); + _impl->_camera_control.zoom(zoom); + } + + CarlaCameraControlPublisher::CarlaCameraControlPublisher(const char* ros_name, const char* parent) : + _impl(std::make_shared()) { + _name = ros_name; + _parent = parent; + } + + CarlaCameraControlPublisher::~CarlaCameraControlPublisher() { + if (!_impl) + return; + + if (_impl->_datawriter) + _impl->_publisher->delete_datawriter(_impl->_datawriter); + + if (_impl->_publisher) + _impl->_participant->delete_publisher(_impl->_publisher); + + if (_impl->_topic) + _impl->_participant->delete_topic(_impl->_topic); + + if (_impl->_participant) + efd::DomainParticipantFactory::get_instance()->delete_participant(_impl->_participant); + } + + CarlaCameraControlPublisher::CarlaCameraControlPublisher(const CarlaCameraControlPublisher& other) { + _frame_id = other._frame_id; + _name = other._name; + _parent = other._parent; + _impl = other._impl; + } + + CarlaCameraControlPublisher& CarlaCameraControlPublisher::operator=(const CarlaCameraControlPublisher& other) { + _frame_id = other._frame_id; + _name = other._name; + _parent = other._parent; + _impl = other._impl; + + return *this; + } + + CarlaCameraControlPublisher::CarlaCameraControlPublisher(CarlaCameraControlPublisher&& other) { + _frame_id = std::move(other._frame_id); + _name = std::move(other._name); + _parent = std::move(other._parent); + _impl = std::move(other._impl); + } + + CarlaCameraControlPublisher& CarlaCameraControlPublisher::operator=(CarlaCameraControlPublisher&& 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; + } +} +} diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/ros2/publishers/CarlaCameraControlPublisher.h b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/ros2/publishers/CarlaCameraControlPublisher.h new file mode 100644 index 00000000000..cc4a22d4350 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/ros2/publishers/CarlaCameraControlPublisher.h @@ -0,0 +1,34 @@ +// Copyright (c) 2024 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 . + +#pragma once + +#include + +#include "carla/ros2/publishers/CarlaPublisher.h" + +namespace carla { +namespace ros2 { + + struct CarlaCameraControlPublisherImpl; + + class CarlaCameraControlPublisher : public CarlaPublisher { + public: + CarlaCameraControlPublisher(const char* ros_name = "", const char* parent = ""); + ~CarlaCameraControlPublisher(); + CarlaCameraControlPublisher(const CarlaCameraControlPublisher&); + CarlaCameraControlPublisher& operator=(const CarlaCameraControlPublisher&); + CarlaCameraControlPublisher(CarlaCameraControlPublisher&&); + CarlaCameraControlPublisher& operator=(CarlaCameraControlPublisher&&); + + bool Init(); + bool Publish(); + void SetData(int32_t seconds, uint32_t nanoseconds, float pan, float tilt, float zoom); + const char* type() const override { return "camera control"; } + + private: + std::shared_ptr _impl; + }; +} +} diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/ros2/subscribers/CarlaCameraControlSubscriber.cpp b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/ros2/subscribers/CarlaCameraControlSubscriber.cpp new file mode 100644 index 00000000000..8030c10c1b2 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/ros2/subscribers/CarlaCameraControlSubscriber.cpp @@ -0,0 +1,242 @@ +#include "CarlaCameraControlSubscriber.h" + +#include "CameraController/ros2/types/CarlaCameraControl.h" +#include "CameraController/ros2/types/CarlaCameraControlPubSubTypes.h" +#include "CameraController/ros2/listeners/CarlaCameraControlSubscriberListener.h" + +#include "carla/ros2/plugin-utils/enable-fastdds-include.h" // start fastdds includes +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include "carla/ros2/plugin-utils/disable-fastdds-include.h" // end fastdds includes + +namespace carla { +namespace ros2 { + + namespace efd = eprosima::fastdds::dds; + using erc = eprosima::fastrtps::types::ReturnCode_t; + + struct CarlaCameraControlSubscriberImpl { + efd::DomainParticipant* _participant { nullptr }; + efd::Subscriber* _subscriber { nullptr }; + efd::Topic* _topic { nullptr }; + efd::DataReader* _datareader { nullptr }; + efd::TypeSupport _type { new carla_interfaces::msg::CarlaCameraControlPubSubType() }; + CarlaCameraControlSubscriberListener _listener {nullptr}; + carla_interfaces::msg::CarlaCameraControl _event {}; + CameraControl _control {}; + bool _new_message {false}; + bool _alive {true}; + void* _camera_control {nullptr}; + }; + + bool CarlaCameraControlSubscriber::Init() { + if (_impl->_type == nullptr) { + std::cerr << "Invalid TypeSupport" << std::endl; + return false; + } + + efd::DomainParticipantQos pqos = efd::PARTICIPANT_QOS_DEFAULT; + pqos.name(_name); + auto factory = efd::DomainParticipantFactory::get_instance(); + _impl->_participant = factory->create_participant(0, pqos); + if (_impl->_participant == nullptr) { + std::cerr << "Failed to create DomainParticipant" << std::endl; + return false; + } + _impl->_type.register_type(_impl->_participant); + + efd::SubscriberQos subqos = efd::SUBSCRIBER_QOS_DEFAULT; + _impl->_subscriber = _impl->_participant->create_subscriber(subqos, nullptr); + if (_impl->_subscriber == nullptr) { + std::cerr << "Failed to create Subscriber" << std::endl; + return false; + } + + efd::TopicQos tqos = efd::TOPIC_QOS_DEFAULT; + const std::string base { "rt/carla/" }; + const std::string subscriber_type {"/camera_control_cmd"}; + std::string topic_name = base; + if (!_parent.empty()) + topic_name += _parent + "/"; + topic_name += _name; + topic_name += subscriber_type; + _impl->_topic = _impl->_participant->create_topic(topic_name, _impl->_type->getName(), tqos); + if (_impl->_topic == nullptr) { + std::cerr << "Failed to create Topic" << std::endl; + return false; + } + + efd::DataReaderQos rqos = efd::DATAREADER_QOS_DEFAULT; + efd::DataReaderListener* listener = (efd::DataReaderListener*)_impl->_listener._impl.get(); + _impl->_datareader = _impl->_subscriber->create_datareader(_impl->_topic, rqos, listener); + if (_impl->_datareader == nullptr) { + std::cerr << "Failed to create DataReader" << std::endl; + return false; + } + return true; + } + + bool CarlaCameraControlSubscriber::Read() { + efd::SampleInfo info; + eprosima::fastrtps::types::ReturnCode_t rcode = _impl->_datareader->take_next_sample(&_impl->_event, &info); + if (rcode == erc::ReturnCodeValue::RETCODE_OK) { + return true; + } + if (rcode == erc::ReturnCodeValue::RETCODE_ERROR) { + std::cerr << "RETCODE_ERROR" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_UNSUPPORTED) { + std::cerr << "RETCODE_UNSUPPORTED" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_BAD_PARAMETER) { + std::cerr << "RETCODE_BAD_PARAMETER" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_PRECONDITION_NOT_MET) { + std::cerr << "RETCODE_PRECONDITION_NOT_MET" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_OUT_OF_RESOURCES) { + std::cerr << "RETCODE_OUT_OF_RESOURCES" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ENABLED) { + std::cerr << "RETCODE_NOT_ENABLED" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_IMMUTABLE_POLICY) { + std::cerr << "RETCODE_IMMUTABLE_POLICY" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_INCONSISTENT_POLICY) { + std::cerr << "RETCODE_INCONSISTENT_POLICY" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_ALREADY_DELETED) { + std::cerr << "RETCODE_ALREADY_DELETED" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_TIMEOUT) { + std::cerr << "RETCODE_TIMEOUT" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_NO_DATA) { + std::cerr << "RETCODE_NO_DATA" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_ILLEGAL_OPERATION) { + std::cerr << "RETCODE_ILLEGAL_OPERATION" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ALLOWED_BY_SECURITY) { + std::cerr << "RETCODE_NOT_ALLOWED_BY_SECURITY" << std::endl; + return false; + } + std::cerr << "UNKNOWN" << std::endl; + return false; + } + + void CarlaCameraControlSubscriber::ForwardMessage(CameraControl control) { + _impl->_control = control; + _impl->_new_message = true; + } + + void CarlaCameraControlSubscriber::DestroySubscriber() { + _impl->_alive = false; + } + + CameraControl CarlaCameraControlSubscriber::GetMessage() { + _impl->_new_message = false; + return _impl->_control; + } + + bool CarlaCameraControlSubscriber::IsAlive() { + return _impl->_alive; + } + + bool CarlaCameraControlSubscriber::HasNewMessage() { + return _impl->_new_message; + } + + void* CarlaCameraControlSubscriber::GetCameraControl() { + return _impl->_camera_control; + } + + CarlaCameraControlSubscriber::CarlaCameraControlSubscriber(void* camera_control, const char* ros_name, const char* parent) : + _impl(std::make_shared()) { + _impl->_listener.SetOwner(this); + _impl->_camera_control = camera_control; + _name = ros_name; + _parent = parent; + } + + CarlaCameraControlSubscriber::~CarlaCameraControlSubscriber() { + if (!_impl) + return; + + if (_impl->_datareader) + _impl->_subscriber->delete_datareader(_impl->_datareader); + + if (_impl->_subscriber) + _impl->_participant->delete_subscriber(_impl->_subscriber); + + if (_impl->_topic) + _impl->_participant->delete_topic(_impl->_topic); + + if (_impl->_participant) + efd::DomainParticipantFactory::get_instance()->delete_participant(_impl->_participant); + } + + CarlaCameraControlSubscriber::CarlaCameraControlSubscriber(const CarlaCameraControlSubscriber& other) { + _frame_id = other._frame_id; + _name = other._name; + _parent = other._parent; + _impl = other._impl; + _impl->_listener.SetOwner(this); + } + + CarlaCameraControlSubscriber& CarlaCameraControlSubscriber::operator=(const CarlaCameraControlSubscriber& other) { + _frame_id = other._frame_id; + _name = other._name; + _parent = other._parent; + _impl = other._impl; + _impl->_listener.SetOwner(this); + + return *this; + } + + CarlaCameraControlSubscriber::CarlaCameraControlSubscriber(CarlaCameraControlSubscriber&& other) { + _frame_id = std::move(other._frame_id); + _name = std::move(other._name); + _parent = std::move(other._parent); + _impl = std::move(other._impl); + _impl->_listener.SetOwner(this); + } + + CarlaCameraControlSubscriber& CarlaCameraControlSubscriber::operator=(CarlaCameraControlSubscriber&& other) { + _frame_id = std::move(other._frame_id); + _name = std::move(other._name); + _parent = std::move(other._parent); + _impl = std::move(other._impl); + _impl->_listener.SetOwner(this); + + return *this; + } +} +} diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/ros2/subscribers/CarlaCameraControlSubscriber.h b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/ros2/subscribers/CarlaCameraControlSubscriber.h new file mode 100644 index 00000000000..ff348bb2e48 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/ros2/subscribers/CarlaCameraControlSubscriber.h @@ -0,0 +1,44 @@ +// Copyright (c) 2024 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 . + +#pragma once + +#include +#include + +#include "carla/ros2/subscribers/CarlaSubscriber.h" +#include "CameraController/ros2/ROS2CameraControlCallbackData.h" + +namespace carla { +namespace ros2 { + + struct CarlaCameraControlSubscriberImpl; + + class CarlaCameraControlSubscriber : public CarlaSubscriber { + public: + CarlaCameraControlSubscriber(void* camera_control, const char* ros_name = "", const char* parent = ""); + ~CarlaCameraControlSubscriber(); + CarlaCameraControlSubscriber(const CarlaCameraControlSubscriber&); + CarlaCameraControlSubscriber& operator=(const CarlaCameraControlSubscriber&); + CarlaCameraControlSubscriber(CarlaCameraControlSubscriber&&); + CarlaCameraControlSubscriber& operator=(CarlaCameraControlSubscriber&&); + + bool HasNewMessage(); + bool IsAlive(); + CameraControl GetMessage(); + void* GetCameraControl(); + + bool Init(); + bool Read(); + const char* type() const override { return "Camera Control "; } + + //Do not call, for internal use only + void ForwardMessage(CameraControl control); + void DestroySubscriber(); + + private: + std::shared_ptr _impl; + }; +} +} diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/ros2/types/CarlaCameraControl.cpp b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/ros2/types/CarlaCameraControl.cpp new file mode 100644 index 00000000000..cd54a866eb3 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/ros2/types/CarlaCameraControl.cpp @@ -0,0 +1,314 @@ +// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// 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. + +/*! + * @file CarlaCameraControl.cpp + * This source file contains the definition of the described types in the IDL file. + * + * This file was generated by the tool gen. + */ + +#ifdef _WIN32 +// Remove linker warning LNK4221 on Visual Studio +namespace { +char dummy; +} // namespace +#endif // _WIN32 + +#include "CarlaCameraControl.h" +#include "carla/ros2/plugin-utils/enable-fastdds-include.h" // start fastdds includes +#include +#include +#include "carla/ros2/plugin-utils/disable-fastdds-include.h" // end fastdds includes + +using namespace eprosima::fastcdr::exception; + +#include + +#define builtin_interfaces_msg_Time_max_cdr_typesize 8ULL; +#define carla_interfaces_msg_CarlaCameraControl_max_cdr_typesize 280ULL; +#define std_msgs_msg_Header_max_cdr_typesize 268ULL; +#define builtin_interfaces_msg_Time_max_key_cdr_typesize 0ULL; +#define carla_interfaces_msg_CarlaCameraControl_max_key_cdr_typesize 0ULL; +#define std_msgs_msg_Header_max_key_cdr_typesize 0ULL; + +carla_interfaces::msg::CarlaCameraControl::CarlaCameraControl() +{ + // std_msgs::msg::Header m_header + + // float m_pan + m_pan = -999.0; + // float m_tilt + m_tilt = -999.0; + // float m_zoom + m_zoom = -999.0; + +} + +carla_interfaces::msg::CarlaCameraControl::~CarlaCameraControl() +{ + + + + +} + +carla_interfaces::msg::CarlaCameraControl::CarlaCameraControl( + const CarlaCameraControl& x) +{ + m_header = x.m_header; + m_pan = x.m_pan; + m_tilt = x.m_tilt; + m_zoom = x.m_zoom; +} + +carla_interfaces::msg::CarlaCameraControl::CarlaCameraControl( + CarlaCameraControl&& x) noexcept +{ + m_header = std::move(x.m_header); + m_pan = x.m_pan; + m_tilt = x.m_tilt; + m_zoom = x.m_zoom; +} + +carla_interfaces::msg::CarlaCameraControl& carla_interfaces::msg::CarlaCameraControl::operator =( + const CarlaCameraControl& x) +{ + + m_header = x.m_header; + m_pan = x.m_pan; + m_tilt = x.m_tilt; + m_zoom = x.m_zoom; + + return *this; +} + +carla_interfaces::msg::CarlaCameraControl& carla_interfaces::msg::CarlaCameraControl::operator =( + CarlaCameraControl&& x) noexcept +{ + + m_header = std::move(x.m_header); + m_pan = x.m_pan; + m_tilt = x.m_tilt; + m_zoom = x.m_zoom; + + return *this; +} + +bool carla_interfaces::msg::CarlaCameraControl::operator ==( + const CarlaCameraControl& x) const +{ + + return (m_header == x.m_header && m_pan == x.m_pan && m_tilt == x.m_tilt && m_zoom == x.m_zoom); +} + +bool carla_interfaces::msg::CarlaCameraControl::operator !=( + const CarlaCameraControl& x) const +{ + return !(*this == x); +} + +size_t carla_interfaces::msg::CarlaCameraControl::getMaxCdrSerializedSize( + size_t current_alignment) +{ + static_cast(current_alignment); + return carla_interfaces_msg_CarlaCameraControl_max_cdr_typesize; +} + +size_t carla_interfaces::msg::CarlaCameraControl::getCdrSerializedSize( + const carla_interfaces::msg::CarlaCameraControl& data, + size_t current_alignment) +{ + (void)data; + size_t initial_alignment = current_alignment; + + + current_alignment += std_msgs::msg::Header::getCdrSerializedSize(data.header(), current_alignment); + current_alignment += 4 + eprosima::fastcdr::Cdr::alignment(current_alignment, 4); + + + current_alignment += 4 + eprosima::fastcdr::Cdr::alignment(current_alignment, 4); + + + current_alignment += 4 + eprosima::fastcdr::Cdr::alignment(current_alignment, 4); + + + + return current_alignment - initial_alignment; +} + +void carla_interfaces::msg::CarlaCameraControl::serialize( + eprosima::fastcdr::Cdr& scdr) const +{ + + scdr << m_header; + scdr << m_pan; + scdr << m_tilt; + scdr << m_zoom; + +} + +void carla_interfaces::msg::CarlaCameraControl::deserialize( + eprosima::fastcdr::Cdr& dcdr) +{ + + dcdr >> m_header; + dcdr >> m_pan; + dcdr >> m_tilt; + dcdr >> m_zoom; +} + +/*! + * @brief This function copies the value in member header + * @param _header New value to be copied in member header + */ +void carla_interfaces::msg::CarlaCameraControl::header( + const std_msgs::msg::Header& _header) +{ + m_header = _header; +} + +/*! + * @brief This function moves the value in member header + * @param _header New value to be moved in member header + */ +void carla_interfaces::msg::CarlaCameraControl::header( + std_msgs::msg::Header&& _header) +{ + m_header = std::move(_header); +} + +/*! + * @brief This function returns a constant reference to member header + * @return Constant reference to member header + */ +const std_msgs::msg::Header& carla_interfaces::msg::CarlaCameraControl::header() const +{ + return m_header; +} + +/*! + * @brief This function returns a reference to member header + * @return Reference to member header + */ +std_msgs::msg::Header& carla_interfaces::msg::CarlaCameraControl::header() +{ + return m_header; +} +/*! + * @brief This function sets a value in member pan + * @param _pan New value for member pan + */ +void carla_interfaces::msg::CarlaCameraControl::pan( + float _pan) +{ + m_pan = _pan; +} + +/*! + * @brief This function returns the value of member pan + * @return Value of member pan + */ +float carla_interfaces::msg::CarlaCameraControl::pan() const +{ + return m_pan; +} + +/*! + * @brief This function returns a reference to member pan + * @return Reference to member pan + */ +float& carla_interfaces::msg::CarlaCameraControl::pan() +{ + return m_pan; +} + +/*! + * @brief This function sets a value in member tilt + * @param _tilt New value for member tilt + */ +void carla_interfaces::msg::CarlaCameraControl::tilt( + float _tilt) +{ + m_tilt = _tilt; +} + +/*! + * @brief This function returns the value of member tilt + * @return Value of member tilt + */ +float carla_interfaces::msg::CarlaCameraControl::tilt() const +{ + return m_tilt; +} + +/*! + * @brief This function returns a reference to member tilt + * @return Reference to member tilt + */ +float& carla_interfaces::msg::CarlaCameraControl::tilt() +{ + return m_tilt; +} + +/*! + * @brief This function sets a value in member zoom + * @param _zoom New value for member zoom + */ +void carla_interfaces::msg::CarlaCameraControl::zoom( + float _zoom) +{ + m_zoom = _zoom; +} + +/*! + * @brief This function returns the value of member zoom + * @return Value of member zoom + */ +float carla_interfaces::msg::CarlaCameraControl::zoom() const +{ + return m_zoom; +} + +/*! + * @brief This function returns a reference to member zoom + * @return Reference to member zoom + */ +float& carla_interfaces::msg::CarlaCameraControl::zoom() +{ + return m_zoom; +} + + + +size_t carla_interfaces::msg::CarlaCameraControl::getKeyMaxCdrSerializedSize( + size_t current_alignment) +{ + static_cast(current_alignment); + return carla_interfaces_msg_CarlaCameraControl_max_key_cdr_typesize; +} + +bool carla_interfaces::msg::CarlaCameraControl::isKeyDefined() +{ + return false; +} + +void carla_interfaces::msg::CarlaCameraControl::serializeKey( + eprosima::fastcdr::Cdr& scdr) const +{ + (void) scdr; +} + + + diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/ros2/types/CarlaCameraControl.h b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/ros2/types/CarlaCameraControl.h new file mode 100644 index 00000000000..e38e1ddd40c --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/ros2/types/CarlaCameraControl.h @@ -0,0 +1,282 @@ +// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// 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. + +/*! + * @file CarlaCameraControl.h + * This header file contains the declaration of the described types in the IDL file. + * + * This file was generated by the tool gen. + */ + +#ifndef _FAST_DDS_GENERATED_CARLA_INTERFACES_MSG_CARLACAMERACONTROL_H_ +#define _FAST_DDS_GENERATED_CARLA_INTERFACES_MSG_CARLACAMERACONTROL_H_ + +#include "carla/ros2/plugin-utils/enable-fastdds-include.h" // start fastdds includes +#include "carla/ros2/types/Header.h" +#include +#include "carla/ros2/plugin-utils/disable-fastdds-include.h" // end fastdds includes + +#include +#include +#include +#include +#include +#include + +#if defined(_WIN32) +#if defined(EPROSIMA_USER_DLL_EXPORT) +#define eProsima_user_DllExport __declspec( dllexport ) +#else +#define eProsima_user_DllExport +#endif // EPROSIMA_USER_DLL_EXPORT +#else +#define eProsima_user_DllExport +#endif // _WIN32 + +#if defined(_WIN32) +#if defined(EPROSIMA_USER_DLL_EXPORT) +#if defined(CARLACAMERACONTROL_SOURCE) +#define CARLACAMERACONTROL_DllAPI __declspec( dllexport ) +#else +#define CARLACAMERACONTROL_DllAPI __declspec( dllimport ) +#endif // CARLACAMERACONTROL_SOURCE +#else +#define CARLACAMERACONTROL_DllAPI +#endif // EPROSIMA_USER_DLL_EXPORT +#else +#define CARLACAMERACONTROL_DllAPI +#endif // _WIN32 + +namespace eprosima { +namespace fastcdr { +class Cdr; +} // namespace fastcdr +} // namespace eprosima + + +namespace carla_interfaces { + namespace msg { + /*! + * @brief This class represents the structure CarlaCameraControl defined by the user in the IDL file. + * @ingroup CarlaCameraControl + */ + class CarlaCameraControl + { + public: + + /*! + * @brief Default constructor. + */ + eProsima_user_DllExport CarlaCameraControl(); + + /*! + * @brief Default destructor. + */ + eProsima_user_DllExport ~CarlaCameraControl(); + + /*! + * @brief Copy constructor. + * @param x Reference to the object carla_interfaces::msg::CarlaCameraControl that will be copied. + */ + eProsima_user_DllExport CarlaCameraControl( + const CarlaCameraControl& x); + + /*! + * @brief Move constructor. + * @param x Reference to the object carla_interfaces::msg::CarlaCameraControl that will be copied. + */ + eProsima_user_DllExport CarlaCameraControl( + CarlaCameraControl&& x) noexcept; + + /*! + * @brief Copy assignment. + * @param x Reference to the object carla_interfaces::msg::CarlaCameraControl that will be copied. + */ + eProsima_user_DllExport CarlaCameraControl& operator =( + const CarlaCameraControl& x); + + /*! + * @brief Move assignment. + * @param x Reference to the object carla_interfaces::msg::CarlaCameraControl that will be copied. + */ + eProsima_user_DllExport CarlaCameraControl& operator =( + CarlaCameraControl&& x) noexcept; + + /*! + * @brief Comparison operator. + * @param x carla_interfaces::msg::CarlaCameraControl object to compare. + */ + eProsima_user_DllExport bool operator ==( + const CarlaCameraControl& x) const; + + /*! + * @brief Comparison operator. + * @param x carla_interfaces::msg::CarlaCameraControl object to compare. + */ + eProsima_user_DllExport bool operator !=( + const CarlaCameraControl& x) const; + + /*! + * @brief This function copies the value in member header + * @param _header New value to be copied in member header + */ + eProsima_user_DllExport void header( + const std_msgs::msg::Header& _header); + + /*! + * @brief This function moves the value in member header + * @param _header New value to be moved in member header + */ + eProsima_user_DllExport void header( + std_msgs::msg::Header&& _header); + + /*! + * @brief This function returns a constant reference to member header + * @return Constant reference to member header + */ + eProsima_user_DllExport const std_msgs::msg::Header& header() const; + + /*! + * @brief This function returns a reference to member header + * @return Reference to member header + */ + eProsima_user_DllExport std_msgs::msg::Header& header(); + /*! + * @brief This function sets a value in member pan + * @param _pan New value for member pan + */ + eProsima_user_DllExport void pan( + float _pan); + + /*! + * @brief This function returns the value of member pan + * @return Value of member pan + */ + eProsima_user_DllExport float pan() const; + + /*! + * @brief This function returns a reference to member pan + * @return Reference to member pan + */ + eProsima_user_DllExport float& pan(); + + /*! + * @brief This function sets a value in member tilt + * @param _tilt New value for member tilt + */ + eProsima_user_DllExport void tilt( + float _tilt); + + /*! + * @brief This function returns the value of member tilt + * @return Value of member tilt + */ + eProsima_user_DllExport float tilt() const; + + /*! + * @brief This function returns a reference to member tilt + * @return Reference to member tilt + */ + eProsima_user_DllExport float& tilt(); + + /*! + * @brief This function sets a value in member zoom + * @param _zoom New value for member zoom + */ + eProsima_user_DllExport void zoom( + float _zoom); + + /*! + * @brief This function returns the value of member zoom + * @return Value of member zoom + */ + eProsima_user_DllExport float zoom() const; + + /*! + * @brief This function returns a reference to member zoom + * @return Reference to member zoom + */ + eProsima_user_DllExport float& zoom(); + + + /*! + * @brief This function returns the maximum serialized size of an object + * depending on the buffer alignment. + * @param current_alignment Buffer alignment. + * @return Maximum serialized size. + */ + eProsima_user_DllExport static size_t getMaxCdrSerializedSize( + size_t current_alignment = 0); + + /*! + * @brief This function returns the serialized size of a data depending on the buffer alignment. + * @param data Data which is calculated its serialized size. + * @param current_alignment Buffer alignment. + * @return Serialized size. + */ + eProsima_user_DllExport static size_t getCdrSerializedSize( + const carla_interfaces::msg::CarlaCameraControl& data, + size_t current_alignment = 0); + + + /*! + * @brief This function serializes an object using CDR serialization. + * @param cdr CDR serialization object. + */ + eProsima_user_DllExport void serialize( + eprosima::fastcdr::Cdr& cdr) const; + + /*! + * @brief This function deserializes an object using CDR serialization. + * @param cdr CDR serialization object. + */ + eProsima_user_DllExport void deserialize( + eprosima::fastcdr::Cdr& cdr); + + + + /*! + * @brief This function returns the maximum serialized size of the Key of an object + * depending on the buffer alignment. + * @param current_alignment Buffer alignment. + * @return Maximum serialized size. + */ + eProsima_user_DllExport static size_t getKeyMaxCdrSerializedSize( + size_t current_alignment = 0); + + /*! + * @brief This function tells you if the Key has been defined for this type + */ + eProsima_user_DllExport static bool isKeyDefined(); + + /*! + * @brief This function serializes the key members of an object using CDR serialization. + * @param cdr CDR serialization object. + */ + eProsima_user_DllExport void serializeKey( + eprosima::fastcdr::Cdr& cdr) const; + + private: + + std_msgs::msg::Header m_header; + float m_pan; + float m_tilt; + float m_zoom; + + }; + } // namespace msg +} // namespace carla_interfaces + +#endif // _FAST_DDS_GENERATED_CARLA_INTERFACES_MSG_CARLACAMERACONTROL_H_ + diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/ros2/types/CarlaCameraControlPubSubTypes.cpp b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/ros2/types/CarlaCameraControlPubSubTypes.cpp new file mode 100644 index 00000000000..afa7b117044 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/ros2/types/CarlaCameraControlPubSubTypes.cpp @@ -0,0 +1,180 @@ +// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// 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. + +/*! + * @file CarlaCameraControlPubSubTypes.cpp + * This header file contains the implementation of the serialization functions. + * + * This file was generated by the tool fastcdrgen. + */ + + + #include "carla/ros2/plugin-utils/enable-fastdds-include.h" // start fastdds includes + #include + #include + #include "carla/ros2/plugin-utils/disable-fastdds-include.h" // end fastdds includes + +#include "CarlaCameraControlPubSubTypes.h" + +using SerializedPayload_t = eprosima::fastrtps::rtps::SerializedPayload_t; +using InstanceHandle_t = eprosima::fastrtps::rtps::InstanceHandle_t; + +namespace carla_interfaces { + namespace msg { + CarlaCameraControlPubSubType::CarlaCameraControlPubSubType() + { + setName("carla_interfaces::msg::dds_::CarlaCameraControl_"); + auto type_size = CarlaCameraControl::getMaxCdrSerializedSize(); + type_size += eprosima::fastcdr::Cdr::alignment(type_size, 4); /* possible submessage alignment */ + m_typeSize = static_cast(type_size) + 4; /*encapsulation*/ + m_isGetKeyDefined = CarlaCameraControl::isKeyDefined(); + size_t keyLength = CarlaCameraControl::getKeyMaxCdrSerializedSize() > 16 ? + CarlaCameraControl::getKeyMaxCdrSerializedSize() : 16; + m_keyBuffer = reinterpret_cast(malloc(keyLength)); + memset(m_keyBuffer, 0, keyLength); + } + + CarlaCameraControlPubSubType::~CarlaCameraControlPubSubType() + { + if (m_keyBuffer != nullptr) + { + free(m_keyBuffer); + } + } + + bool CarlaCameraControlPubSubType::serialize( + void* data, + SerializedPayload_t* payload) + { + CarlaCameraControl* p_type = static_cast(data); + + // Object that manages the raw buffer. + eprosima::fastcdr::FastBuffer fastbuffer(reinterpret_cast(payload->data), payload->max_size); + // Object that serializes the data. + eprosima::fastcdr::Cdr ser(fastbuffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, eprosima::fastcdr::Cdr::DDS_CDR); + payload->encapsulation = ser.endianness() == eprosima::fastcdr::Cdr::BIG_ENDIANNESS ? CDR_BE : CDR_LE; + + try + { + // Serialize encapsulation + ser.serialize_encapsulation(); + // Serialize the object. + p_type->serialize(ser); + } + catch (eprosima::fastcdr::exception::Exception& /*exception*/) + { + return false; + } + + // Get the serialized length + payload->length = static_cast(ser.getSerializedDataLength()); + return true; + } + + bool CarlaCameraControlPubSubType::deserialize( + SerializedPayload_t* payload, + void* data) + { + try + { + // Convert DATA to pointer of your type + CarlaCameraControl* p_type = static_cast(data); + + // Object that manages the raw buffer. + eprosima::fastcdr::FastBuffer fastbuffer(reinterpret_cast(payload->data), payload->length); + + // Object that deserializes the data. + eprosima::fastcdr::Cdr deser(fastbuffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, eprosima::fastcdr::Cdr::DDS_CDR); + + // Deserialize encapsulation. + deser.read_encapsulation(); + payload->encapsulation = deser.endianness() == eprosima::fastcdr::Cdr::BIG_ENDIANNESS ? CDR_BE : CDR_LE; + + // Deserialize the object. + p_type->deserialize(deser); + } + catch (eprosima::fastcdr::exception::Exception& /*exception*/) + { + return false; + } + + return true; + } + + std::function CarlaCameraControlPubSubType::getSerializedSizeProvider( + void* data) + { + return [data]() -> uint32_t + { + return static_cast(type::getCdrSerializedSize(*static_cast(data))) + + 4u /*encapsulation*/; + }; + } + + void* CarlaCameraControlPubSubType::createData() + { + return reinterpret_cast(new CarlaCameraControl()); + } + + void CarlaCameraControlPubSubType::deleteData( + void* data) + { + delete(reinterpret_cast(data)); + } + + bool CarlaCameraControlPubSubType::getKey( + void* data, + InstanceHandle_t* handle, + bool force_md5) + { + if (!m_isGetKeyDefined) + { + return false; + } + + CarlaCameraControl* p_type = static_cast(data); + + // Object that manages the raw buffer. + eprosima::fastcdr::FastBuffer fastbuffer(reinterpret_cast(m_keyBuffer), + CarlaCameraControl::getKeyMaxCdrSerializedSize()); + + // Object that serializes the data. + eprosima::fastcdr::Cdr ser(fastbuffer, eprosima::fastcdr::Cdr::BIG_ENDIANNESS); + p_type->serializeKey(ser); + if (force_md5 || CarlaCameraControl::getKeyMaxCdrSerializedSize() > 16) + { + m_md5.init(); + m_md5.update(m_keyBuffer, static_cast(ser.getSerializedDataLength())); + m_md5.finalize(); + for (uint8_t i = 0; i < 16; ++i) + { + handle->value[i] = m_md5.digest[i]; + } + } + else + { + for (uint8_t i = 0; i < 16; ++i) + { + handle->value[i] = m_keyBuffer[i]; + } + } + return true; + } + + + } //End of namespace msg + + +} //End of namespace carla_interfaces + diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/ros2/types/CarlaCameraControlPubSubTypes.h b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/ros2/types/CarlaCameraControlPubSubTypes.h new file mode 100644 index 00000000000..80f6c3f5ff0 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CameraController/Source/CameraController/ros2/types/CarlaCameraControlPubSubTypes.h @@ -0,0 +1,113 @@ +// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// 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. + +/*! + * @file CarlaCameraControlPubSubTypes.h + * This header file contains the declaration of the serialization functions. + * + * This file was generated by the tool fastcdrgen. + */ + + +#ifndef _FAST_DDS_GENERATED_CARLA_INTERFACES_MSG_CARLACAMERACONTROL_PUBSUBTYPES_H_ +#define _FAST_DDS_GENERATED_CARLA_INTERFACES_MSG_CARLACAMERACONTROL_PUBSUBTYPES_H_ + +#include "carla/ros2/plugin-utils/enable-fastdds-include.h" // start fastdds includes +#include +#include +#include "carla/ros2/types/HeaderPubSubTypes.h" +#include "carla/ros2/plugin-utils/disable-fastdds-include.h" // end fastdds includes + +#include "CarlaCameraControl.h" + +#if !defined(GEN_API_VER) || (GEN_API_VER != 1) +#error \ + Generated CarlaCameraControl is not compatible with current installed Fast DDS. Please, regenerate it with fastddsgen. +#endif // GEN_API_VER + +namespace carla_interfaces +{ + namespace msg + { + + /*! + * @brief This class represents the TopicDataType of the type CarlaCameraControl defined by the user in the IDL file. + * @ingroup CarlaCameraControl + */ + class CarlaCameraControlPubSubType : public eprosima::fastdds::dds::TopicDataType + { + public: + + typedef CarlaCameraControl type; + + eProsima_user_DllExport CarlaCameraControlPubSubType(); + + eProsima_user_DllExport virtual ~CarlaCameraControlPubSubType() override; + + eProsima_user_DllExport virtual bool serialize( + void* data, + eprosima::fastrtps::rtps::SerializedPayload_t* payload) override; + + eProsima_user_DllExport virtual bool deserialize( + eprosima::fastrtps::rtps::SerializedPayload_t* payload, + void* data) override; + + eProsima_user_DllExport virtual std::function getSerializedSizeProvider( + void* data) override; + + eProsima_user_DllExport virtual bool getKey( + void* data, + eprosima::fastrtps::rtps::InstanceHandle_t* ihandle, + bool force_md5 = false) override; + + eProsima_user_DllExport virtual void* createData() override; + + eProsima_user_DllExport virtual void deleteData( + void* data) override; + + #ifdef TOPIC_DATA_TYPE_API_HAS_IS_BOUNDED + eProsima_user_DllExport inline bool is_bounded() const override + { + return false; + } + + #endif // TOPIC_DATA_TYPE_API_HAS_IS_BOUNDED + + #ifdef TOPIC_DATA_TYPE_API_HAS_IS_PLAIN + eProsima_user_DllExport inline bool is_plain() const override + { + return false; + } + + #endif // TOPIC_DATA_TYPE_API_HAS_IS_PLAIN + + #ifdef TOPIC_DATA_TYPE_API_HAS_CONSTRUCT_SAMPLE + eProsima_user_DllExport inline bool construct_sample( + void* memory) const override + { + (void)memory; + return false; + } + + #endif // TOPIC_DATA_TYPE_API_HAS_CONSTRUCT_SAMPLE + + MD5 m_md5; + unsigned char* m_keyBuffer; + + }; + } +} + +#endif // _FAST_DDS_GENERATED_CARLA_INTERFACES_MSG_CARLACAMERACONTROL_PUBSUBTYPES_H_ + diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CarlaTemplateExternalInterface/CarlaTemplateExternalInterface.uplugin b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CarlaTemplateExternalInterface/CarlaTemplateExternalInterface.uplugin new file mode 100644 index 00000000000..d218722b6f6 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CarlaTemplateExternalInterface/CarlaTemplateExternalInterface.uplugin @@ -0,0 +1,26 @@ +{ + "FileVersion" : 3, + "Version" : "0.0.0", + "VersionName": "0.0.0", + "FriendlyName": "CarlaTemplateExternalInterface", + "Description": "Carla Template External Interface Plugin", + "Category" : "Science", + "MarketplaceURL" : "", + "EnabledByDefault" : true, + "CanContainContent": true, + "IsBetaVersion" : true, + "Installed": false, + "Modules": [ + { + "Name": "CarlaTemplateExternalInterface", + "Type": "Runtime", + "LoadingPhase": "Default" + } + ], + "Plugins": [ + { + "Name": "Carla", + "Enabled": true + } + ] +} diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CarlaTemplateExternalInterface/Source/CarlaInterface/CarlaActor.cpp b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CarlaTemplateExternalInterface/Source/CarlaInterface/CarlaActor.cpp new file mode 100644 index 00000000000..81e31c38548 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CarlaTemplateExternalInterface/Source/CarlaInterface/CarlaActor.cpp @@ -0,0 +1,38 @@ +// Copyright (c) 2024 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 . + +#include "CarlaActor.h" +#include "CarlaTemplateExternalInterfaceActor.h" +#include "CarlaTemplateExternalInterfaceActorData.h" + +const FString FCarlaTemplateExternalInterfaceActor::CustomType = "CarlaTemplateExternalInterface_CarlaTemplateExternalInterface"; + +FCarlaTemplateExternalInterfaceActor::FCarlaTemplateExternalInterfaceActor( + IdType ActorId, + AActor* Actor, + TSharedPtr Info, + carla::rpc::ActorState InState, + UWorld* World) + : FCarlaActor(ActorId, Actor, Info, InState, World) +{ + Type = ActorType::Custom; + ActorData = MakeShared(); +} + +TSharedPtr FCarlaTemplateExternalInterfaceActor::CreateInstance( + FCarlaActor::IdType ActorId, + AActor* Actor, + TSharedPtr Info, + carla::rpc::ActorState InState, + UWorld* World) + { + return MakeShared(ActorId, Actor, std::move(Info), InState, World); + } + +void FCarlaTemplateExternalInterfaceActor::RegisterClassWithFactory() + { + CarlaActorConstructorFactory::Instance().Register(TEXT("FCarlaTemplateExternalInterfaceActor"), ACarlaTemplateExternalInterfaceActor::StaticClass(), CreateInstance); + } diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CarlaTemplateExternalInterface/Source/CarlaInterface/CarlaActor.h b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CarlaTemplateExternalInterface/Source/CarlaInterface/CarlaActor.h new file mode 100644 index 00000000000..02aa584cf64 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CarlaTemplateExternalInterface/Source/CarlaInterface/CarlaActor.h @@ -0,0 +1,33 @@ +// Copyright (c) 2024 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 . + +#pragma once + +#include "Carla/Actor/CarlaActor.h" +#include "Carla/Actor/CarlaActorConstructorFactory.h" + +class FCarlaTemplateExternalInterfaceActor : public FCarlaActor +{ +public: + FCarlaTemplateExternalInterfaceActor( + IdType ActorId, + AActor* Actor, + TSharedPtr Info, + carla::rpc::ActorState InState, + UWorld* World); + + static TSharedPtr CreateInstance(FCarlaActor::IdType ActorId, + AActor* Actor, + TSharedPtr Info, + carla::rpc::ActorState InState, + UWorld* World); + + static void RegisterClassWithFactory(); + + protected: + static const FString CustomType; + +}; diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CarlaTemplateExternalInterface/Source/CarlaInterface/CarlaTemplateExternalInterfaceActorData.cpp b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CarlaTemplateExternalInterface/Source/CarlaInterface/CarlaTemplateExternalInterfaceActorData.cpp new file mode 100644 index 00000000000..28ab43e4467 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CarlaTemplateExternalInterface/Source/CarlaInterface/CarlaTemplateExternalInterfaceActorData.cpp @@ -0,0 +1,17 @@ +// Copyright (c) 2024 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 . + +#include "CarlaTemplateExternalInterfaceActorData.h" + +void FCarlaTemplateExternalInterfaceData::RecordActorData(FCarlaActor* CarlaActor, UCarlaEpisode* CarlaEpisode) +{ + FActorData::RecordActorData(CarlaActor, CarlaEpisode); +} + +void FCarlaTemplateExternalInterfaceData::RestoreActorData(FCarlaActor* CarlaActor, UCarlaEpisode* CarlaEpisode) +{ + FActorData::RestoreActorData(CarlaActor, CarlaEpisode); +} diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CarlaTemplateExternalInterface/Source/CarlaInterface/CarlaTemplateExternalInterfaceActorData.h b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CarlaTemplateExternalInterface/Source/CarlaInterface/CarlaTemplateExternalInterfaceActorData.h new file mode 100644 index 00000000000..63479302d04 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CarlaTemplateExternalInterface/Source/CarlaInterface/CarlaTemplateExternalInterfaceActorData.h @@ -0,0 +1,18 @@ +// Copyright (c) 2024 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 . + +#pragma once + +#include "Carla/Actor/ActorData.h" + +class FCarlaTemplateExternalInterfaceData : public FActorData +{ +public: + + virtual void RecordActorData(FCarlaActor* CarlaActor, UCarlaEpisode* CarlaEpisode) override; + + virtual void RestoreActorData(FCarlaActor* CarlaActor, UCarlaEpisode* CarlaEpisode) override; +}; diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CarlaTemplateExternalInterface/Source/CarlaTemplateExternalInterface.Build.cs b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CarlaTemplateExternalInterface/Source/CarlaTemplateExternalInterface.Build.cs new file mode 100644 index 00000000000..a0ba1eee1ce --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CarlaTemplateExternalInterface/Source/CarlaTemplateExternalInterface.Build.cs @@ -0,0 +1,28 @@ +using UnrealBuildTool; +using System.IO; + +public class CarlaTemplateExternalInterface : ModuleRules +{ + public CarlaTemplateExternalInterface(ReadOnlyTargetRules Target) : base(Target) + { + bEnableExceptions = true; + PrivatePCHHeaderFile = "CarlaTemplateExternalInterface.h"; + + PublicDependencyModuleNames.AddRange(new string[] { + "Carla", + "Chaos", + "ChaosVehicles", + "Core", + "CoreUObject", + "Engine", + "Foliage" + }); + + if (Target.Type == TargetType.Editor) + { + PublicDependencyModuleNames.AddRange(new string[] { "UnrealEd" }); + } + + PrivateDependencyModuleNames.AddRange(new string[] { }); + } +} diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CarlaTemplateExternalInterface/Source/CarlaTemplateExternalInterface.cpp b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CarlaTemplateExternalInterface/Source/CarlaTemplateExternalInterface.cpp new file mode 100644 index 00000000000..d46e46302a0 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CarlaTemplateExternalInterface/Source/CarlaTemplateExternalInterface.cpp @@ -0,0 +1,18 @@ +#include "CarlaTemplateExternalInterface.h" + +#define LOCTEXT_NAMESPACE "FCarlaTemplateExternalInterface" + +DEFINE_LOG_CATEGORY(LogCarlaTemplateExternalInterface) + +void FCarlaTemplateExternalInterface::StartupModule() +{ +} + +void FCarlaTemplateExternalInterface::ShutdownModule() +{ +} + +#undef LOCTEXT_NAMESPACE + +IMPLEMENT_MODULE(FCarlaTemplateExternalInterface, CarlaTemplateExternalInterface) + diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CarlaTemplateExternalInterface/Source/CarlaTemplateExternalInterface.h b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CarlaTemplateExternalInterface/Source/CarlaTemplateExternalInterface.h new file mode 100644 index 00000000000..ab89963ec1a --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CarlaTemplateExternalInterface/Source/CarlaTemplateExternalInterface.h @@ -0,0 +1,18 @@ + +#pragma once + +#include "CoreMinimal.h" +#include "Logging/LogMacros.h" +#include "Modules/ModuleManager.h" + +DECLARE_LOG_CATEGORY_EXTERN(LogCarlaTemplateExternalInterface, Log, All); + +class FCarlaTemplateExternalInterface : public IModuleInterface +{ +public: + + /** IModuleInterface implementation */ + virtual void StartupModule() override; + virtual void ShutdownModule() override; +}; + diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CarlaTemplateExternalInterface/Source/CarlaTemplateExternalInterfaceActor.cpp b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CarlaTemplateExternalInterface/Source/CarlaTemplateExternalInterfaceActor.cpp new file mode 100644 index 00000000000..cc10df827f7 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CarlaTemplateExternalInterface/Source/CarlaTemplateExternalInterfaceActor.cpp @@ -0,0 +1,66 @@ +#include "CarlaTemplateExternalInterfaceActor.h" +#include "CarlaTemplateExternalInterfaceBlueprintLibrary.h" +#include "ros2/ROS2CarlaTemplate.h" + +ACarlaTemplateExternalInterfaceActor::ACarlaTemplateExternalInterfaceActor(const FObjectInitializer &ObjectInitializer) : Super(ObjectInitializer) +{ +// Add code that is required on startup +} + +void ACarlaTemplateExternalInterfaceActor::BeginPlay() +{ + Super::BeginPlay(); + TemplateFrameDelegate.BindUFunction(this, FName("TemplateFrameCallback")); + Activate(); +} + +void ACarlaTemplateExternalInterfaceActor::SetSensorTick(double SensorTick){ + this->SensorTick = SensorTick; +} + +void ACarlaTemplateExternalInterfaceActor::SetTemplateValue(double Value){ + this->TemplateValue = Value; +} + +void ACarlaTemplateExternalInterfaceActor::Set(const FActorDescription &ActorDescription) +{ + Super::Set(ActorDescription); + UCarlaTemplateExternalInterfaceBlueprintLibrary::SetCarlaTemplateExternalInterface(ActorDescription, this); +} + +void ACarlaTemplateExternalInterfaceActor::SetOwner(AActor* OwningActor) +{ + Super::SetOwner(OwningActor); +} + +void ACarlaTemplateExternalInterfaceActor::TemplateFrameCallback(){ + if (!Active){ + return; + } + + auto ROS2CarlaTemplate = carla::ros2::ROS2CarlaTemplate::GetInstance(); + auto StreamId = carla::streaming::detail::token_type(GetToken()).get_stream_id(); + FTransform ActorTransform = GetActorTransform(); + AActor* ParentActor = GetAttachParentActor(); + if (ParentActor) + { + ActorTransform = GetActorTransform().GetRelativeTransform(ParentActor->GetActorTransform()); + } + char const *data = "Hello World"; + + ROS2CarlaTemplate->ProcessDataFromTemplate(StreamId, ActorTransform, data, this); +} + +void ACarlaTemplateExternalInterfaceActor::Activate(){ + Active = true; + if (!GetWorldTimerManager().IsTimerActive(TemplateFrameTimer)){ + GetWorldTimerManager().SetTimer(TemplateFrameTimer, TemplateFrameDelegate, SensorTick, true); + } +} + +void ACarlaTemplateExternalInterfaceActor::Deactivate(){ + Active = false; + if (GetWorldTimerManager().IsTimerActive(TemplateFrameTimer)){ + GetWorldTimerManager().ClearTimer(TemplateFrameTimer); + } +} diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CarlaTemplateExternalInterface/Source/CarlaTemplateExternalInterfaceActor.h b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CarlaTemplateExternalInterface/Source/CarlaTemplateExternalInterfaceActor.h new file mode 100644 index 00000000000..d18bb19f08a --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CarlaTemplateExternalInterface/Source/CarlaTemplateExternalInterfaceActor.h @@ -0,0 +1,59 @@ +#pragma once + +#include "CoreMinimal.h" +#include "GameFramework/Actor.h" + +#include "Carla/Sensor/Sensor.h" +#include "Carla/Actor/ActorDefinition.h" +#include "Carla/Actor/ActorDescription.h" + +#include "CarlaTemplateExternalInterfaceActor.generated.h" + +UCLASS() +class CARLATEMPLATEEXTERNALINTERFACE_API ACarlaTemplateExternalInterfaceActor : public ASensor +{ + GENERATED_BODY() + +public: + + ACarlaTemplateExternalInterfaceActor(const FObjectInitializer &ObjectInitializer); + + void SetSensorTick(double SensorTick); + + void SetTemplateValue(double Value); + + void Set(const FActorDescription &ActorDescription) override; + + void SetOwner(AActor *Owner) override; + +protected: + virtual void BeginPlay() override; + +public: + UFUNCTION(BlueprintCallable) + void Activate(); + + UFUNCTION(BlueprintCallable) + void Deactivate(); + + UFUNCTION() + void TemplateFrameCallback(); + +protected: + FTimerHandle TemplateFrameTimer; + + FTimerDelegate TemplateFrameDelegate; + + UPROPERTY(BlueprintReadWrite) + float TemplateValue = 0.25; + + UPROPERTY(BlueprintReadWrite) + float SensorTick = 0.05; //20HZ + + UPROPERTY(BlueprintReadOnly) + bool Active = true; + +public: + UPROPERTY(BlueprintReadOnly) + FString OutputName; +}; diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CarlaTemplateExternalInterface/Source/CarlaTemplateExternalInterfaceBlueprintLibrary.cpp b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CarlaTemplateExternalInterface/Source/CarlaTemplateExternalInterfaceBlueprintLibrary.cpp new file mode 100644 index 00000000000..bac1de37ea1 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CarlaTemplateExternalInterface/Source/CarlaTemplateExternalInterfaceBlueprintLibrary.cpp @@ -0,0 +1,107 @@ +#pragma once + +#include "CarlaTemplateExternalInterfaceBlueprintLibrary.h" +#include "CarlaTemplateExternalInterfaceActor.h" +#include "Carla/Actor/ActorBlueprintFunctionLibrary.h" +#include "Carla.h" +#include "Carla/Server/CarlaServer.h" +#include "Carla/Game/CarlaGameInstance.h" +#include "Carla/Game/CarlaStatics.h" +#include "Carla/Sensor/Sensor.h" + +#if WITH_EDITOR +# define CARLA_ABFL_CHECK_ACTOR(ActorPtr) \ + if (!IsValid(ActorPtr)) \ + { \ + UE_LOG(LogCarlaTemplateExternalInterface, Error, TEXT("Cannot set empty actor!")); \ + return; \ + } +#else +# define CARLA_ABFL_CHECK_ACTOR(ActorPtr) \ + IsValid(ActorPtr); +#endif // WITH_EDITOR + +FActorDefinition UCarlaTemplateExternalInterfaceBlueprintLibrary::MakeCarlaTemplateExternalInterfaceDefinition(TSubclassOf Class) +{ + FActorDefinition Definition; + bool Success; + MakeCarlaTemplateExternalInterfaceDefinition(Success, Definition, Class); + check(Success); + return Definition; +} + +template +static FString JoinStrings(const FString &Separator, ARGS && ... Args) +{ + return FString::Join(TArray{std::forward(Args) ...}, *Separator); +} + +template +static void FillIdAndTags(FActorDefinition &Def, TStrs && ... Strings) +{ + Def.Id = JoinStrings(TEXT("."), std::forward(Strings) ...).ToLower(); + Def.Tags = JoinStrings(TEXT(","), std::forward(Strings) ...).ToLower(); + + // each actor gets an actor role name attribute (empty by default) + FActorVariation ActorRole; + ActorRole.Id = TEXT("role_name"); + ActorRole.Type = EActorAttributeType::String; + ActorRole.RecommendedValues = { TEXT("default") }; + ActorRole.bRestrictToRecommended = false; + Def.Variations.Emplace(ActorRole); + + // ROS2 + FActorVariation Var; + Var.Id = TEXT("ros_name"); + Var.Type = EActorAttributeType::String; + Var.RecommendedValues = { Def.Id }; + Var.bRestrictToRecommended = false; + Def.Variations.Emplace(Var); +} + +void UCarlaTemplateExternalInterfaceBlueprintLibrary::MakeCarlaTemplateExternalInterfaceDefinition( + bool &Success, + FActorDefinition &Definition, + TSubclassOf Class) +{ + FillIdAndTags(Definition, TEXT("sensor"), TEXT("other"), TEXT("template")); + + Definition.Class = Class; + + FActorVariation Tick; + Tick.Id = TEXT("sensor_tick"); + Tick.Type = EActorAttributeType::Float; + Tick.RecommendedValues = { TEXT("0.0") }; + Tick.bRestrictToRecommended = false; + + FActorVariation TemplateActorValue; + TemplateActorValue.Id = TEXT("template_value"); + TemplateActorValue.Type = EActorAttributeType::Float; + TemplateActorValue.RecommendedValues = { TEXT("0.25") }; + TemplateActorValue.bRestrictToRecommended = false; + + Definition.Variations.Append({ + Tick, + TemplateActorValue}); + + Definition.Attributes.Emplace(FActorAttribute{ + TEXT("sensor_type"), + EActorAttributeType::String, + "template"}); + + Success = UActorBlueprintFunctionLibrary::CheckActorDefinition(Definition); +} + +void UCarlaTemplateExternalInterfaceBlueprintLibrary::SetCarlaTemplateExternalInterface( + const FActorDescription &Description, + ACarlaTemplateExternalInterfaceActor *CarlaTemplateExternalInterface) +{ + CARLA_ABFL_CHECK_ACTOR(CarlaTemplateExternalInterface); + + CarlaTemplateExternalInterface->SetTemplateValue( + UActorBlueprintFunctionLibrary::RetrieveActorAttributeToFloat("template_value", Description.Variations, 0.25f) + ); + + CarlaTemplateExternalInterface->OutputName = UActorBlueprintFunctionLibrary::RetrieveActorAttributeToString("ros_name", Description.Variations, "ABC"); + +} \ No newline at end of file diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CarlaTemplateExternalInterface/Source/CarlaTemplateExternalInterfaceBlueprintLibrary.h b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CarlaTemplateExternalInterface/Source/CarlaTemplateExternalInterfaceBlueprintLibrary.h new file mode 100644 index 00000000000..7fadbb62e49 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CarlaTemplateExternalInterface/Source/CarlaTemplateExternalInterfaceBlueprintLibrary.h @@ -0,0 +1,24 @@ +#pragma once + +#include +#include "Kismet/BlueprintFunctionLibrary.h" +#include + +#include "CarlaTemplateExternalInterfaceBlueprintLibrary.generated.h" + +class ACarlaTemplateExternalInterfaceActor; + +UCLASS() +class CARLATEMPLATEEXTERNALINTERFACE_API UCarlaTemplateExternalInterfaceBlueprintLibrary : public UBlueprintFunctionLibrary +{ + GENERATED_BODY() + + public: + UFUNCTION(BlueprintCallable) + static FActorDefinition MakeCarlaTemplateExternalInterfaceDefinition(TSubclassOf Class); + + static void MakeCarlaTemplateExternalInterfaceDefinition(bool &Success, FActorDefinition &Definition, TSubclassOf Class); + + UFUNCTION(BlueprintCallable) + static void SetCarlaTemplateExternalInterface(const FActorDescription &Description, ACarlaTemplateExternalInterfaceActor *CarlaTemplateExternalInterface); +}; \ No newline at end of file diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CarlaTemplateExternalInterface/Source/CarlaTemplateExternalInterfaceGameSubsystem.cpp b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CarlaTemplateExternalInterface/Source/CarlaTemplateExternalInterfaceGameSubsystem.cpp new file mode 100644 index 00000000000..c74275d5579 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CarlaTemplateExternalInterface/Source/CarlaTemplateExternalInterfaceGameSubsystem.cpp @@ -0,0 +1,15 @@ + +#include "CarlaTemplateExternalInterfaceGameSubsystem.h" +#include "CarlaInterface/CarlaActor.h" +#include "ros2/ROS2CarlaTemplate.h" + +void UCarlaTemplateExternalInterfaceSubsystem::Initialize(FSubsystemCollectionBase& Collection) +{ + FCarlaTemplateExternalInterfaceActor::RegisterClassWithFactory(); + carla::ros2::ROS2CarlaTemplate::GetInstance(); +} + +void UCarlaTemplateExternalInterfaceSubsystem::Deinitialize() +{ + +} diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CarlaTemplateExternalInterface/Source/CarlaTemplateExternalInterfaceGameSubsystem.h b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CarlaTemplateExternalInterface/Source/CarlaTemplateExternalInterfaceGameSubsystem.h new file mode 100644 index 00000000000..58baa747fa0 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CarlaTemplateExternalInterface/Source/CarlaTemplateExternalInterfaceGameSubsystem.h @@ -0,0 +1,19 @@ +#pragma once + +#include "Subsystems/GameInstanceSubsystem.h" + +#include "CarlaTemplateExternalInterfaceGameSubsystem.generated.h" + +UCLASS() +class UCarlaTemplateExternalInterfaceSubsystem : public UGameInstanceSubsystem +{ + GENERATED_BODY() +public: + // Begin USubsystem + virtual void Initialize(FSubsystemCollectionBase& Collection) override; + virtual void Deinitialize() override; + // End USubsystem + +private: + // All my variables +}; \ No newline at end of file diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CarlaTemplateExternalInterface/Source/ros2/ROS2CarlaTemplate.cpp b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CarlaTemplateExternalInterface/Source/ros2/ROS2CarlaTemplate.cpp new file mode 100644 index 00000000000..763bb799756 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CarlaTemplateExternalInterface/Source/ros2/ROS2CarlaTemplate.cpp @@ -0,0 +1,133 @@ +// Copyright (c) 2024 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 . + +#include "ROS2CarlaTemplate.h" +#include "carla/Logging.h" +#include "carla/geom/GeoLocation.h" +#include "carla/geom/Vector3D.h" +#include "carla/sensor/data/DVSEvent.h" +#include "carla/sensor/data/LidarData.h" +#include "carla/sensor/data/SemanticLidarData.h" +#include "carla/sensor/data/RadarData.h" +#include "carla/sensor/data/Image.h" +#include "carla/sensor/s11n/ImageSerializer.h" +#include "carla/sensor/s11n/SensorHeaderSerializer.h" + +#include "carla/ros2/publishers/BasicPublisher.h" +#include "carla/ros2/publishers/CarlaPublisher.h" +#include "carla/ros2/publishers/CarlaClockPublisher.h" +#include "carla/ros2/publishers/CarlaTransformPublisher.h" +#include "carla/ros2/publishers/BasicPublisher.h" + + +#include + +namespace carla { +namespace ros2 { + +// static fields +std::shared_ptr ROS2CarlaTemplate::_instance; + +void ROS2CarlaTemplate::Enable(bool enable) { + _enabled = enable; + log_info("ROS2CarlaTemplate enabled: ", _enabled); +} + +void ROS2CarlaTemplate::SetFrame(uint64_t frame) { + _frame = frame; +} + +void ROS2CarlaTemplate::SetTimestamp(double timestamp) { + if (IsEnabled()){ + double integral; + const double fractional = modf(timestamp, &integral); + const double multiplier = 1000000000.0; + _seconds = static_cast(integral); + _nanoseconds = static_cast(fractional * multiplier); + } +} + +void ROS2CarlaTemplate::Shutdown() { + for (auto& element : _publishers) { + element.second.reset(); + } + for (auto& element : _transforms) { + element.second.reset(); + } + _enabled = false; +} + +void ROS2CarlaTemplate::RemoveActorRosPublishers(void *actor) +{ + auto p_it = _publishers.find(actor); + if (p_it != _publishers.end()) { + _publishers.erase(actor); + } + + auto t_it = _transforms.find(actor); + if (t_it != _transforms.end()) { + _transforms.erase(actor); + } +} + +std::pair, std::shared_ptr> ROS2CarlaTemplate::GetOrCreateSensor(carla::streaming::detail::stream_id_type id, void* actor) { + auto it_publishers = _publishers.find(actor); + auto it_transforms = _transforms.find(actor); + std::shared_ptr publisher {}; + std::shared_ptr transform {}; + auto ROS2Interfaces = UActorDispatcher::GetInterfaces(); + if (it_publishers != _publishers.end()) { + publisher = it_publishers->second; + if (it_transforms != _transforms.end()) { + transform = it_transforms->second; + } + } else { + //Sensor not found, creating one of the given type + const std::string string_id = std::to_string(id); + std::string ros_name =ROS2Interfaces->GetActorRosName(actor); + std::string parent_ros_name = ROS2Interfaces->GetActorParentRosName(actor); + if (ros_name == "CarlaTemplateExternalInterface__") { + ros_name.pop_back(); + ros_name.pop_back(); + ros_name += string_id; + ROS2Interfaces->UpdateActorRosName(actor, ros_name); + } + std::shared_ptr new_publisher = std::make_shared(ros_name.c_str(), parent_ros_name.c_str()); + if (new_publisher->Init()) { + _publishers.insert({actor, new_publisher}); + publisher = new_publisher; + } + std::shared_ptr new_transform = std::make_shared(ros_name.c_str(), parent_ros_name.c_str()); + if (new_transform->Init()) { + _transforms.insert({actor, new_transform}); + transform = new_transform; + } + } + return { publisher, transform }; +} + +void ROS2CarlaTemplate::ProcessDataFromTemplate( + carla::streaming::detail::stream_id_type stream_id, + const carla::geom::Transform sensor_transform, + const char* data, + void *actor) { + + log_info("Sensor CarlaTemplateExternalInterfaceSensor to ROS data: frame.", _frame, "stream.", stream_id); + { + auto sensors = GetOrCreateSensor(stream_id, actor); + if (sensors.first) { + sensors.first->SetData(data); + sensors.first->Publish(); + } + if (sensors.second) { + sensors.second->SetData(_seconds, _nanoseconds, (const float*)&sensor_transform.location, (const float*)&sensor_transform.rotation); + sensors.second->Publish(); + } + } +} + +} // namespace ros2 +} // namespace carla diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CarlaTemplateExternalInterface/Source/ros2/ROS2CarlaTemplate.h b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CarlaTemplateExternalInterface/Source/ros2/ROS2CarlaTemplate.h new file mode 100644 index 00000000000..b4e30eb242e --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/CarlaTemplateExternalInterface/Source/ros2/ROS2CarlaTemplate.h @@ -0,0 +1,78 @@ +// Copyright (c) 2024 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 . + +#pragma once + +#include "Carla/Actor/ActorDispatcher.h" +#include "carla/Buffer.h" +#include "carla/BufferView.h" +#include "carla/geom/Transform.h" +#include "carla/ros2/ROS2.h" +#include "carla/ros2/ROS2CallbackData.h" +#include "carla/streaming/detail/Types.h" + +#include +#include +#include +#include + +namespace carla { +namespace ros2 { + +class BasicPublisher; + +class ROS2CarlaTemplate : public ROS2 +{ + public: + + // deleting copy constructor for singleton + ROS2CarlaTemplate(const ROS2CarlaTemplate& obj) = delete; + static std::shared_ptr GetInstance() { + if (!_instance) + { + _instance = std::shared_ptr(new ROS2CarlaTemplate); + auto ROS2Interfaces = UActorDispatcher::GetInterfaces(); + ROS2Interfaces->RegisterInterface(_instance); + } + return _instance; + } + + virtual void Enable(bool enable) override; + virtual void Shutdown() override; + virtual bool IsEnabled() override { return _enabled; } + virtual void SetFrame(uint64_t frame) override; + virtual void SetTimestamp(double timestamp) override; + + // ros_name managing + virtual void RemoveActorRosPublishers(void *actor) override; + + // receiving data to publish + void ProcessDataFromTemplate( + carla::streaming::detail::stream_id_type stream_id, + const carla::geom::Transform sensor_transform, + const char *data, + void *actor = nullptr); + + // enabling streams to publish + virtual void EnableStream(carla::streaming::detail::stream_id_type id) { _publish_stream.insert(id); } + virtual bool IsStreamEnabled(carla::streaming::detail::stream_id_type id) { return _publish_stream.count(id) > 0; } + virtual void ResetStreams() { _publish_stream.clear(); } + + static std::shared_ptr _instance; + + private: + std::pair, std::shared_ptr> GetOrCreateSensor(carla::streaming::detail::stream_id_type id, void* actor); + + // singleton + ROS2CarlaTemplate() {}; + + std::unordered_map> _publishers; + std::unordered_map> _transforms; + std::unordered_set _publish_stream; +}; + +} // namespace ros2 +} // namespace carla diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Content/BP_MultirotorFactory.uasset b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Content/BP_MultirotorFactory.uasset new file mode 100644 index 00000000000..d4877346489 Binary files /dev/null and b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Content/BP_MultirotorFactory.uasset differ diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Content/Blueprints/Vehicles/BaseMultirotor.uasset b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Content/Blueprints/Vehicles/BaseMultirotor.uasset new file mode 100644 index 00000000000..fd9ca1ed469 Binary files /dev/null and b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Content/Blueprints/Vehicles/BaseMultirotor.uasset differ diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Content/Blueprints/Vehicles/Quadrotor/BP_AirSimQuadrotor.uasset b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Content/Blueprints/Vehicles/Quadrotor/BP_AirSimQuadrotor.uasset new file mode 100644 index 00000000000..3eee58b8a6b Binary files /dev/null and b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Content/Blueprints/Vehicles/Quadrotor/BP_AirSimQuadrotor.uasset differ diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Content/Static/Vehicles/MiniQuadCopter/Prop_Red_Plastic.uasset b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Content/Static/Vehicles/MiniQuadCopter/Prop_Red_Plastic.uasset new file mode 100644 index 00000000000..0e32ae5603c Binary files /dev/null and b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Content/Static/Vehicles/MiniQuadCopter/Prop_Red_Plastic.uasset differ diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Content/Static/Vehicles/MiniQuadCopter/Prop_White_Plastic.uasset b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Content/Static/Vehicles/MiniQuadCopter/Prop_White_Plastic.uasset new file mode 100644 index 00000000000..d5b762a7069 Binary files /dev/null and b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Content/Static/Vehicles/MiniQuadCopter/Prop_White_Plastic.uasset differ diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Content/Static/Vehicles/Propeller.uasset b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Content/Static/Vehicles/Propeller.uasset new file mode 100644 index 00000000000..da7bb0dcd7c Binary files /dev/null and b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Content/Static/Vehicles/Propeller.uasset differ diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Content/Static/Vehicles/QuadRotor1/Material.uasset b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Content/Static/Vehicles/QuadRotor1/Material.uasset new file mode 100644 index 00000000000..006abfd1ce5 Binary files /dev/null and b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Content/Static/Vehicles/QuadRotor1/Material.uasset differ diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Content/Static/Vehicles/QuadRotor1/Material_001.uasset b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Content/Static/Vehicles/QuadRotor1/Material_001.uasset new file mode 100644 index 00000000000..e4e8f2b3f44 Binary files /dev/null and b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Content/Static/Vehicles/QuadRotor1/Material_001.uasset differ diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Content/Static/Vehicles/QuadRotor1/Material_002.uasset b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Content/Static/Vehicles/QuadRotor1/Material_002.uasset new file mode 100644 index 00000000000..a8fd99456d7 Binary files /dev/null and b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Content/Static/Vehicles/QuadRotor1/Material_002.uasset differ diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Content/Static/Vehicles/QuadRotor1/Material_004.uasset b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Content/Static/Vehicles/QuadRotor1/Material_004.uasset new file mode 100644 index 00000000000..4efedc1a800 Binary files /dev/null and b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Content/Static/Vehicles/QuadRotor1/Material_004.uasset differ diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Content/Static/Vehicles/QuadRotor1/QuadCopter.uasset b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Content/Static/Vehicles/QuadRotor1/QuadCopter.uasset new file mode 100644 index 00000000000..fd3d5eeb3c6 Binary files /dev/null and b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Content/Static/Vehicles/QuadRotor1/QuadCopter.uasset differ diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Content/Static/Vehicles/QuadRotor1/T_QuadCopter_Body_2.uasset b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Content/Static/Vehicles/QuadRotor1/T_QuadCopter_Body_2.uasset new file mode 100644 index 00000000000..095f212e857 Binary files /dev/null and b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Content/Static/Vehicles/QuadRotor1/T_QuadCopter_Body_2.uasset differ diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Content/Static/Vehicles/QuadRotor1/T_QuadCopter_Carmera.uasset b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Content/Static/Vehicles/QuadRotor1/T_QuadCopter_Carmera.uasset new file mode 100644 index 00000000000..e4ec0fa8d7c Binary files /dev/null and b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Content/Static/Vehicles/QuadRotor1/T_QuadCopter_Carmera.uasset differ diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Content/Static/Vehicles/QuadRotor1/T_QuadCopter_Frame.uasset b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Content/Static/Vehicles/QuadRotor1/T_QuadCopter_Frame.uasset new file mode 100644 index 00000000000..13136bad7ac Binary files /dev/null and b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Content/Static/Vehicles/QuadRotor1/T_QuadCopter_Frame.uasset differ diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Content/Static/Vehicles/QuadRotor1/T_QuadCopter_Prop.uasset b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Content/Static/Vehicles/QuadRotor1/T_QuadCopter_Prop.uasset new file mode 100644 index 00000000000..91f9f8ea51f Binary files /dev/null and b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Content/Static/Vehicles/QuadRotor1/T_QuadCopter_Prop.uasset differ diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Content/Static/Vehicles/Quadrotor1.uasset b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Content/Static/Vehicles/Quadrotor1.uasset new file mode 100644 index 00000000000..2de0200b7dd Binary files /dev/null and b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Content/Static/Vehicles/Quadrotor1.uasset differ diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/FlyingVehicles.uplugin b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/FlyingVehicles.uplugin new file mode 100644 index 00000000000..7e435aaa30b --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/FlyingVehicles.uplugin @@ -0,0 +1,26 @@ +{ + "FileVersion" : 3, + "Version" : "0.0.0", + "VersionName": "0.0.0", + "FriendlyName": "FlyingVehicles", + "Description": "Flying Vehicle Plugin", + "Category" : "Science", + "MarketplaceURL" : "", + "EnabledByDefault" : true, + "CanContainContent": true, + "IsBetaVersion" : true, + "Installed": false, + "Modules": [ + { + "Name": "FlyingVehicles", + "Type": "Runtime", + "LoadingPhase": "Default" + } + ], + "Plugins": [ + { + "Name": "Carla", + "Enabled": true + } + ] +} diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/CarlaInterface/ActorData.cpp b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/CarlaInterface/ActorData.cpp new file mode 100644 index 00000000000..97e7e311827 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/CarlaInterface/ActorData.cpp @@ -0,0 +1,34 @@ +// Copyright (c) 2024 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 . + +#include "ActorData.h" +#include "FlyingVehicles/MultirotorPawn.h" +#include "Carla/Actor/CarlaActor.h" + + +void FMultirotorData::RecordActorData(FCarlaActor* CarlaActor, UCarlaEpisode* CarlaEpisode) +{ + FActorData::RecordActorData(CarlaActor, CarlaEpisode); + AActor* Actor = CarlaActor->GetActor(); + AMultirotorPawn* Multirotor = Cast(Actor); + if (bSimulatePhysics) + { + PhysicsControl = Multirotor->GetMultirotorPhysicsControl(); + } +} + +void FMultirotorData::RestoreActorData(FCarlaActor* CarlaActor, UCarlaEpisode* CarlaEpisode) +{ + FActorData::RestoreActorData(CarlaActor, CarlaEpisode); + AActor* Actor = CarlaActor->GetActor(); + AMultirotorPawn* Multirotor = Cast(Actor); + Multirotor->SetSimulatePhysics(bSimulatePhysics); + if (bSimulatePhysics) + { + Multirotor->ApplyMultirotorPhysicsControl(PhysicsControl); + } + Multirotor->ApplyMultirotorControl(Control); +} diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/CarlaInterface/ActorData.h b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/CarlaInterface/ActorData.h new file mode 100644 index 00000000000..6f6c95310c8 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/CarlaInterface/ActorData.h @@ -0,0 +1,25 @@ +// Copyright (c) 2024 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 . + +#pragma once + +#include "Carla/Actor/ActorData.h" +#include "FlyingVehicles/MultirotorControl.h" +#include "FlyingVehicles/MultirotorPhysicsControl.h" + + +class FMultirotorData : public FActorData +{ +public: + + FMultirotorPhysicsControl PhysicsControl; + + FMultirotorControl Control; + + virtual void RecordActorData(FCarlaActor* CarlaActor, UCarlaEpisode* CarlaEpisode) override; + + virtual void RestoreActorData(FCarlaActor* CarlaActor, UCarlaEpisode* CarlaEpisode) override; +}; \ No newline at end of file diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/CarlaInterface/CarlaActor.cpp b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/CarlaInterface/CarlaActor.cpp new file mode 100644 index 00000000000..54cf7e8c8f0 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/CarlaInterface/CarlaActor.cpp @@ -0,0 +1,118 @@ +// Copyright (c) 2024 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 . + +#include "CarlaActor.h" +#include "FlyingVehicles/MultirotorPawn.h" + +const FString FMultirotorActor::CustomType = "FlyingVehicle_Multirotor"; + +FMultirotorActor::FMultirotorActor( + IdType ActorId, + AActor* Actor, + TSharedPtr Info, + carla::rpc::ActorState InState, + UWorld* World) + : FCarlaActor(ActorId, Actor, Info, InState, World) +{ + Type = ActorType::Custom; + ActorData = MakeShared(); +} + +TSharedPtr FMultirotorActor::CreateInstance( + FCarlaActor::IdType ActorId, + AActor* Actor, + TSharedPtr Info, + carla::rpc::ActorState InState, + UWorld* World) + { + return MakeShared(ActorId, Actor, std::move(Info), InState, World); + } + +void FMultirotorActor::RegisterClassWithFactory() + { + CarlaActorConstructorFactory::Instance().Register(TEXT("FMultirotorActor"), AMultirotorPawn::StaticClass(), CreateInstance); + } + +// FMultirotorActor Functions ---------------- + +ECarlaServerResponse FMultirotorActor::ApplyMultirotorPhysicsControl( + const FMultirotorPhysicsControl& PhysicsControl) +{ + if (IsDormant()) + { + FMultirotorData* lActorData = GetActorData(); + lActorData->PhysicsControl = PhysicsControl; + } + else + { + auto Multirotor = Cast(GetActor()); + if (Multirotor == nullptr) + { + return ECarlaServerResponse::NotAVehicle; + } + + Multirotor->ApplyMultirotorPhysicsControl(PhysicsControl); + } + return ECarlaServerResponse::Success; +} + +ECarlaServerResponse FMultirotorActor::GetMultirotorPhysicsControl(FMultirotorPhysicsControl &PhysicsControl) +{ + if (IsDormant()) + { + FMultirotorData* lActorData = GetActorData(); + PhysicsControl = lActorData->PhysicsControl; + } + else + { + auto Multirotor = Cast(GetActor()); + if (Multirotor == nullptr) + { + return ECarlaServerResponse::NotAVehicle; + } + PhysicsControl = Multirotor->GetMultirotorPhysicsControl(); + } + return ECarlaServerResponse::Success; +} + +ECarlaServerResponse FMultirotorActor::ApplyControlToMultirotor( + const FMultirotorControl& Control, const EVehicleInputPriority& Priority) +{ + if (IsDormant()) + { + FMultirotorData* lActorData = GetActorData(); + lActorData->Control = Control; + } + else + { + auto Multirotor = Cast(GetActor()); + if (Multirotor == nullptr) + { + return ECarlaServerResponse::NotAVehicle; + } + Multirotor->ApplyMultirotorControl(Control); + } + return ECarlaServerResponse::Success; +} + +ECarlaServerResponse FMultirotorActor::GetMultirotorControl(FMultirotorControl& Control) +{ + if (IsDormant()) + { + FMultirotorData* lActorData = GetActorData(); + Control = lActorData->Control; + } + else + { + auto Multirotor = Cast(GetActor()); + if (Multirotor == nullptr) + { + return ECarlaServerResponse::NotAVehicle; + } + Control = Multirotor->GetMultirotorControl(); + } + return ECarlaServerResponse::Success; +} diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/CarlaInterface/CarlaActor.h b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/CarlaInterface/CarlaActor.h new file mode 100644 index 00000000000..32b97821a56 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/CarlaInterface/CarlaActor.h @@ -0,0 +1,45 @@ +// Copyright (c) 2024 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 . + +#pragma once + +#include "Carla/Actor/CarlaActor.h" +#include "Carla/Actor/CarlaActorConstructorFactory.h" + +class FMultirotorActor : public FCarlaActor +{ +public: + FMultirotorActor( + IdType ActorId, + AActor* Actor, + TSharedPtr Info, + carla::rpc::ActorState InState, + UWorld* World); + + static TSharedPtr CreateInstance(FCarlaActor::IdType ActorId, + AActor* Actor, + TSharedPtr Info, + carla::rpc::ActorState InState, + UWorld* World); + + static void RegisterClassWithFactory(); + + virtual ECarlaServerResponse GetMultirotorPhysicsControl(FMultirotorPhysicsControl& PhysicsControl) final; + + virtual ECarlaServerResponse ApplyMultirotorPhysicsControl( + const FMultirotorPhysicsControl& PhysicsControl) final; + + virtual ECarlaServerResponse ApplyControlToMultirotor( + const FMultirotorControl&, const EVehicleInputPriority&) final; + + virtual ECarlaServerResponse GetMultirotorControl(FMultirotorControl&) final; + + protected: + static const FString CustomType; + +}; + + diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/FlyingVehicles.Build.cs b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/FlyingVehicles.Build.cs new file mode 100644 index 00000000000..90920b7beb0 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/FlyingVehicles.Build.cs @@ -0,0 +1,68 @@ +using UnrealBuildTool; +using System.IO; + +public class FlyingVehicles : ModuleRules +{ + public FlyingVehicles(ReadOnlyTargetRules Target) : base(Target) + { + void AddDynamicLibrary(string library) + { + PublicAdditionalLibraries.Add(library); + RuntimeDependencies.Add(library); + PublicDelayLoadDLLs.Add(library); + System.Console.WriteLine("Dynamic Library Added: " + library); + } + + bEnableExceptions = true; + PrivatePCHHeaderFile = "FlyingVehicles.h"; + PCHUsage = ModuleRules.PCHUsageMode.UseExplicitOrSharedPCHs; + bUseRTTI = true; + + PublicDependencyModuleNames.AddRange(new string[] { + "Carla", + "Chaos", + "ChaosVehicles", + "Core", + "CoreUObject", + "Engine", + "Foliage", + "HTTP", + "ImageWriteQueue", + "InputCore", + "Json", + "JsonUtilities", + "Landscape", + "MeshDescription", + "PhysicsCore", + "ProceduralMeshComponent", + "Slate", + "SlateCore", + "StaticMeshDescription", + "RHI", + "Renderer" + }); + + if (Target.Type == TargetType.Editor) + { + PublicDependencyModuleNames.AddRange(new string[] { "UnrealEd" }); + } + + PrivateDependencyModuleNames.AddRange(new string[] { }); + + string PluginSourcePath = Path.GetFullPath(ModuleDirectory); + string PluginBinariesBuildPath = Path.Combine(PluginSourcePath, "..", "..", "..", "..", "..", "..", ".."); + PublicIncludePaths.Add(Path.Combine(PluginBinariesBuildPath, "Build/Ros2Native/install/include")); + + string CarlaPluginBinariesLinuxPath = Path.Combine(PluginSourcePath, "..", "..", "..", "..", "Carla", "Binaries", "Linux"); + AddDynamicLibrary(Path.Combine(CarlaPluginBinariesLinuxPath, "libcarla-ros2-native.so")); + RuntimeDependencies.Add(Path.Combine(CarlaPluginBinariesLinuxPath, "libfoonathan_memory-0.7.3.so")); + RuntimeDependencies.Add(Path.Combine(CarlaPluginBinariesLinuxPath, "libfastcdr.so")); + RuntimeDependencies.Add(Path.Combine(CarlaPluginBinariesLinuxPath, "libfastcdr.so.1")); + RuntimeDependencies.Add(Path.Combine(CarlaPluginBinariesLinuxPath, "libfastcdr.so.1.1.0")); + RuntimeDependencies.Add(Path.Combine(CarlaPluginBinariesLinuxPath, "libfastrtps.so")); + RuntimeDependencies.Add(Path.Combine(CarlaPluginBinariesLinuxPath, "libfastrtps.so.2.11")); + RuntimeDependencies.Add(Path.Combine(CarlaPluginBinariesLinuxPath, "libfastrtps.so.2.11.2")); + + + } +} diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/FlyingVehicles.cpp b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/FlyingVehicles.cpp new file mode 100644 index 00000000000..a9af840575c --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/FlyingVehicles.cpp @@ -0,0 +1,20 @@ +#include "FlyingVehicles.h" + + +#define LOCTEXT_NAMESPACE "FFlyingVehicles" + + +DEFINE_LOG_CATEGORY(LogFlyingVehicles) + +void FFlyingVehicles::StartupModule() +{ +} + +void FFlyingVehicles::ShutdownModule() +{ +} + +#undef LOCTEXT_NAMESPACE + +IMPLEMENT_MODULE(FFlyingVehicles, FlyingVehicles) + diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/FlyingVehicles.h b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/FlyingVehicles.h new file mode 100644 index 00000000000..2b6c23faab4 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/FlyingVehicles.h @@ -0,0 +1,19 @@ + +#pragma once + +#include "Engine.h" +#include "Logging/LogMacros.h" +#include "CoreMinimal.h" +#include "Modules/ModuleManager.h" + +DECLARE_LOG_CATEGORY_EXTERN(LogFlyingVehicles, Log, All); + +class FFlyingVehicles : public IModuleInterface +{ +public: + + /** IModuleInterface implementation */ + virtual void StartupModule() override; + virtual void ShutdownModule() override; +}; + diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/MultirotorBlueprintHelpers.cpp b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/MultirotorBlueprintHelpers.cpp new file mode 100644 index 00000000000..2cd141229ce --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/MultirotorBlueprintHelpers.cpp @@ -0,0 +1,125 @@ +#include "MultirotorBlueprintHelpers.h" + +static void AddRecommendedValuesForActorRoleName( + FActorDefinition &Definition, + TArray &&RecommendedValues) +{ + for (auto &&ActorVariation: Definition.Variations) + { + if (ActorVariation.Id == "role_name") + { + ActorVariation.RecommendedValues = RecommendedValues; + return; + } + } +} + +template +static void FillActorDefinitionArray( + const TArray &ParameterArray, + TArray &Definitions, + Functor Maker) +{ + for (auto &Item : ParameterArray) + { + FActorDefinition Definition; + bool Success = false; + Maker(Item, Success, Definition); + if (Success) + { + Definitions.Emplace(std::move(Definition)); + } + } +} + +template +static void FillIdAndTags(FActorDefinition &Def, TStrs && ... Strings) +{ + Def.Id = JoinStrings(TEXT("."), std::forward(Strings) ...).ToLower(); + Def.Tags = JoinStrings(TEXT(","), std::forward(Strings) ...).ToLower(); + + // each actor gets an actor role name attribute (empty by default) + FActorVariation ActorRole; + ActorRole.Id = TEXT("role_name"); + ActorRole.Type = EActorAttributeType::String; + ActorRole.RecommendedValues = { TEXT("default") }; + ActorRole.bRestrictToRecommended = false; + Def.Variations.Emplace(ActorRole); + + // ROS2 + FActorVariation Var; + Var.Id = TEXT("ros_name"); + Var.Type = EActorAttributeType::String; + Var.RecommendedValues = { Def.Id }; + Var.bRestrictToRecommended = false; + Def.Variations.Emplace(Var); +} + +template +static FString JoinStrings(const FString &Separator, ARGS && ... Args) +{ + return FString::Join(TArray{std::forward(Args) ...}, *Separator); +} + +static FString ColorToFString(const FColor &Color) +{ + return JoinStrings( + TEXT(","), + FString::FromInt(Color.R), + FString::FromInt(Color.G), + FString::FromInt(Color.B)); +} + +void UMultirotorBlueprintHelpers::MakeMultirotorDefinitions( + const TArray &ParameterArray, + TArray &Definitions) +{ + FillActorDefinitionArray(ParameterArray, Definitions, &MakeMultirotorDefinition); +} + +void UMultirotorBlueprintHelpers::MakeMultirotorDefinition( + const FMultirotorParameters &Parameters, + bool &Success, + FActorDefinition &Definition) +{ + /// @todo We need to validate here the params. + FillIdAndTags(Definition, TEXT("multirotor"), Parameters.Make, Parameters.Model); + AddRecommendedValuesForActorRoleName(Definition, + {TEXT("autopilot"), TEXT("scenario"), TEXT("ego_vehicle")}); + Definition.Class = Parameters.Class; + + if (Parameters.RecommendedColors.Num() > 0) + { + FActorVariation Colors; + Colors.Id = TEXT("color"); + Colors.Type = EActorAttributeType::RGBColor; + Colors.bRestrictToRecommended = false; + for (auto &Color : Parameters.RecommendedColors) + { + Colors.RecommendedValues.Emplace(ColorToFString(Color)); + } + Definition.Variations.Emplace(Colors); + } + + Definition.Attributes.Emplace(FActorAttribute{ + TEXT("object_type"), + EActorAttributeType::String, + Parameters.ObjectType}); + + Definition.Attributes.Emplace(FActorAttribute{ + TEXT("number_of_rotors"), + EActorAttributeType::Int, + FString::FromInt(Parameters.NumberOfRotors)}); + + Definition.Attributes.Emplace(FActorAttribute{ + TEXT("generation"), + EActorAttributeType::Int, + FString::FromInt(Parameters.Generation)}); + + Definition.Attributes.Emplace(FActorAttribute{ + TEXT("control_type"), + EActorAttributeType::String, + "multirotor"}); + + Success = UActorBlueprintFunctionLibrary::CheckActorDefinition(Definition); +} diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/MultirotorBlueprintHelpers.h b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/MultirotorBlueprintHelpers.h new file mode 100644 index 00000000000..65805ef061b --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/MultirotorBlueprintHelpers.h @@ -0,0 +1,41 @@ +#pragma once + +#include "CoreMinimal.h" + +#include "Carla/Actor/ActorBlueprintFunctionLibrary.h" + +#include "MultirotorBlueprintHelpers.generated.h" + +UCLASS() +class UMultirotorBlueprintHelpers : public UBlueprintFunctionLibrary +{ + GENERATED_BODY() + +public: + template + static T* GetActorComponent(AActor* actor, FString name) + { + TArray components; + actor->GetComponents(components); + T* found = nullptr; + for (T* component : components) { + if (component->GetName().Compare(name) == 0) { + found = component; + break; + } + } + return found; + }; + + UFUNCTION(Category = "Carla Actor", BlueprintCallable) + static void MakeMultirotorDefinition( + const FMultirotorParameters &Parameters, + bool &Success, + FActorDefinition &Definition); + + UFUNCTION(Category = "Carla Actor", BlueprintCallable) + static void MakeMultirotorDefinitions( + const TArray &ParameterArray, + TArray &Definitions); + +}; diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/MultirotorControl.h b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/MultirotorControl.h new file mode 100644 index 00000000000..575bc07890a --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/MultirotorControl.h @@ -0,0 +1,13 @@ +#pragma once + +#include "MultirotorControl.generated.h" + +USTRUCT(BlueprintType) +struct FLYINGVEHICLES_API FMultirotorControl +{ + GENERATED_BODY() + + UPROPERTY(Category = "Multirotor Control", EditAnywhere, BlueprintReadWrite) + TArray Throttle; // [0, 1] +}; + diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/MultirotorGameSubsystem.cpp b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/MultirotorGameSubsystem.cpp new file mode 100644 index 00000000000..8982492a856 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/MultirotorGameSubsystem.cpp @@ -0,0 +1,16 @@ + +#include "MultirotorGameSubsystem.h" + +#include "FlyingVehicles/CarlaInterface/CarlaActor.h" +#include "FlyingVehicles/ros2/ROS2Multirotor.h" + +void UMultirotorSubsystem::Initialize(FSubsystemCollectionBase& Collection) +{ + FMultirotorActor::RegisterClassWithFactory(); + carla::ros2::ROS2Multirotor::GetInstance(); // will register the instance +} + +void UMultirotorSubsystem::Deinitialize() +{ + +} diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/MultirotorGameSubsystem.h b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/MultirotorGameSubsystem.h new file mode 100644 index 00000000000..4cafd33f3e6 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/MultirotorGameSubsystem.h @@ -0,0 +1,19 @@ +#pragma once + +#include "Subsystems/GameInstanceSubsystem.h" + +#include "MultirotorGameSubsystem.generated.h" + +UCLASS() +class UMultirotorSubsystem : public UGameInstanceSubsystem +{ + GENERATED_BODY() +public: + // Begin USubsystem + virtual void Initialize(FSubsystemCollectionBase& Collection) override; + virtual void Deinitialize() override; + // End USubsystem + +private: + // All my variables +}; \ No newline at end of file diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/MultirotorMovementComponent.cpp b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/MultirotorMovementComponent.cpp new file mode 100644 index 00000000000..e9e41f0d26f --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/MultirotorMovementComponent.cpp @@ -0,0 +1,71 @@ +#include "MultirotorMovementComponent.h" + +#include "MultirotorBlueprintHelpers.h" +#include "MultirotorPawn.h" + +void UMultirotorMovementComponent::Initialize(AMultirotorPawn* Owner) +{ + + check(RotorSetups.Num() > 0); + + Multirotor = Owner; + + auto RootComponent = Cast(Multirotor->GetRootComponent()); + check(RootComponent); + + for (size_t i = 0; i < RotorSetups.Num(); ++i) { + UStaticMeshComponent* RotorMesh = UMultirotorBlueprintHelpers::GetActorComponent(Multirotor, TEXT("Prop") + FString::FromInt(i)); + if (RotorMesh) { + auto ComponentName = TEXT("RotorComponent") + FString::FromInt(i); + URotorPhysics *Rotor = NewObject(this, *ComponentName); + Rotors.Add(Rotor); + auto Location = RotorMesh->GetRelativeLocation(); + Rotor->Initialize(RootComponent, RotorMesh, Location, &RotorSetups[i]); + + } + else { + UE_LOG(LogFlyingVehicles, Warning, TEXT("Error: Could not find Prop%d element"), i); + } + } +} + +void UMultirotorMovementComponent::Shutdown() +{ + Multirotor = nullptr; + Rotors.Empty(); +} + +void UMultirotorMovementComponent::SetRotorInput(const TArray& Throttle) +{ + if (Throttle.Num() != Rotors.Num()) + { + UE_LOG(LogFlyingVehicles, Warning, TEXT("Got rotor command with %d inputs (expected %d)"), Throttle.Num(), Rotors.Num()); + return; + } + + for(size_t i = 0; i < Rotors.Num(); ++i) + { + Rotors[i]->SetThrottle(Throttle[i]); + } +} + +float UMultirotorMovementComponent::GetRotorSpeed(size_t Index) const +{ + if (Index >= Rotors.Num()) { + return 0; + } + + return Rotors[Index]->GetRotorSpeed(); +} + +void UMultirotorMovementComponent::UpdatePhysics() +{ + for (auto Rotor : Rotors) { + if (IsValid(Rotor)) { + Rotor->UpdatePhysics(); + } + else { + UE_LOG(LogFlyingVehicles, Warning, TEXT("Can't update rotor physics, rotor component is not valid")); + } + } +} diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/MultirotorMovementComponent.h b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/MultirotorMovementComponent.h new file mode 100644 index 00000000000..8811fbb1857 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/MultirotorMovementComponent.h @@ -0,0 +1,38 @@ +#pragma once + +#include "RotorPhysics.h" +#include "RotorSetup.h" + +#include "UObject/Object.h" + +#include "MultirotorMovementComponent.generated.h" + +class AMultirotorPawn; + +UCLASS(Blueprintable, meta=(BlueprintSpawnableComponent)) +class FLYINGVEHICLES_API UMultirotorMovementComponent : public UObject +{ + GENERATED_BODY() + +public: + UPROPERTY(Category="Rotor Setup", EditAnywhere, BlueprintReadWrite) + TArray RotorSetups; + + void Initialize(AMultirotorPawn* Owner); + + void Shutdown(); + + void UpdatePhysics(); + + void SetRotorInput(const TArray& Throttle); + + float GetRotorSpeed(size_t Index) const; + +private: + + UPROPERTY() + AMultirotorPawn* Multirotor; + + UPROPERTY() + TArray Rotors; +}; diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/MultirotorParameters.h b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/MultirotorParameters.h new file mode 100644 index 00000000000..06efb69027a --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/MultirotorParameters.h @@ -0,0 +1,33 @@ +#pragma once + +#include "MultirotorPawn.h" + +#include "MultirotorParameters.generated.h" + +USTRUCT(BlueprintType) +struct FLYINGVEHICLES_API FMultirotorParameters +{ + GENERATED_BODY() + + UPROPERTY(EditAnywhere, BlueprintReadWrite) + FString Make; + + UPROPERTY(EditAnywhere, BlueprintReadWrite) + FString Model; + + UPROPERTY(EditAnywhere, BlueprintReadWrite) + TSubclassOf Class; + + UPROPERTY(EditAnywhere, BlueprintReadWrite) + int32 NumberOfRotors = 4; + + UPROPERTY(EditAnywhere, BlueprintReadWrite) + int32 Generation = 0; + + /// (OPTIONAL) Use for custom classification of vehicles. + UPROPERTY(EditAnywhere, BlueprintReadWrite) + FString ObjectType; + + UPROPERTY(EditAnywhere, BlueprintReadWrite) + TArray RecommendedColors; +}; diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/MultirotorPawn.cpp b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/MultirotorPawn.cpp new file mode 100644 index 00000000000..b75b0e1581d --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/MultirotorPawn.cpp @@ -0,0 +1,92 @@ +#include "MultirotorPawn.h" + +#include "Components/StaticMeshComponent.h" +#include "MultirotorMovementComponent.h" + +#include "MultirotorBlueprintHelpers.h" + +#define PI_CONSTANT 3.14159265358979 + +AMultirotorPawn::AMultirotorPawn() +{ + MultirotorMovementComponent = CreateDefaultSubobject(TEXT("MultirotorMovementComponent")); +} + +void AMultirotorPawn::BeginPlay() +{ + Super::BeginPlay(); + + MultirotorMovementComponent->Initialize(this); + + for (auto i = 0; i < 4; ++i) { + rotating_movements_.Add(UMultirotorBlueprintHelpers::GetActorComponent(this, TEXT("Rotation") + FString::FromInt(i))); + } +} + +void AMultirotorPawn::Tick(float DeltaSeconds) +{ + Super::Tick(DeltaSeconds); + + check(IsValid(MultirotorMovementComponent)); + + MultirotorMovementComponent->UpdatePhysics(); + + UpdateRotorSpeeds(); + +} + +void AMultirotorPawn::EndPlay(const EEndPlayReason::Type EndPlayReason) +{ + Super::EndPlay(EndPlayReason); + MultirotorMovementComponent->Shutdown(); + rotating_movements_.Empty(); +} + +FMultirotorControl AMultirotorPawn::GetMultirotorControl() const +{ + FMultirotorControl Control; + TArray Throttle; + for(size_t i = 0; i < rotating_movements_.Num(); ++i) + { + Throttle.Add(MultirotorMovementComponent->GetRotorSpeed(i)); + } + Control.Throttle = Throttle; + return Control; +} + +FMultirotorPhysicsControl AMultirotorPawn::GetMultirotorPhysicsControl() const +{ + FMultirotorPhysicsControl Control; + TArray Rotors = MultirotorMovementComponent->RotorSetups; + Control.Rotors = Rotors; + return Control; +} + + +void AMultirotorPawn::ApplyMultirotorControl(const FMultirotorControl& Control) +{ + MultirotorMovementComponent->SetRotorInput(Control.Throttle); +} + +void AMultirotorPawn::ApplyMultirotorPhysicsControl(const FMultirotorPhysicsControl& Control) +{ + (void)Control; +} + +void AMultirotorPawn::UpdateRotorSpeeds() +{ + for (auto rotor_index = 0; rotor_index < 4; ++rotor_index) { + auto comp = rotating_movements_[rotor_index]; + if (comp != nullptr) { + float rotor_speed = MultirotorMovementComponent->GetRotorSpeed(rotor_index); + comp->RotationRate.Yaw = rotor_speed * 180.0F / PI_CONSTANT * RotatorFactor; + } + } +} + +void AMultirotorPawn::SetSimulatePhysics(bool Enabled) +{ + (void)Enabled; + return; +} + diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/MultirotorPawn.h b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/MultirotorPawn.h new file mode 100644 index 00000000000..e4f34bedfb7 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/MultirotorPawn.h @@ -0,0 +1,48 @@ +#pragma once + +#include "GameFramework/RotatingMovementComponent.h" + +#include "MultirotorControl.h" +#include "MultirotorPhysicsControl.h" + +#include + +#include "MultirotorPawn.generated.h" + + +class UMultirotorMovementComponent; + +UCLASS() +class FLYINGVEHICLES_API AMultirotorPawn : public APawn +{ + GENERATED_BODY() + +public: + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "Debugging") + float RotatorFactor = 1.0f; + + AMultirotorPawn(); + virtual void BeginPlay() override; + virtual void Tick(float DeltaSeconds) override; + virtual void EndPlay(const EEndPlayReason::Type EndPlayReason) override; + + FMultirotorControl GetMultirotorControl() const; + + FMultirotorPhysicsControl GetMultirotorPhysicsControl() const; + + void ApplyMultirotorControl(const FMultirotorControl& Control); + + void ApplyMultirotorPhysicsControl(const FMultirotorPhysicsControl& Control); + + void SetSimulatePhysics(bool Enabled); + +private: + void UpdateRotorSpeeds(); + + UPROPERTY() + TArray rotating_movements_; + + UPROPERTY(Category="Multirotors", Instanced, VisibleAnywhere, BlueprintReadOnly, meta = (AllowPrivateAccess = "true")) + UMultirotorMovementComponent * MultirotorMovementComponent = nullptr; +}; + diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/MultirotorPhysicsControl.h b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/MultirotorPhysicsControl.h new file mode 100644 index 00000000000..93d4accbfc9 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/MultirotorPhysicsControl.h @@ -0,0 +1,15 @@ +#pragma once + +#include "RotorSetup.h" + +#include "MultirotorPhysicsControl.generated.h" + +USTRUCT(BlueprintType) +struct FLYINGVEHICLES_API FMultirotorPhysicsControl +{ + GENERATED_BODY() + + UPROPERTY(Category = "Rotor Setup", EditAnywhere, BlueprintReadWrite) + TArray Rotors; +}; + diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/RotorPhysics.cpp b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/RotorPhysics.cpp new file mode 100644 index 00000000000..470e06f86c8 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/RotorPhysics.cpp @@ -0,0 +1,66 @@ +#include "RotorPhysics.h" + +#include "Math/UnrealMathUtility.h" + +#define PI_CONSTANT 3.14159265358979 + +URotorPhysics::URotorPhysics(const FObjectInitializer& ObjectInitializer) : + Super(ObjectInitializer) +{ + +} +void URotorPhysics::Initialize(UPrimitiveComponent* RootComponent, UStaticMeshComponent* RotorMeshComponent, FVector MeshLocation, FRotorSetup* RotorSetup) +{ + Mesh = RootComponent; + RotorMesh = RotorMeshComponent; + Setup = RotorSetup; + Location = MeshLocation; + + CalculateMaxThrust(); +} + +void URotorPhysics::SetThrottle(float Throttle) +{ + ThrottleSetpoint = FMath::Clamp(Throttle, 0.0F, 1.0F); +} + +float URotorPhysics::GetThrottle() const +{ + return CurrentThrottle; +} + +float URotorPhysics::GetRotorSpeed() const +{ + return CurrentSpeed; +} + +void URotorPhysics::UpdatePhysics() +{ + CurrentThrottle = ThrottleSetpoint; + + const float AirDensityRatio = 1.0F; + const float TurningDirection = Setup->Clockwise ? 1.0F : -1.0F; + + // Compute Thrusts + const float Thrust = CurrentThrottle * MaxThrust; + const float TorqueScaler = CurrentThrottle * MaxTorque * TurningDirection; + CurrentSpeed = sqrt(CurrentThrottle * MaxSpeedSquare) * TurningDirection; + + const float Force = Thrust * AirDensityRatio * 100.0F; + const float Torque = TorqueScaler * AirDensityRatio * 100.0F * 100.0F; + + Mesh->AddForceAtLocationLocal(FVector(0.0F, 0.0F, Force), Location); + RotorMesh->AddTorqueInRadians(FVector(0.0F, 0.0F, Torque)); +} + +void URotorPhysics::CalculateMaxThrust() +{ + const float MaxRevolutionsPerSecond = Setup->MaxRPM / 60.0F; + const float MaxSpeed = MaxRevolutionsPerSecond * 2.0F * PI_CONSTANT; // radians / sec + MaxSpeedSquare = pow(MaxSpeed, 2.0f); + + const float AirDensity = 1.225f; // kg/m^3 + const float NSquared = MaxRevolutionsPerSecond * MaxRevolutionsPerSecond; + MaxThrust = Setup->ThrustCoefficient * AirDensity * NSquared * static_cast(pow(Setup->PropellerDiameter, 4.0F)); + MaxTorque = Setup->TorqueCoefficient * AirDensity * NSquared * static_cast(pow(Setup->PropellerDiameter, 5.0F)) / (2.0F * PI_CONSTANT); +} diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/RotorPhysics.h b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/RotorPhysics.h new file mode 100644 index 00000000000..a3dcdd7a43a --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/RotorPhysics.h @@ -0,0 +1,48 @@ +#pragma once + +#include "RotorSetup.h" + +#include "RotorPhysics.generated.h" + +UCLASS() +class FLYINGVEHICLES_API URotorPhysics : public UObject +{ + GENERATED_BODY() +public: + + URotorPhysics(const FObjectInitializer &ObjectInitializer); + + /// Initialize the physics component. + void Initialize(UPrimitiveComponent* MeshComponent, UStaticMeshComponent* RotorMeshComponent, FVector MeshLocation, FRotorSetup* RotorSetup); + + /// Set the throttle setpoint. + void SetThrottle(float Throttle); + + /// Get the current actual throttle value + float GetThrottle() const; + + /// Get rotor rotation speed in rad/sec + float GetRotorSpeed() const; + + /// Update the rotor's physics + /// Using the given setpoint, update the rotor's physics. + /// Sets forces and torques on the static mesh component. + void UpdatePhysics(); + +private: + void CalculateMaxThrust(); + + UPrimitiveComponent* Mesh = nullptr; + FRotorSetup* Setup = nullptr; + UStaticMeshComponent* RotorMesh = nullptr; + FVector Location; + + float ThrottleSetpoint = 0.0F; + float CurrentThrottle = 0.0F; + + float MaxThrust; + float MaxTorque; + float MaxSpeedSquare; + + float CurrentSpeed; // radians/sec +}; diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/RotorSetup.h b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/RotorSetup.h new file mode 100644 index 00000000000..bd1778987b3 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/RotorSetup.h @@ -0,0 +1,27 @@ +#pragma once + +#include "RotorSetup.generated.h" + +USTRUCT(BlueprintType) +struct FLYINGVEHICLES_API FRotorSetup +{ + GENERATED_BODY() + + UPROPERTY(Category = "Rotor Setup", EditAnywhere, BlueprintReadWrite) + float ThrustCoefficient = 0.109919F; + + UPROPERTY(Category = "Rotor Setup", EditAnywhere, BlueprintReadWrite) + float TorqueCoefficient = 0.040164F; + + UPROPERTY(Category = "Rotor Setup", EditAnywhere, BlueprintReadWrite) + float MaxRPM = 6396.667F; // revolutions per minute + + UPROPERTY(Category = "Rotor Setup", EditAnywhere, BlueprintReadWrite) + float PropellerDiameter = 0.2286F; //diameter in meters, default is for DJI Phantom 2 + + UPROPERTY(Category = "Rotor Setup", EditAnywhere, BlueprintReadWrite) + float PropellerHeight = .01F; //height of cylindrical area when propeller rotates, 1 cm + + UPROPERTY(Category = "Rotor Setup", EditAnywhere, BlueprintReadWrite) + bool Clockwise = true; // If false, spins counter-clockwise +}; diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/ros2/MultirotorROS2Handler.cpp b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/ros2/MultirotorROS2Handler.cpp new file mode 100644 index 00000000000..e992c8235a0 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/ros2/MultirotorROS2Handler.cpp @@ -0,0 +1,27 @@ +// Copyright (c) 2024 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 . + +#include "MultirotorROS2Handler.h" +#include "Carla/Vehicle/CarlaWheeledVehicle.h" +#include "Carla/Vehicle/VehicleControl.h" + +void MultirotorROS2Handler::operator()(carla::ros2::MultirotorControl &Source) +{ + if (!_Actor) return; + AMultirotorPawn *Multirotor = Cast(_Actor); + if (!Multirotor) return; + + // setup control values + FMultirotorControl NewControl; + TArray newThrottle; + newThrottle.SetNumUninitialized(Source.throttle.size()); + for (int i = 0; i < Source.throttle.size(); i++){ + newThrottle[i] = Source.throttle[i]; + } + NewControl.Throttle = newThrottle; + + Multirotor->ApplyMultirotorControl(NewControl); +} diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/ros2/MultirotorROS2Handler.h b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/ros2/MultirotorROS2Handler.h new file mode 100644 index 00000000000..0008ed38dca --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/ros2/MultirotorROS2Handler.h @@ -0,0 +1,25 @@ +// Copyright (c) 2024 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 . + +#pragma once + +#include +#include "carla/ros2/ROS2.h" +#include + +/// visitor class +class MultirotorROS2Handler +{ + public: + MultirotorROS2Handler() = delete; + MultirotorROS2Handler(AActor *Actor, std::string RosName) : _Actor(Actor), _RosName(RosName) {}; + + void operator()(carla::ros2::MultirotorControl &Source); + + private: + AActor *_Actor {nullptr}; + std::string _RosName; +}; diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/ros2/ROS2Multirotor.cpp b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/ros2/ROS2Multirotor.cpp new file mode 100644 index 00000000000..63974bc1c76 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/ros2/ROS2Multirotor.cpp @@ -0,0 +1,93 @@ +// Copyright (c) 2024 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 . + +#include "ROS2Multirotor.h" +#include "carla/Logging.h" +#include "MultirotorROS2Handler.h" + +#include "subscribers/CarlaMultirotorControlSubscriber.h" + + + +namespace carla { +namespace ros2 { + +std::shared_ptr ROS2Multirotor::_instance; + + +void ROS2Multirotor::SetFrame(uint64_t frame) { + _frame = frame; + //log_info("ROS2 new frame: ", _frame); + + for (auto multirotorControllerPair : _multirotorControllers) { + std::shared_ptr multirotorController = multirotorControllerPair.second; + void* actor = multirotorController->GetMultirotor(); + if (multirotorController->IsAlive()) { + if (multirotorController->HasNewMessage()) { + auto it = _multirotor_actor_callbacks.find(actor); + if (it != _multirotor_actor_callbacks.end()) { + MultirotorControl control = multirotorController->GetMessage(); + it->second(actor, control); + } + } + } else { + RemoveMultirotorActorCallback(actor); + } + } +} + +void ROS2Multirotor::RegisterActor(FActorDescription& Description, std::string RosName, void *Actor) +{ + + for (auto &&Attr : Description.Variations) + { + if (Attr.Key == "control_type" && (Attr.Value.Value == "multirotor")) + { + AddMultirotorActorCallback(Actor, RosName, [RosName](void *Actor, carla::ros2::ROS2MultirotorCallbackData Data) -> void + { + AActor *UEActor = static_cast(Actor); + MultirotorROS2Handler Handler(UEActor, RosName); + std::visit(Handler, Data); + }); + } + } +} + +void ROS2Multirotor::RemoveActor(void* Actor) +{ + RemoveActorCallback(Actor); +} + +void ROS2Multirotor::AddMultirotorActorCallback(void* actor, std::string ros_name, MultirotorActorCallback callback) { + _multirotor_actor_callbacks.insert({actor, std::move(callback)}); + + auto newController = std::make_shared(actor, ros_name.c_str()); + _multirotorControllers.insert({actor, newController}); + newController->Init(); +} + +void ROS2Multirotor::RemoveActorCallback(void* actor){ + + auto mIt = _multirotor_actor_callbacks.find(actor); + if (mIt != _multirotor_actor_callbacks.end()) { + RemoveMultirotorActorCallback(actor); + } +} + +void ROS2Multirotor::RemoveMultirotorActorCallback(void* actor) { + _multirotorControllers.erase(actor); + _multirotor_actor_callbacks.erase(actor); +} + + +void ROS2Multirotor::Shutdown() { + _multirotorControllers.clear(); + _enabled = false; + +} + +} // namespace ros2 +} // namespace carla diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/ros2/ROS2Multirotor.h b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/ros2/ROS2Multirotor.h new file mode 100644 index 00000000000..6944e29d442 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/ros2/ROS2Multirotor.h @@ -0,0 +1,70 @@ +// Copyright (c) 2024 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 . + +#pragma once + +#include "Carla/Actor/ActorDispatcher.h" +#include "carla/Buffer.h" +#include "carla/BufferView.h" +#include "carla/geom/Transform.h" +#include "carla/ros2/ROS2.h" +#include "ROS2MultirotorCallbackData.h" +#include "carla/streaming/detail/Types.h" + +#include +#include +#include +#include + + +namespace carla { +namespace ros2 { + + class CarlaMultirotorControlSubscriber; + +class ROS2Multirotor : public ROS2 +{ + public: + + // deleting copy constructor for singleton + ROS2Multirotor(const ROS2Multirotor& obj) = delete; + + static std::shared_ptr GetInstance() { + if (!_instance) + { + _instance = std::shared_ptr(new ROS2Multirotor); + auto ROS2Interfaces = UActorDispatcher::GetInterfaces(); + ROS2Interfaces->RegisterInterface(_instance); + } + return _instance; + } + + // virtual void Enable(bool enable) override; + virtual void Shutdown() override; + // virtual bool IsEnabled() override { return _enabled; } + virtual void SetFrame(uint64_t frame) override; + // virtual void SetTimestamp(double timestamp) override; + virtual void RegisterActor(FActorDescription& Description, std::string RosName, void* Actor) override; + virtual void RemoveActor(void* Actor) override; + + void AddMultirotorActorCallback(void* actor, std::string ros_name, MultirotorActorCallback callback); + void RemoveActorCallback(void* actor); + void RemoveMultirotorActorCallback(void* actor); + + // UPROPERTY(); + static std::shared_ptr _instance; + + private: + + // singleton + ROS2Multirotor() {}; + + std::unordered_map> _multirotorControllers; + std::unordered_map _multirotor_actor_callbacks; +}; + +} // namespace ros2 +} // namespace carla diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/ros2/ROS2MultirotorCallbackData.h b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/ros2/ROS2MultirotorCallbackData.h new file mode 100644 index 00000000000..a9396d3aa4e --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/ros2/ROS2MultirotorCallbackData.h @@ -0,0 +1,25 @@ +// Copyright (c) 2024 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 . + +#pragma once + +#include +#include + +namespace carla { +namespace ros2 { + + struct MultirotorControl + { + std::vector throttle; + }; + + using ROS2MultirotorCallbackData = std::variant; + + using MultirotorActorCallback = std::function; + +} // namespace ros2 +} // namespace carla diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/ros2/listeners/CarlaMultirotorSubscriberListener.cpp b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/ros2/listeners/CarlaMultirotorSubscriberListener.cpp new file mode 100644 index 00000000000..c2044d764b6 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/ros2/listeners/CarlaMultirotorSubscriberListener.cpp @@ -0,0 +1,111 @@ +#include "CarlaMultirotorSubscriberListener.h" +#include + +#include "carla/ros2/plugin-utils/enable-fastdds-include.h" // start fastdds includes +#include +#include +#include +#include +#include "carla/ros2/plugin-utils/disable-fastdds-include.h" // end fastdds includes + +#include "FlyingVehicles/ros2/types/CarlaMultirotorControl.h" +#include "FlyingVehicles/ros2/subscribers/CarlaMultirotorControlSubscriber.h" +#include "carla/ros2/ROS2CallbackData.h" + +namespace carla { +namespace ros2 { + + namespace efd = eprosima::fastdds::dds; + using erc = eprosima::fastrtps::types::ReturnCode_t; + + class CarlaMultirotorSubscriberListenerImpl : public efd::DataReaderListener { + public: + void on_subscription_matched( + efd::DataReader* reader, + const efd::SubscriptionMatchedStatus& info) override; + void on_data_available(efd::DataReader* reader) override; + + int _matched {0}; + bool _first_connected {false}; + CarlaMultirotorControlSubscriber* _owner {nullptr}; + carla_interfaces::msg::CarlaMultirotorControl _message {}; + }; + + void CarlaMultirotorSubscriberListenerImpl::on_subscription_matched(efd::DataReader* reader, const efd::SubscriptionMatchedStatus& info) + { + if (info.current_count_change == 1) { + _matched = info.total_count; + _first_connected = true; + } else if (info.current_count_change == -1) { + _matched = info.total_count; + if (_matched == 0) { + _owner->DestroySubscriber(); + } + } else { + std::cerr << info.current_count_change + << " is not a valid value for PublicationMatchedStatus current count change" << std::endl; + } + } + + void CarlaMultirotorSubscriberListenerImpl::on_data_available(efd::DataReader* reader) + { + efd::SampleInfo info; + eprosima::fastrtps::types::ReturnCode_t rcode = reader->take_next_sample(&_message, &info); + if (rcode == erc::ReturnCodeValue::RETCODE_OK) { + MultirotorControl control; + control.throttle = _message.throttle(); + _owner->ForwardMessage(control); + } + if (rcode == erc::ReturnCodeValue::RETCODE_ERROR) { + std::cerr << "RETCODE_ERROR" << std::endl; + } + if (rcode == erc::ReturnCodeValue::RETCODE_UNSUPPORTED) { + std::cerr << "RETCODE_UNSUPPORTED" << std::endl; + } + if (rcode == erc::ReturnCodeValue::RETCODE_BAD_PARAMETER) { + std::cerr << "RETCODE_BAD_PARAMETER" << std::endl; + } + if (rcode == erc::ReturnCodeValue::RETCODE_PRECONDITION_NOT_MET) { + std::cerr << "RETCODE_PRECONDITION_NOT_MET" << std::endl; + } + if (rcode == erc::ReturnCodeValue::RETCODE_OUT_OF_RESOURCES) { + std::cerr << "RETCODE_OUT_OF_RESOURCES" << std::endl; + } + if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ENABLED) { + std::cerr << "RETCODE_NOT_ENABLED" << std::endl; + } + if (rcode == erc::ReturnCodeValue::RETCODE_IMMUTABLE_POLICY) { + std::cerr << "RETCODE_IMMUTABLE_POLICY" << std::endl; + } + if (rcode == erc::ReturnCodeValue::RETCODE_INCONSISTENT_POLICY) { + std::cerr << "RETCODE_INCONSISTENT_POLICY" << std::endl; + } + if (rcode == erc::ReturnCodeValue::RETCODE_ALREADY_DELETED) { + std::cerr << "RETCODE_ALREADY_DELETED" << std::endl; + } + if (rcode == erc::ReturnCodeValue::RETCODE_TIMEOUT) { + std::cerr << "RETCODE_TIMEOUT" << std::endl; + } + if (rcode == erc::ReturnCodeValue::RETCODE_NO_DATA) { + std::cerr << "RETCODE_NO_DATA" << std::endl; + } + if (rcode == erc::ReturnCodeValue::RETCODE_ILLEGAL_OPERATION) { + std::cerr << "RETCODE_ILLEGAL_OPERATION" << std::endl; + } + if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ALLOWED_BY_SECURITY) { + std::cerr << "RETCODE_NOT_ALLOWED_BY_SECURITY" << std::endl; + } + } + + void CarlaMultirotorSubscriberListener::SetOwner(CarlaMultirotorControlSubscriber* owner) { + _impl->_owner = owner; + } + + CarlaMultirotorSubscriberListener::CarlaMultirotorSubscriberListener(CarlaMultirotorControlSubscriber* owner) : + _impl(std::make_unique()) { + _impl->_owner = owner; + } + + CarlaMultirotorSubscriberListener::~CarlaMultirotorSubscriberListener() {} + +}} diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/ros2/listeners/CarlaMultirotorSubscriberListener.h b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/ros2/listeners/CarlaMultirotorSubscriberListener.h new file mode 100644 index 00000000000..0dfe148c69f --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/ros2/listeners/CarlaMultirotorSubscriberListener.h @@ -0,0 +1,29 @@ +// Copyright (c) 2024 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 . + +#pragma once + +#include + +namespace carla { +namespace ros2 { + + class CarlaMultirotorSubscriberListenerImpl; + class CarlaMultirotorControlSubscriber; + + class CarlaMultirotorSubscriberListener { + public: + CarlaMultirotorSubscriberListener(CarlaMultirotorControlSubscriber* owner); + ~CarlaMultirotorSubscriberListener(); + CarlaMultirotorSubscriberListener(const CarlaMultirotorSubscriberListener&) = delete; + CarlaMultirotorSubscriberListener& operator=(const CarlaMultirotorSubscriberListener&) = delete; + CarlaMultirotorSubscriberListener(CarlaMultirotorSubscriberListener&&) = delete; + CarlaMultirotorSubscriberListener& operator=(CarlaMultirotorSubscriberListener&&) = delete; + + void SetOwner(CarlaMultirotorControlSubscriber* owner); + + std::unique_ptr _impl; + }; +} +} diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/ros2/subscribers/CarlaMultirotorControlSubscriber.cpp b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/ros2/subscribers/CarlaMultirotorControlSubscriber.cpp new file mode 100644 index 00000000000..8f59cd2eedd --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/ros2/subscribers/CarlaMultirotorControlSubscriber.cpp @@ -0,0 +1,243 @@ + +#include "CarlaMultirotorControlSubscriber.h" + +#include "FlyingVehicles/ros2/types/CarlaMultirotorControl.h" +#include "FlyingVehicles/ros2/types/CarlaMultirotorControlPubSubTypes.h" +#include "FlyingVehicles/ros2/listeners/CarlaMultirotorSubscriberListener.h" + +#include "carla/ros2/plugin-utils/enable-fastdds-include.h" // start fastdds includes +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include "carla/ros2/plugin-utils/disable-fastdds-include.h" // end fastdds includes + +namespace carla { +namespace ros2 { + + namespace efd = eprosima::fastdds::dds; + using erc = eprosima::fastrtps::types::ReturnCode_t; + + struct CarlaMultirotorControlSubscriberImpl { + efd::DomainParticipant* _participant { nullptr }; + efd::Subscriber* _subscriber { nullptr }; + efd::Topic* _topic { nullptr }; + efd::DataReader* _datareader { nullptr }; + efd::TypeSupport _type { new carla_interfaces::msg::CarlaMultirotorControlPubSubType() }; + CarlaMultirotorSubscriberListener _listener {nullptr}; + carla_interfaces::msg::CarlaMultirotorControl _event {}; + MultirotorControl _control {}; + bool _new_message {false}; + bool _alive {true}; + void* _multirotor {nullptr}; + }; + + bool CarlaMultirotorControlSubscriber::Init() { + if (_impl->_type == nullptr) { + std::cerr << "Invalid TypeSupport" << std::endl; + return false; + } + + efd::DomainParticipantQos pqos = efd::PARTICIPANT_QOS_DEFAULT; + pqos.name(_name); + auto factory = efd::DomainParticipantFactory::get_instance(); + _impl->_participant = factory->create_participant(0, pqos); + if (_impl->_participant == nullptr) { + std::cerr << "Failed to create DomainParticipant" << std::endl; + return false; + } + _impl->_type.register_type(_impl->_participant); + + efd::SubscriberQos subqos = efd::SUBSCRIBER_QOS_DEFAULT; + _impl->_subscriber = _impl->_participant->create_subscriber(subqos, nullptr); + if (_impl->_subscriber == nullptr) { + std::cerr << "Failed to create Subscriber" << std::endl; + return false; + } + + efd::TopicQos tqos = efd::TOPIC_QOS_DEFAULT; + const std::string base { "rt/carla/" }; + const std::string publisher_type {"/multirotor_control_cmd"}; + std::string topic_name = base; + if (!_parent.empty()) + topic_name += _parent + "/"; + topic_name += _name; + topic_name += publisher_type; + _impl->_topic = _impl->_participant->create_topic(topic_name, _impl->_type->getName(), tqos); + if (_impl->_topic == nullptr) { + std::cerr << "Failed to create Topic" << std::endl; + return false; + } + + efd::DataReaderQos rqos = efd::DATAREADER_QOS_DEFAULT; + efd::DataReaderListener* listener = (efd::DataReaderListener*)_impl->_listener._impl.get(); + _impl->_datareader = _impl->_subscriber->create_datareader(_impl->_topic, rqos, listener); + if (_impl->_datareader == nullptr) { + std::cerr << "Failed to create DataReader" << std::endl; + return false; + } + return true; + } + + bool CarlaMultirotorControlSubscriber::Read() { + efd::SampleInfo info; + eprosima::fastrtps::types::ReturnCode_t rcode = _impl->_datareader->take_next_sample(&_impl->_event, &info); + if (rcode == erc::ReturnCodeValue::RETCODE_OK) { + return true; + } + if (rcode == erc::ReturnCodeValue::RETCODE_ERROR) { + std::cerr << "RETCODE_ERROR" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_UNSUPPORTED) { + std::cerr << "RETCODE_UNSUPPORTED" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_BAD_PARAMETER) { + std::cerr << "RETCODE_BAD_PARAMETER" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_PRECONDITION_NOT_MET) { + std::cerr << "RETCODE_PRECONDITION_NOT_MET" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_OUT_OF_RESOURCES) { + std::cerr << "RETCODE_OUT_OF_RESOURCES" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ENABLED) { + std::cerr << "RETCODE_NOT_ENABLED" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_IMMUTABLE_POLICY) { + std::cerr << "RETCODE_IMMUTABLE_POLICY" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_INCONSISTENT_POLICY) { + std::cerr << "RETCODE_INCONSISTENT_POLICY" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_ALREADY_DELETED) { + std::cerr << "RETCODE_ALREADY_DELETED" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_TIMEOUT) { + std::cerr << "RETCODE_TIMEOUT" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_NO_DATA) { + std::cerr << "RETCODE_NO_DATA" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_ILLEGAL_OPERATION) { + std::cerr << "RETCODE_ILLEGAL_OPERATION" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ALLOWED_BY_SECURITY) { + std::cerr << "RETCODE_NOT_ALLOWED_BY_SECURITY" << std::endl; + return false; + } + std::cerr << "UNKNOWN" << std::endl; + return false; + } + + void CarlaMultirotorControlSubscriber::ForwardMessage(MultirotorControl control) { + _impl->_control = control; + _impl->_new_message = true; + } + + void CarlaMultirotorControlSubscriber::DestroySubscriber() { + _impl->_alive = false; + } + + MultirotorControl CarlaMultirotorControlSubscriber::GetMessage() { + _impl->_new_message = false; + return _impl->_control; + } + + bool CarlaMultirotorControlSubscriber::IsAlive() { + return _impl->_alive; + } + + bool CarlaMultirotorControlSubscriber::HasNewMessage() { + return _impl->_new_message; + } + + void* CarlaMultirotorControlSubscriber::GetMultirotor() { + return _impl->_multirotor; + } + + CarlaMultirotorControlSubscriber::CarlaMultirotorControlSubscriber(void* multirotor, const char* ros_name, const char* parent) : + _impl(std::make_shared()) { + _impl->_listener.SetOwner(this); + _impl->_multirotor = multirotor; + _name = ros_name; + _parent = parent; + } + + CarlaMultirotorControlSubscriber::~CarlaMultirotorControlSubscriber() { + if (!_impl) + return; + + if (_impl->_datareader) + _impl->_subscriber->delete_datareader(_impl->_datareader); + + if (_impl->_subscriber) + _impl->_participant->delete_subscriber(_impl->_subscriber); + + if (_impl->_topic) + _impl->_participant->delete_topic(_impl->_topic); + + if (_impl->_participant) + efd::DomainParticipantFactory::get_instance()->delete_participant(_impl->_participant); + } + + CarlaMultirotorControlSubscriber::CarlaMultirotorControlSubscriber(const CarlaMultirotorControlSubscriber& other) { + _frame_id = other._frame_id; + _name = other._name; + _parent = other._parent; + _impl = other._impl; + _impl->_listener.SetOwner(this); + } + + CarlaMultirotorControlSubscriber& CarlaMultirotorControlSubscriber::operator=(const CarlaMultirotorControlSubscriber& other) { + _frame_id = other._frame_id; + _name = other._name; + _parent = other._parent; + _impl = other._impl; + _impl->_listener.SetOwner(this); + + return *this; + } + + CarlaMultirotorControlSubscriber::CarlaMultirotorControlSubscriber(CarlaMultirotorControlSubscriber&& other) { + _frame_id = std::move(other._frame_id); + _name = std::move(other._name); + _parent = std::move(other._parent); + _impl = std::move(other._impl); + _impl->_listener.SetOwner(this); + } + + CarlaMultirotorControlSubscriber& CarlaMultirotorControlSubscriber::operator=(CarlaMultirotorControlSubscriber&& other) { + _frame_id = std::move(other._frame_id); + _name = std::move(other._name); + _parent = std::move(other._parent); + _impl = std::move(other._impl); + _impl->_listener.SetOwner(this); + + return *this; + } +} +} diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/ros2/subscribers/CarlaMultirotorControlSubscriber.h b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/ros2/subscribers/CarlaMultirotorControlSubscriber.h new file mode 100644 index 00000000000..24710e3c980 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/ros2/subscribers/CarlaMultirotorControlSubscriber.h @@ -0,0 +1,46 @@ +// Copyright (c) 2024 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 . + +#pragma once + +#include +#include + +#include "carla/ros2/subscribers/CarlaSubscriber.h" +#include "FlyingVehicles/ros2/ROS2MultirotorCallbackData.h" + +namespace carla { +namespace ros2 { + + struct CarlaMultirotorControlSubscriberImpl; + + class CarlaMultirotorControlSubscriber : public CarlaSubscriber { + public: + CarlaMultirotorControlSubscriber(void* multirotor, const char* ros_name = "", const char* parent = ""); + ~CarlaMultirotorControlSubscriber(); + CarlaMultirotorControlSubscriber(const CarlaMultirotorControlSubscriber&); + CarlaMultirotorControlSubscriber& operator=(const CarlaMultirotorControlSubscriber&); + CarlaMultirotorControlSubscriber(CarlaMultirotorControlSubscriber&&); + CarlaMultirotorControlSubscriber& operator=(CarlaMultirotorControlSubscriber&&); + + bool HasNewMessage(); + bool IsAlive(); + MultirotorControl GetMessage(); + void* GetMultirotor(); + + bool Init(); + bool Read(); + const char* type() const override { return "Multirotor control"; } + + //Do not call, for internal use only + void ForwardMessage(MultirotorControl control); + void DestroySubscriber(); + private: + void SetData(int32_t seconds, uint32_t nanoseconds, uint32_t actor_id, std::vector&& data); + + private: + std::shared_ptr _impl; + }; +} +} diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/ros2/types/CarlaMultirotorControl.cpp b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/ros2/types/CarlaMultirotorControl.cpp new file mode 100644 index 00000000000..98011122af7 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/ros2/types/CarlaMultirotorControl.cpp @@ -0,0 +1,250 @@ +// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// 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. + +/*! + * @file CarlaMultirotorControl.cpp + * This source file contains the definition of the described types in the IDL file. + * + * This file was generated by the tool gen. + */ + +#ifdef _WIN32 +// Remove linker warning LNK4221 on Visual Studio +namespace { +char dummy; +} // namespace +#endif // _WIN32 + +#include "CarlaMultirotorControl.h" + +#include "carla/ros2/plugin-utils/enable-fastdds-include.h" // start fastdds includes +#include +#include +#include "carla/ros2/plugin-utils/disable-fastdds-include.h" // end fastdds includes + +using namespace eprosima::fastcdr::exception; + +#include + +#define builtin_interfaces_msg_Time_max_cdr_typesize 8ULL; +#define carla_interfaces_msg_CarlaMultirotorControl_max_cdr_typesize 672ULL; +#define std_msgs_msg_Header_max_cdr_typesize 268ULL; +#define builtin_interfaces_msg_Time_max_key_cdr_typesize 0ULL; +#define carla_interfaces_msg_CarlaMultirotorControl_max_key_cdr_typesize 0ULL; +#define std_msgs_msg_Header_max_key_cdr_typesize 0ULL; + +carla_interfaces::msg::CarlaMultirotorControl::CarlaMultirotorControl() +{ + // std_msgs::msg::Header m_header + + // sequence m_throttle + + +} + +carla_interfaces::msg::CarlaMultirotorControl::~CarlaMultirotorControl() +{ + + +} + +carla_interfaces::msg::CarlaMultirotorControl::CarlaMultirotorControl( + const CarlaMultirotorControl& x) +{ + m_header = x.m_header; + m_throttle = x.m_throttle; +} + +carla_interfaces::msg::CarlaMultirotorControl::CarlaMultirotorControl( + CarlaMultirotorControl&& x) noexcept +{ + m_header = std::move(x.m_header); + m_throttle = std::move(x.m_throttle); +} + +carla_interfaces::msg::CarlaMultirotorControl& carla_interfaces::msg::CarlaMultirotorControl::operator =( + const CarlaMultirotorControl& x) +{ + + m_header = x.m_header; + m_throttle = x.m_throttle; + + return *this; +} + +carla_interfaces::msg::CarlaMultirotorControl& carla_interfaces::msg::CarlaMultirotorControl::operator =( + CarlaMultirotorControl&& x) noexcept +{ + + m_header = std::move(x.m_header); + m_throttle = std::move(x.m_throttle); + + return *this; +} + +bool carla_interfaces::msg::CarlaMultirotorControl::operator ==( + const CarlaMultirotorControl& x) const +{ + + return (m_header == x.m_header && m_throttle == x.m_throttle); +} + +bool carla_interfaces::msg::CarlaMultirotorControl::operator !=( + const CarlaMultirotorControl& x) const +{ + return !(*this == x); +} + +size_t carla_interfaces::msg::CarlaMultirotorControl::getMaxCdrSerializedSize( + size_t current_alignment) +{ + static_cast(current_alignment); + return carla_interfaces_msg_CarlaMultirotorControl_max_cdr_typesize; +} + +size_t carla_interfaces::msg::CarlaMultirotorControl::getCdrSerializedSize( + const carla_interfaces::msg::CarlaMultirotorControl& data, + size_t current_alignment) +{ + (void)data; + size_t initial_alignment = current_alignment; + + + current_alignment += std_msgs::msg::Header::getCdrSerializedSize(data.header(), current_alignment); + current_alignment += 4 + eprosima::fastcdr::Cdr::alignment(current_alignment, 4); + + if (data.throttle().size() > 0) + { + current_alignment += (data.throttle().size() * 4) + eprosima::fastcdr::Cdr::alignment(current_alignment, 4); + } + + + + + return current_alignment - initial_alignment; +} + +void carla_interfaces::msg::CarlaMultirotorControl::serialize( + eprosima::fastcdr::Cdr& scdr) const +{ + + scdr << m_header; + scdr << m_throttle; + +} + +void carla_interfaces::msg::CarlaMultirotorControl::deserialize( + eprosima::fastcdr::Cdr& dcdr) +{ + + dcdr >> m_header; + dcdr >> m_throttle; +} + +/*! + * @brief This function copies the value in member header + * @param _header New value to be copied in member header + */ +void carla_interfaces::msg::CarlaMultirotorControl::header( + const std_msgs::msg::Header& _header) +{ + m_header = _header; +} + +/*! + * @brief This function moves the value in member header + * @param _header New value to be moved in member header + */ +void carla_interfaces::msg::CarlaMultirotorControl::header( + std_msgs::msg::Header&& _header) +{ + m_header = std::move(_header); +} + +/*! + * @brief This function returns a constant reference to member header + * @return Constant reference to member header + */ +const std_msgs::msg::Header& carla_interfaces::msg::CarlaMultirotorControl::header() const +{ + return m_header; +} + +/*! + * @brief This function returns a reference to member header + * @return Reference to member header + */ +std_msgs::msg::Header& carla_interfaces::msg::CarlaMultirotorControl::header() +{ + return m_header; +} +/*! + * @brief This function copies the value in member throttle + * @param _throttle New value to be copied in member throttle + */ +void carla_interfaces::msg::CarlaMultirotorControl::throttle( + const std::vector& _throttle) +{ + m_throttle = _throttle; +} + +/*! + * @brief This function moves the value in member throttle + * @param _throttle New value to be moved in member throttle + */ +void carla_interfaces::msg::CarlaMultirotorControl::throttle( + std::vector&& _throttle) +{ + m_throttle = std::move(_throttle); +} + +/*! + * @brief This function returns a constant reference to member throttle + * @return Constant reference to member throttle + */ +const std::vector& carla_interfaces::msg::CarlaMultirotorControl::throttle() const +{ + return m_throttle; +} + +/*! + * @brief This function returns a reference to member throttle + * @return Reference to member throttle + */ +std::vector& carla_interfaces::msg::CarlaMultirotorControl::throttle() +{ + return m_throttle; +} + + +size_t carla_interfaces::msg::CarlaMultirotorControl::getKeyMaxCdrSerializedSize( + size_t current_alignment) +{ + static_cast(current_alignment); + return carla_interfaces_msg_CarlaMultirotorControl_max_key_cdr_typesize; +} + +bool carla_interfaces::msg::CarlaMultirotorControl::isKeyDefined() +{ + return false; +} + +void carla_interfaces::msg::CarlaMultirotorControl::serializeKey( + eprosima::fastcdr::Cdr& scdr) const +{ + (void) scdr; +} + + + diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/ros2/types/CarlaMultirotorControl.h b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/ros2/types/CarlaMultirotorControl.h new file mode 100644 index 00000000000..1778dd6152e --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/ros2/types/CarlaMultirotorControl.h @@ -0,0 +1,249 @@ +// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// 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. + +/*! + * @file CarlaMultirotorControl.h + * This header file contains the declaration of the described types in the IDL file. + * + * This file was generated by the tool gen. + */ + +#ifndef _FAST_DDS_GENERATED_CARLA_INTERFACES_MSG_CARLAMULTIROTORCONTROL_H_ +#define _FAST_DDS_GENERATED_CARLA_INTERFACES_MSG_CARLAMULTIROTORCONTROL_H_ + +#include "carla/ros2/plugin-utils/enable-fastdds-include.h" // start fastdds includes +#include "carla/ros2/types/Header.h" +#include +#include "carla/ros2/plugin-utils/disable-fastdds-include.h" // end fastdds includes + +#include +#include +#include +#include +#include +#include + + +#if defined(_WIN32) +#if defined(EPROSIMA_USER_DLL_EXPORT) +#define eProsima_user_DllExport __declspec( dllexport ) +#else +#define eProsima_user_DllExport +#endif // EPROSIMA_USER_DLL_EXPORT +#else +#define eProsima_user_DllExport +#endif // _WIN32 + +#if defined(_WIN32) +#if defined(EPROSIMA_USER_DLL_EXPORT) +#if defined(CARLAMULTIROTORCONTROL_SOURCE) +#define CARLAMULTIROTORCONTROL_DllAPI __declspec( dllexport ) +#else +#define CARLAMULTIROTORCONTROL_DllAPI __declspec( dllimport ) +#endif // CARLAMULTIROTORCONTROL_SOURCE +#else +#define CARLAMULTIROTORCONTROL_DllAPI +#endif // EPROSIMA_USER_DLL_EXPORT +#else +#define CARLAMULTIROTORCONTROL_DllAPI +#endif // _WIN32 + +namespace eprosima { +namespace fastcdr { +class Cdr; +} // namespace fastcdr +} // namespace eprosima + + +namespace carla_interfaces { + namespace msg { + /*! + * @brief This class represents the structure CarlaMultirotorControl defined by the user in the IDL file. + * @ingroup CarlaMultirotorControl + */ + class CarlaMultirotorControl + { + public: + + /*! + * @brief Default constructor. + */ + eProsima_user_DllExport CarlaMultirotorControl(); + + /*! + * @brief Default destructor. + */ + eProsima_user_DllExport ~CarlaMultirotorControl(); + + /*! + * @brief Copy constructor. + * @param x Reference to the object carla_interfaces::msg::CarlaMultirotorControl that will be copied. + */ + eProsima_user_DllExport CarlaMultirotorControl( + const CarlaMultirotorControl& x); + + /*! + * @brief Move constructor. + * @param x Reference to the object carla_interfaces::msg::CarlaMultirotorControl that will be copied. + */ + eProsima_user_DllExport CarlaMultirotorControl( + CarlaMultirotorControl&& x) noexcept; + + /*! + * @brief Copy assignment. + * @param x Reference to the object carla_interfaces::msg::CarlaMultirotorControl that will be copied. + */ + eProsima_user_DllExport CarlaMultirotorControl& operator =( + const CarlaMultirotorControl& x); + + /*! + * @brief Move assignment. + * @param x Reference to the object carla_interfaces::msg::CarlaMultirotorControl that will be copied. + */ + eProsima_user_DllExport CarlaMultirotorControl& operator =( + CarlaMultirotorControl&& x) noexcept; + + /*! + * @brief Comparison operator. + * @param x carla_interfaces::msg::CarlaMultirotorControl object to compare. + */ + eProsima_user_DllExport bool operator ==( + const CarlaMultirotorControl& x) const; + + /*! + * @brief Comparison operator. + * @param x carla_interfaces::msg::CarlaMultirotorControl object to compare. + */ + eProsima_user_DllExport bool operator !=( + const CarlaMultirotorControl& x) const; + + /*! + * @brief This function copies the value in member header + * @param _header New value to be copied in member header + */ + eProsima_user_DllExport void header( + const std_msgs::msg::Header& _header); + + /*! + * @brief This function moves the value in member header + * @param _header New value to be moved in member header + */ + eProsima_user_DllExport void header( + std_msgs::msg::Header&& _header); + + /*! + * @brief This function returns a constant reference to member header + * @return Constant reference to member header + */ + eProsima_user_DllExport const std_msgs::msg::Header& header() const; + + /*! + * @brief This function returns a reference to member header + * @return Reference to member header + */ + eProsima_user_DllExport std_msgs::msg::Header& header(); + /*! + * @brief This function copies the value in member throttle + * @param _throttle New value to be copied in member throttle + */ + eProsima_user_DllExport void throttle( + const std::vector& _throttle); + + /*! + * @brief This function moves the value in member throttle + * @param _throttle New value to be moved in member throttle + */ + eProsima_user_DllExport void throttle( + std::vector&& _throttle); + + /*! + * @brief This function returns a constant reference to member throttle + * @return Constant reference to member throttle + */ + eProsima_user_DllExport const std::vector& throttle() const; + + /*! + * @brief This function returns a reference to member throttle + * @return Reference to member throttle + */ + eProsima_user_DllExport std::vector& throttle(); + + /*! + * @brief This function returns the maximum serialized size of an object + * depending on the buffer alignment. + * @param current_alignment Buffer alignment. + * @return Maximum serialized size. + */ + eProsima_user_DllExport static size_t getMaxCdrSerializedSize( + size_t current_alignment = 0); + + /*! + * @brief This function returns the serialized size of a data depending on the buffer alignment. + * @param data Data which is calculated its serialized size. + * @param current_alignment Buffer alignment. + * @return Serialized size. + */ + eProsima_user_DllExport static size_t getCdrSerializedSize( + const carla_interfaces::msg::CarlaMultirotorControl& data, + size_t current_alignment = 0); + + + /*! + * @brief This function serializes an object using CDR serialization. + * @param cdr CDR serialization object. + */ + eProsima_user_DllExport void serialize( + eprosima::fastcdr::Cdr& cdr) const; + + /*! + * @brief This function deserializes an object using CDR serialization. + * @param cdr CDR serialization object. + */ + eProsima_user_DllExport void deserialize( + eprosima::fastcdr::Cdr& cdr); + + + + /*! + * @brief This function returns the maximum serialized size of the Key of an object + * depending on the buffer alignment. + * @param current_alignment Buffer alignment. + * @return Maximum serialized size. + */ + eProsima_user_DllExport static size_t getKeyMaxCdrSerializedSize( + size_t current_alignment = 0); + + /*! + * @brief This function tells you if the Key has been defined for this type + */ + eProsima_user_DllExport static bool isKeyDefined(); + + /*! + * @brief This function serializes the key members of an object using CDR serialization. + * @param cdr CDR serialization object. + */ + eProsima_user_DllExport void serializeKey( + eprosima::fastcdr::Cdr& cdr) const; + + private: + + std_msgs::msg::Header m_header; + std::vector m_throttle; + + }; + } // namespace msg +} // namespace carla_interfaces + +#endif // _FAST_DDS_GENERATED_CARLA_INTERFACES_MSG_CARLAMULTIROTORCONTROL_H_ + diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/ros2/types/CarlaMultirotorControlPubSubTypes.cpp b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/ros2/types/CarlaMultirotorControlPubSubTypes.cpp new file mode 100644 index 00000000000..21b248cd153 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/ros2/types/CarlaMultirotorControlPubSubTypes.cpp @@ -0,0 +1,179 @@ +// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// 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. + +/*! + * @file CarlaMultirotorControlPubSubTypes.cpp + * This header file contains the implementation of the serialization functions. + * + * This file was generated by the tool fastcdrgen. + */ + +#include "carla/ros2/plugin-utils/enable-fastdds-include.h" // start fastdds includes +#include +#include +#include "carla/ros2/plugin-utils/disable-fastdds-include.h" // end fastdds includes + +#include "CarlaMultirotorControlPubSubTypes.h" + +using SerializedPayload_t = eprosima::fastrtps::rtps::SerializedPayload_t; +using InstanceHandle_t = eprosima::fastrtps::rtps::InstanceHandle_t; + +namespace carla_interfaces { + namespace msg { + CarlaMultirotorControlPubSubType::CarlaMultirotorControlPubSubType() + { + setName("carla_interfaces::msg::dds_::CarlaMultirotorControl_"); + auto type_size = CarlaMultirotorControl::getMaxCdrSerializedSize(); + type_size += eprosima::fastcdr::Cdr::alignment(type_size, 4); /* possible submessage alignment */ + m_typeSize = static_cast(type_size) + 4; /*encapsulation*/ + m_isGetKeyDefined = CarlaMultirotorControl::isKeyDefined(); + size_t keyLength = CarlaMultirotorControl::getKeyMaxCdrSerializedSize() > 16 ? + CarlaMultirotorControl::getKeyMaxCdrSerializedSize() : 16; + m_keyBuffer = reinterpret_cast(malloc(keyLength)); + memset(m_keyBuffer, 0, keyLength); + } + + CarlaMultirotorControlPubSubType::~CarlaMultirotorControlPubSubType() + { + if (m_keyBuffer != nullptr) + { + free(m_keyBuffer); + } + } + + bool CarlaMultirotorControlPubSubType::serialize( + void* data, + SerializedPayload_t* payload) + { + CarlaMultirotorControl* p_type = static_cast(data); + + // Object that manages the raw buffer. + eprosima::fastcdr::FastBuffer fastbuffer(reinterpret_cast(payload->data), payload->max_size); + // Object that serializes the data. + eprosima::fastcdr::Cdr ser(fastbuffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, eprosima::fastcdr::Cdr::DDS_CDR); + payload->encapsulation = ser.endianness() == eprosima::fastcdr::Cdr::BIG_ENDIANNESS ? CDR_BE : CDR_LE; + + try + { + // Serialize encapsulation + ser.serialize_encapsulation(); + // Serialize the object. + p_type->serialize(ser); + } + catch (eprosima::fastcdr::exception::Exception& /*exception*/) + { + return false; + } + + // Get the serialized length + payload->length = static_cast(ser.getSerializedDataLength()); + return true; + } + + bool CarlaMultirotorControlPubSubType::deserialize( + SerializedPayload_t* payload, + void* data) + { + try + { + // Convert DATA to pointer of your type + CarlaMultirotorControl* p_type = static_cast(data); + + // Object that manages the raw buffer. + eprosima::fastcdr::FastBuffer fastbuffer(reinterpret_cast(payload->data), payload->length); + + // Object that deserializes the data. + eprosima::fastcdr::Cdr deser(fastbuffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, eprosima::fastcdr::Cdr::DDS_CDR); + + // Deserialize encapsulation. + deser.read_encapsulation(); + payload->encapsulation = deser.endianness() == eprosima::fastcdr::Cdr::BIG_ENDIANNESS ? CDR_BE : CDR_LE; + + // Deserialize the object. + p_type->deserialize(deser); + } + catch (eprosima::fastcdr::exception::Exception& /*exception*/) + { + return false; + } + + return true; + } + + std::function CarlaMultirotorControlPubSubType::getSerializedSizeProvider( + void* data) + { + return [data]() -> uint32_t + { + return static_cast(type::getCdrSerializedSize(*static_cast(data))) + + 4u /*encapsulation*/; + }; + } + + void* CarlaMultirotorControlPubSubType::createData() + { + return reinterpret_cast(new CarlaMultirotorControl()); + } + + void CarlaMultirotorControlPubSubType::deleteData( + void* data) + { + delete(reinterpret_cast(data)); + } + + bool CarlaMultirotorControlPubSubType::getKey( + void* data, + InstanceHandle_t* handle, + bool force_md5) + { + if (!m_isGetKeyDefined) + { + return false; + } + + CarlaMultirotorControl* p_type = static_cast(data); + + // Object that manages the raw buffer. + eprosima::fastcdr::FastBuffer fastbuffer(reinterpret_cast(m_keyBuffer), + CarlaMultirotorControl::getKeyMaxCdrSerializedSize()); + + // Object that serializes the data. + eprosima::fastcdr::Cdr ser(fastbuffer, eprosima::fastcdr::Cdr::BIG_ENDIANNESS); + p_type->serializeKey(ser); + if (force_md5 || CarlaMultirotorControl::getKeyMaxCdrSerializedSize() > 16) + { + m_md5.init(); + m_md5.update(m_keyBuffer, static_cast(ser.getSerializedDataLength())); + m_md5.finalize(); + for (uint8_t i = 0; i < 16; ++i) + { + handle->value[i] = m_md5.digest[i]; + } + } + else + { + for (uint8_t i = 0; i < 16; ++i) + { + handle->value[i] = m_keyBuffer[i]; + } + } + return true; + } + + + } //End of namespace msg + + +} //End of namespace carla_interfaces + diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/ros2/types/CarlaMultirotorControlPubSubTypes.h b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/ros2/types/CarlaMultirotorControlPubSubTypes.h new file mode 100644 index 00000000000..5e56c7dd366 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/FlyingVehicles/Source/FlyingVehicles/ros2/types/CarlaMultirotorControlPubSubTypes.h @@ -0,0 +1,115 @@ +// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// 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. + +/*! + * @file CarlaMultirotorControlPubSubTypes.h + * This header file contains the declaration of the serialization functions. + * + * This file was generated by the tool fastcdrgen. + */ + + +#ifndef _FAST_DDS_GENERATED_CARLA_INTERFACES_MSG_CARLAMULTIROTORCONTROL_PUBSUBTYPES_H_ +#define _FAST_DDS_GENERATED_CARLA_INTERFACES_MSG_CARLAMULTIROTORCONTROL_PUBSUBTYPES_H_ + +#include "carla/ros2/plugin-utils/enable-fastdds-include.h" // start fastdds includes +#include +#include +#include "carla/ros2/types/HeaderPubSubTypes.h" +#include "carla/ros2/plugin-utils/disable-fastdds-include.h" // end fastdds includes + +#include "CarlaMultirotorControl.h" + + + +#if !defined(GEN_API_VER) || (GEN_API_VER != 1) +#error \ + Generated CarlaMultirotorControl is not compatible with current installed Fast DDS. Please, regenerate it with fastddsgen. +#endif // GEN_API_VER + +namespace carla_interfaces +{ + namespace msg + { + + /*! + * @brief This class represents the TopicDataType of the type CarlaMultirotorControl defined by the user in the IDL file. + * @ingroup CarlaMultirotorControl + */ + class CarlaMultirotorControlPubSubType : public eprosima::fastdds::dds::TopicDataType + { + public: + + typedef CarlaMultirotorControl type; + + eProsima_user_DllExport CarlaMultirotorControlPubSubType(); + + eProsima_user_DllExport virtual ~CarlaMultirotorControlPubSubType() override; + + eProsima_user_DllExport virtual bool serialize( + void* data, + eprosima::fastrtps::rtps::SerializedPayload_t* payload) override; + + eProsima_user_DllExport virtual bool deserialize( + eprosima::fastrtps::rtps::SerializedPayload_t* payload, + void* data) override; + + eProsima_user_DllExport virtual std::function getSerializedSizeProvider( + void* data) override; + + eProsima_user_DllExport virtual bool getKey( + void* data, + eprosima::fastrtps::rtps::InstanceHandle_t* ihandle, + bool force_md5 = false) override; + + eProsima_user_DllExport virtual void* createData() override; + + eProsima_user_DllExport virtual void deleteData( + void* data) override; + + #ifdef TOPIC_DATA_TYPE_API_HAS_IS_BOUNDED + eProsima_user_DllExport inline bool is_bounded() const override + { + return false; + } + + #endif // TOPIC_DATA_TYPE_API_HAS_IS_BOUNDED + + #ifdef TOPIC_DATA_TYPE_API_HAS_IS_PLAIN + eProsima_user_DllExport inline bool is_plain() const override + { + return false; + } + + #endif // TOPIC_DATA_TYPE_API_HAS_IS_PLAIN + + #ifdef TOPIC_DATA_TYPE_API_HAS_CONSTRUCT_SAMPLE + eProsima_user_DllExport inline bool construct_sample( + void* memory) const override + { + (void)memory; + return false; + } + + #endif // TOPIC_DATA_TYPE_API_HAS_CONSTRUCT_SAMPLE + + MD5 m_md5; + unsigned char* m_keyBuffer; + + }; + } +} + +#endif // _FAST_DDS_GENERATED_CARLA_INTERFACES_MSG_CARLAMULTIROTORCONTROL_PUBSUBTYPES_H_ + diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/RosUtils/RosUtils.uplugin b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/RosUtils/RosUtils.uplugin new file mode 100644 index 00000000000..ee338faf4d5 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/RosUtils/RosUtils.uplugin @@ -0,0 +1,26 @@ +{ + "FileVersion" : 3, + "Version" : "0.0.0", + "VersionName": "0.0.0", + "FriendlyName": "RosUtils", + "Description": "RosUtils Plugin", + "Category" : "Science", + "MarketplaceURL" : "", + "EnabledByDefault" : true, + "CanContainContent": true, + "IsBetaVersion" : true, + "Installed": false, + "Modules": [ + { + "Name": "RosUtils", + "Type": "Runtime", + "LoadingPhase": "Default" + } + ], + "Plugins": [ + { + "Name": "Carla", + "Enabled": true + } + ] +} diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/RosUtils/Source/RosUtils/RosMessageConversions.cpp b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/RosUtils/Source/RosUtils/RosMessageConversions.cpp new file mode 100644 index 00000000000..bc54e3ca70e --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/RosUtils/Source/RosUtils/RosMessageConversions.cpp @@ -0,0 +1,46 @@ +#include "RosMessageConversions.h" + +#include "carla/ros2/plugin-utils/enable-fastdds-include.h" // start fastdds includes +#include "carla/ros2/types/Header.h" +#include "carla/ros2/types/Transform.h" +#include "carla/ros2/types/Vector3.h" +#include "carla/ros2/types/Twist.h" +#include "carla/ros2/types/Point.h" +#include "carla/ros2/types/Quaternion.h" +#include "carla/ros2/plugin-utils/disable-fastdds-include.h" // end fastdds includes + +#include "StructDefinitions.h" + +FTransform FRosMessageConversions::RosToUnrealTransform(const geometry_msgs::msg::Transform& RosTransform) { + auto RosTranslate = RosTransform.translation(); + auto RosQuat = RosTransform.rotation(); + return FTransform{ + FRotator{FQuat(RosQuat.x(), RosQuat.y(), RosQuat.z(), RosQuat.w())}, + FVector{RosTranslate.x(), RosTranslate.y(), RosTranslate.z()}, + FVector{1.0, 1.0, 1.0} // scale + }; +} + +FTransform FRosMessageConversions::RosToUnrealTransform(const geometry_msgs::msg::Transform& RosTransform, const geometry_msgs::msg::Vector3& RosScale) { + FTransform Transform = RosToUnrealTransform(RosTransform); + Transform.SetScale3D(FVector{RosScale.x(), RosScale.y(), RosScale.z()}); + return Transform; +} + +FVector FRosMessageConversions::RosToUnrealPoint(const geometry_msgs::msg::Point& RosPoint){ + return FVector{RosPoint.x(), RosPoint.y(), RosPoint.z()}; +} + +FQuat FRosMessageConversions::RosToUnrealQuaternion(const geometry_msgs::msg::Quaternion& RosQuat){ + return FQuat(RosQuat.x(), RosQuat.y(), RosQuat.z(), RosQuat.w()); +} + +FTwist FRosMessageConversions::RosToUnrealTwist(const geometry_msgs::msg::Twist& RosTwist){ + FTwist Output; + Output.Linear = FVector{RosTwist.linear().x(), RosTwist.linear().y(), RosTwist.linear().z()}; + Output.Angular = FVector{RosTwist.angular().x(), RosTwist.angular().y(), RosTwist.angular().z()}; + + return Output; +} + + diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/RosUtils/Source/RosUtils/RosMessageConversions.h b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/RosUtils/Source/RosUtils/RosMessageConversions.h new file mode 100644 index 00000000000..82eaff482dd --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/RosUtils/Source/RosUtils/RosMessageConversions.h @@ -0,0 +1,35 @@ + +#pragma once + +#include "Engine.h" +#include "Logging/LogMacros.h" +#include "CoreMinimal.h" +#include "Modules/ModuleManager.h" + +struct FTwist; + +namespace geometry_msgs { + namespace msg { + class Transform; + class Vector3; + class Twist; + class Point; + class Quaternion; + } +} + + +class ROSUTILS_API FRosMessageConversions : public UBlueprintFunctionLibrary +{ +public: + + static FTransform RosToUnrealTransform(const geometry_msgs::msg::Transform& RosTransform); + static FTransform RosToUnrealTransform(const geometry_msgs::msg::Transform& RosTransform, const geometry_msgs::msg::Vector3& RosScale); + + static FVector RosToUnrealPoint(const geometry_msgs::msg::Point& RosPoint); + + static FQuat RosToUnrealQuaternion(const geometry_msgs::msg::Quaternion& RosQuat); + + static FTwist RosToUnrealTwist(const geometry_msgs::msg::Twist& RosTransform); +}; + diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/RosUtils/Source/RosUtils/RosUtils.Build.cs b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/RosUtils/Source/RosUtils/RosUtils.Build.cs new file mode 100644 index 00000000000..a3d9cb35bc5 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/RosUtils/Source/RosUtils/RosUtils.Build.cs @@ -0,0 +1,68 @@ +using UnrealBuildTool; +using System.IO; + +public class RosUtils : ModuleRules +{ + public RosUtils(ReadOnlyTargetRules Target) : base(Target) + { + void AddDynamicLibrary(string library) + { + PublicAdditionalLibraries.Add(library); + RuntimeDependencies.Add(library); + PublicDelayLoadDLLs.Add(library); + System.Console.WriteLine("Dynamic Library Added: " + library); + } + + bEnableExceptions = true; + PrivatePCHHeaderFile = "RosUtils.h"; + PCHUsage = ModuleRules.PCHUsageMode.UseExplicitOrSharedPCHs; + bUseRTTI = true; + + PublicDependencyModuleNames.AddRange(new string[] { + "Carla", + "Chaos", + "ChaosVehicles", + "Core", + "CoreUObject", + "Engine", + "Foliage", + "HTTP", + "ImageWriteQueue", + "InputCore", + "Json", + "JsonUtilities", + "Landscape", + "MeshDescription", + "PhysicsCore", + "ProceduralMeshComponent", + "Slate", + "SlateCore", + "StaticMeshDescription", + "RHI", + "Renderer" + }); + + if (Target.Type == TargetType.Editor) + { + PublicDependencyModuleNames.AddRange(new string[] { "UnrealEd" }); + } + + PrivateDependencyModuleNames.AddRange(new string[] { }); + + string PluginSourcePath = Path.GetFullPath(ModuleDirectory); + string PluginBinariesBuildPath = Path.Combine(PluginSourcePath, "..", "..", "..", "..", "..", "..", ".."); + PublicIncludePaths.Add(Path.Combine(PluginBinariesBuildPath, "Build/Ros2Native/install/include")); + + string CarlaPluginBinariesLinuxPath = Path.Combine(PluginSourcePath, "..", "..", "..", "..", "Carla", "Binaries", "Linux"); + AddDynamicLibrary(Path.Combine(CarlaPluginBinariesLinuxPath, "libcarla-ros2-native.so")); + RuntimeDependencies.Add(Path.Combine(CarlaPluginBinariesLinuxPath, "libfoonathan_memory-0.7.3.so")); + RuntimeDependencies.Add(Path.Combine(CarlaPluginBinariesLinuxPath, "libfastcdr.so")); + RuntimeDependencies.Add(Path.Combine(CarlaPluginBinariesLinuxPath, "libfastcdr.so.1")); + RuntimeDependencies.Add(Path.Combine(CarlaPluginBinariesLinuxPath, "libfastcdr.so.1.1.0")); + RuntimeDependencies.Add(Path.Combine(CarlaPluginBinariesLinuxPath, "libfastrtps.so")); + RuntimeDependencies.Add(Path.Combine(CarlaPluginBinariesLinuxPath, "libfastrtps.so.2.11")); + RuntimeDependencies.Add(Path.Combine(CarlaPluginBinariesLinuxPath, "libfastrtps.so.2.11.2")); + + + } +} diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/RosUtils/Source/RosUtils/RosUtils.cpp b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/RosUtils/Source/RosUtils/RosUtils.cpp new file mode 100644 index 00000000000..489e7921757 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/RosUtils/Source/RosUtils/RosUtils.cpp @@ -0,0 +1,28 @@ +#include "RosUtils.h" + +#include "carla/ros2/plugin-utils/enable-fastdds-include.h" // start fastdds includes +#include "carla/ros2/types/Header.h" +#include "carla/ros2/types/Transform.h" +#include "carla/ros2/types/Vector3.h" +#include "carla/ros2/types/Twist.h" +#include "carla/ros2/plugin-utils/disable-fastdds-include.h" // end fastdds includes + +#include "StructDefinitions.h" + +#define LOCTEXT_NAMESPACE "FRosUtils" + + +DEFINE_LOG_CATEGORY(LogRosUtils) + +void FRosUtils::StartupModule() +{ +} + +void FRosUtils::ShutdownModule() +{ +} + +#undef LOCTEXT_NAMESPACE + +IMPLEMENT_MODULE(FRosUtils, RosUtils) + diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/RosUtils/Source/RosUtils/RosUtils.h b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/RosUtils/Source/RosUtils/RosUtils.h new file mode 100644 index 00000000000..f7e90f5a8e6 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/RosUtils/Source/RosUtils/RosUtils.h @@ -0,0 +1,20 @@ + +#pragma once + +#include "Engine.h" +#include "Logging/LogMacros.h" +#include "CoreMinimal.h" +#include "Modules/ModuleManager.h" + +DECLARE_LOG_CATEGORY_EXTERN(LogRosUtils, Log, All); + +class FRosUtils : public IModuleInterface +{ +public: + + /** IModuleInterface implementation */ + virtual void StartupModule() override; + virtual void ShutdownModule() override; + +}; + diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/RosUtils/Source/RosUtils/StructDefinitions.h b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/RosUtils/Source/RosUtils/StructDefinitions.h new file mode 100644 index 00000000000..7c7acfffe11 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/RosUtils/Source/RosUtils/StructDefinitions.h @@ -0,0 +1,24 @@ + +#pragma once + +#include "Engine.h" +#include "Logging/LogMacros.h" +#include "CoreMinimal.h" +#include "Modules/ModuleManager.h" + +#include "StructDefinitions.generated.h" + +USTRUCT(BlueprintType) +struct FTwist +{ + GENERATED_BODY() + + UPROPERTY(BlueprintReadWrite) + FVector Linear; + + UPROPERTY(BlueprintReadWrite) + FVector Angular; + + FTwist(){} + +}; \ No newline at end of file diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/RosUtils/Source/RosUtils/ros2/SubscriberListener.h b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/RosUtils/Source/RosUtils/ros2/SubscriberListener.h new file mode 100644 index 00000000000..5e1d87fb54f --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/RosUtils/Source/RosUtils/ros2/SubscriberListener.h @@ -0,0 +1,35 @@ +// Copyright (c) 2024 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 . + +#pragma once + +#include + +namespace carla { +namespace ros2 { + + template + class SubscriberListenerImpl; + + template + class TopicSubscription; + + template + class ROSUTILS_API SubscriberListener { + public: + SubscriberListener(TopicSubscription* owner); + ~SubscriberListener(); + SubscriberListener(const SubscriberListener&) = delete; + SubscriberListener& operator=(const SubscriberListener&) = delete; + SubscriberListener(SubscriberListener&&) = delete; + SubscriberListener& operator=(SubscriberListener&&) = delete; + + void SetOwner(TopicSubscription* owner); + + std::unique_ptr> _impl; + }; +} +} + +#include "SubscriberListener.tpp" \ No newline at end of file diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/RosUtils/Source/RosUtils/ros2/SubscriberListener.tpp b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/RosUtils/Source/RosUtils/ros2/SubscriberListener.tpp new file mode 100644 index 00000000000..a5caae4a808 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/RosUtils/Source/RosUtils/ros2/SubscriberListener.tpp @@ -0,0 +1,115 @@ +#include + +#include "carla/ros2/plugin-utils/enable-fastdds-include.h" // start fastdds includes +#include +#include +#include +#include +#include "carla/ros2/plugin-utils/disable-fastdds-include.h" // end fastdds includes + +#include "TopicSubscription.h" +#include "carla/ros2/ROS2CallbackData.h" + +namespace carla { +namespace ros2 { + + namespace efd = eprosima::fastdds::dds; + using erc = eprosima::fastrtps::types::ReturnCode_t; + + template + class SubscriberListenerImpl : public efd::DataReaderListener { + + public: + void on_subscription_matched( + efd::DataReader* reader, + const efd::SubscriptionMatchedStatus& info) override; + void on_data_available(efd::DataReader* reader) override; + + int _matched {0}; + bool _first_connected {false}; + TopicSubscription* _owner {nullptr}; + MessageType _message {}; + }; + + template + void SubscriberListenerImpl::on_subscription_matched(efd::DataReader* reader, const efd::SubscriptionMatchedStatus& info) + { + if (info.current_count_change == 1) { + _matched = info.total_count; + _first_connected = true; + } else if (info.current_count_change == -1) { + _matched = info.total_count; + if (_matched == 0) { + _owner->DestroySubscriber(); + } + } else { + std::cerr << info.current_count_change + << " is not a valid value for PublicationMatchedStatus current count change" << std::endl; + } + } + + template + void SubscriberListenerImpl::on_data_available(efd::DataReader* reader) + { + efd::SampleInfo info; + eprosima::fastrtps::types::ReturnCode_t rcode = reader->take_next_sample(&_message, &info); + if (rcode == erc::ReturnCodeValue::RETCODE_OK) { + _owner->ForwardMessage(_message); + } + if (rcode == erc::ReturnCodeValue::RETCODE_ERROR) { + std::cerr << "RETCODE_ERROR" << std::endl; + } + if (rcode == erc::ReturnCodeValue::RETCODE_UNSUPPORTED) { + std::cerr << "RETCODE_UNSUPPORTED" << std::endl; + } + if (rcode == erc::ReturnCodeValue::RETCODE_BAD_PARAMETER) { + std::cerr << "RETCODE_BAD_PARAMETER" << std::endl; + } + if (rcode == erc::ReturnCodeValue::RETCODE_PRECONDITION_NOT_MET) { + std::cerr << "RETCODE_PRECONDITION_NOT_MET" << std::endl; + } + if (rcode == erc::ReturnCodeValue::RETCODE_OUT_OF_RESOURCES) { + std::cerr << "RETCODE_OUT_OF_RESOURCES" << std::endl; + } + if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ENABLED) { + std::cerr << "RETCODE_NOT_ENABLED" << std::endl; + } + if (rcode == erc::ReturnCodeValue::RETCODE_IMMUTABLE_POLICY) { + std::cerr << "RETCODE_IMMUTABLE_POLICY" << std::endl; + } + if (rcode == erc::ReturnCodeValue::RETCODE_INCONSISTENT_POLICY) { + std::cerr << "RETCODE_INCONSISTENT_POLICY" << std::endl; + } + if (rcode == erc::ReturnCodeValue::RETCODE_ALREADY_DELETED) { + std::cerr << "RETCODE_ALREADY_DELETED" << std::endl; + } + if (rcode == erc::ReturnCodeValue::RETCODE_TIMEOUT) { + std::cerr << "RETCODE_TIMEOUT" << std::endl; + } + if (rcode == erc::ReturnCodeValue::RETCODE_NO_DATA) { + std::cerr << "RETCODE_NO_DATA" << std::endl; + } + if (rcode == erc::ReturnCodeValue::RETCODE_ILLEGAL_OPERATION) { + std::cerr << "RETCODE_ILLEGAL_OPERATION" << std::endl; + } + if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ALLOWED_BY_SECURITY) { + std::cerr << "RETCODE_NOT_ALLOWED_BY_SECURITY" << std::endl; + } + } + + template + void SubscriberListener::SetOwner(TopicSubscription* owner) { + _impl->_owner = owner; + } + + template + SubscriberListener::SubscriberListener(TopicSubscription* owner) : + _impl(std::make_unique>()) { + _impl->_owner = owner; + } + + template + SubscriberListener::~SubscriberListener() {} + +}} + diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/RosUtils/Source/RosUtils/ros2/TopicPublisher.h b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/RosUtils/Source/RosUtils/ros2/TopicPublisher.h new file mode 100644 index 00000000000..0940d3eb135 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/RosUtils/Source/RosUtils/ros2/TopicPublisher.h @@ -0,0 +1,46 @@ +// Copyright (c) 2024 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 . + +#pragma once + +#include +#include + +#include "carla/ros2/publishers/CarlaPublisher.h" + +namespace carla { +namespace ros2 { + + class ROSUTILS_API TopicPublisherBase : public CarlaPublisher { + public: + virtual bool Init(std::string topic_name) {return false;} + }; + + template + struct TopicPublisherImpl; + + template + class TopicPublisher : public TopicPublisherBase { + public: + TopicPublisher(const char* ros_name = "", const char* parent = ""); + ~TopicPublisher(); + TopicPublisher(const TopicPublisher&); + TopicPublisher& operator=(const TopicPublisher&); + TopicPublisher(TopicPublisher&&); + TopicPublisher& operator=(TopicPublisher&&); + + virtual bool Init(std::string topic_name) override; + virtual const char* type() const override { return "template"; } + + void MakeHeader(MessageType& Message, int32_t seconds, uint32_t nanoseconds); + bool Publish(MessageType Message); + + + private: + std::shared_ptr> _impl; + }; +} +} + +#include "TopicPublisher.tpp" diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/RosUtils/Source/RosUtils/ros2/TopicPublisher.tpp b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/RosUtils/Source/RosUtils/ros2/TopicPublisher.tpp new file mode 100644 index 00000000000..17923c75f60 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/RosUtils/Source/RosUtils/ros2/TopicPublisher.tpp @@ -0,0 +1,224 @@ +#include + +#include "carla/ros2/listeners/CarlaListener.h" + +#include "carla/ros2/plugin-utils/enable-fastdds-include.h" // start fastdds includes +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include "carla/ros2/plugin-utils/disable-fastdds-include.h" // end fastdds includes + +namespace carla { +namespace ros2 { + + namespace efd = eprosima::fastdds::dds; + using erc = eprosima::fastrtps::types::ReturnCode_t; + + template + struct TopicPublisherImpl { + efd::DomainParticipant* _participant { nullptr }; + efd::Publisher* _publisher { nullptr }; + efd::Topic* _topic { nullptr }; + efd::DataWriter* _datawriter { nullptr }; + efd::TypeSupport _type { new PubSubTypes() }; + CarlaListener _listener {}; + }; + + template + bool TopicPublisher::Init(std::string topic_name) { + if (_impl->_type == nullptr) { + std::cerr << "Invalid TypeSupport" << std::endl; + return false; + } + + efd::DomainParticipantQos pqos = efd::PARTICIPANT_QOS_DEFAULT; + pqos.name(_name); + auto factory = efd::DomainParticipantFactory::get_instance(); + _impl->_participant = factory->create_participant(0, pqos); + if (_impl->_participant == nullptr) { + std::cerr << "Failed to create DomainParticipant" << std::endl; + return false; + } + _impl->_type.register_type(_impl->_participant); + + efd::PublisherQos pubqos = efd::PUBLISHER_QOS_DEFAULT; + _impl->_publisher = _impl->_participant->create_publisher(pubqos, nullptr); + if (_impl->_publisher == nullptr) { + std::cerr << "Failed to create Publisher" << std::endl; + return false; + } + + efd::TopicQos tqos = efd::TOPIC_QOS_DEFAULT; + const std::string base { "rt/carla/" }; + std::string full_topic_name = base; + if (!_parent.empty()) + full_topic_name += _parent + "/"; + full_topic_name += _name + "/"; + full_topic_name += topic_name; + _impl->_topic = _impl->_participant->create_topic(full_topic_name, _impl->_type->getName(), tqos); + if (_impl->_topic == nullptr) { + std::cerr << "Failed to create Topic" << std::endl; + return false; + } + efd::DataWriterQos wqos = efd::DATAWRITER_QOS_DEFAULT; + wqos.endpoint().history_memory_policy = eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + efd::DataWriterListener* listener = (efd::DataWriterListener*)_impl->_listener._impl.get(); + _impl->_datawriter = _impl->_publisher->create_datawriter(_impl->_topic, wqos, listener); + if (_impl->_datawriter == nullptr) { + std::cerr << "Failed to create DataWriter" << std::endl; + return false; + } + + _frame_id = _name; + return true; + } + + template + bool TopicPublisher::Publish(MessageType Message) { + eprosima::fastrtps::rtps::InstanceHandle_t instance_handle; + eprosima::fastrtps::types::ReturnCode_t rcode = _impl->_datawriter->write(&Message, instance_handle); + if (rcode == erc::ReturnCodeValue::RETCODE_OK) { + return true; + } + if (rcode == erc::ReturnCodeValue::RETCODE_ERROR) { + std::cerr << "RETCODE_ERROR" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_UNSUPPORTED) { + std::cerr << "RETCODE_UNSUPPORTED" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_BAD_PARAMETER) { + std::cerr << "RETCODE_BAD_PARAMETER" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_PRECONDITION_NOT_MET) { + std::cerr << "RETCODE_PRECONDITION_NOT_MET" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_OUT_OF_RESOURCES) { + std::cerr << "RETCODE_OUT_OF_RESOURCES" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ENABLED) { + std::cerr << "RETCODE_NOT_ENABLED" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_IMMUTABLE_POLICY) { + std::cerr << "RETCODE_IMMUTABLE_POLICY" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_INCONSISTENT_POLICY) { + std::cerr << "RETCODE_INCONSISTENT_POLICY" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_ALREADY_DELETED) { + std::cerr << "RETCODE_ALREADY_DELETED" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_TIMEOUT) { + std::cerr << "RETCODE_TIMEOUT" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_NO_DATA) { + std::cerr << "RETCODE_NO_DATA" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_ILLEGAL_OPERATION) { + std::cerr << "RETCODE_ILLEGAL_OPERATION" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ALLOWED_BY_SECURITY) { + std::cerr << "RETCODE_NOT_ALLOWED_BY_SECURITY" << std::endl; + return false; + } + std::cerr << "UNKNOWN" << std::endl; + return false; + } + + template + TopicPublisher::TopicPublisher(const char* ros_name, const char* parent) : + _impl(std::make_shared>()) { + _name = ros_name; + _parent = parent; + } + + template + TopicPublisher::~TopicPublisher() { + if (!_impl) + return; + + if (_impl->_datawriter) + _impl->_publisher->delete_datawriter(_impl->_datawriter); + + if (_impl->_publisher) + _impl->_participant->delete_publisher(_impl->_publisher); + + if (_impl->_topic) + _impl->_participant->delete_topic(_impl->_topic); + + if (_impl->_participant) + efd::DomainParticipantFactory::get_instance()->delete_participant(_impl->_participant); + } + + template + TopicPublisher::TopicPublisher(const TopicPublisher& other) { + _frame_id = other._frame_id; + _name = other._name; + _parent = other._parent; + _impl = other._impl; + } + + template + TopicPublisher& TopicPublisher::operator=(const TopicPublisher& other) { + _frame_id = other._frame_id; + _name = other._name; + _parent = other._parent; + _impl = other._impl; + + return *this; + } + + template + TopicPublisher::TopicPublisher(TopicPublisher&& other) { + _frame_id = std::move(other._frame_id); + _name = std::move(other._name); + _parent = std::move(other._parent); + _impl = std::move(other._impl); + } + + template + TopicPublisher& TopicPublisher::operator=(TopicPublisher&& 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; + } + + template + void TopicPublisher::MakeHeader(MessageType& Message, int32_t seconds, uint32_t nanoseconds){ + builtin_interfaces::msg::Time time; + time.sec(seconds); + time.nanosec(nanoseconds); + + std_msgs::msg::Header header; + header.stamp(std::move(time)); + header.frame_id(_frame_id); + Message.header(header); + } +} +} diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/RosUtils/Source/RosUtils/ros2/TopicSubscription.h b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/RosUtils/Source/RosUtils/ros2/TopicSubscription.h new file mode 100644 index 00000000000..db3dc032cbf --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/RosUtils/Source/RosUtils/ros2/TopicSubscription.h @@ -0,0 +1,60 @@ +// Copyright (c) 2024 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 . + +#pragma once + +#include +#include + +#include "carla/ros2/subscribers/CarlaSubscriber.h" + +namespace carla { +namespace ros2 { + template + struct TopicSubscriptionImpl; + + template + using MessageCallback = std::function; + + class ROSUTILS_API TopicSubscriptionBase : public CarlaSubscriber { + public: + virtual bool HasNewMessage() {return false;} + virtual bool IsAlive() {return false;} + virtual void Spin() {} + virtual void* GetOwner() {return nullptr;} + }; + + template + class ROSUTILS_API TopicSubscription : public TopicSubscriptionBase { + public: + TopicSubscription(void* owning_actor, const char* ros_name = "", const char* parent = ""); + ~TopicSubscription(); + TopicSubscription(const TopicSubscription&); + TopicSubscription& operator=(const TopicSubscription&); + TopicSubscription(TopicSubscription&&); + TopicSubscription& operator=(TopicSubscription&&); + + virtual bool HasNewMessage(); + virtual bool IsAlive(); + virtual void Spin(); + virtual void* GetOwner(); + MessageType GetMessage(); + + bool Subscribe(const std::string topic_name, MessageCallback callback); + bool Read(); + const char* type() const override { return typeid(MessageType).name(); } + + //Do not call, for internal use only + void ForwardMessage(MessageType message); + void DestroySubscriber(); + + private: + std::shared_ptr> _impl; + std::optional> callback; + }; +} +} + +#include "TopicSubscription.tpp" + \ No newline at end of file diff --git a/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/RosUtils/Source/RosUtils/ros2/TopicSubscription.tpp b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/RosUtils/Source/RosUtils/ros2/TopicSubscription.tpp new file mode 100644 index 00000000000..7d836b887e6 --- /dev/null +++ b/Unreal/CarlaUnreal/Plugins/CarlaIntegrationPlugins/RosUtils/Source/RosUtils/ros2/TopicSubscription.tpp @@ -0,0 +1,271 @@ +#include "carla/ros2/plugin-utils/enable-fastdds-include.h" // start fastdds includes +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include "carla/ros2/plugin-utils/disable-fastdds-include.h" // end fastdds includes + +#include "SubscriberListener.h" + +namespace carla { + namespace ros2 { + + namespace efd = eprosima::fastdds::dds; + using erc = eprosima::fastrtps::types::ReturnCode_t; + + template + struct TopicSubscriptionImpl { + efd::DomainParticipant* _participant { nullptr }; + efd::Subscriber* _subscriber { nullptr }; + efd::Topic* _topic { nullptr }; + efd::DataReader* _datareader { nullptr }; + efd::TypeSupport _type { new PubSubType() }; + SubscriberListener _listener {nullptr}; + MessageType _event {}; + MessageType _message {}; + bool _new_message {false}; + bool _alive {true}; + void* _owningActor {nullptr}; + }; + + template + bool TopicSubscription::Subscribe(const std::string topic_name_tail, MessageCallback topic_callback) { + if (_impl->_type == nullptr) { + std::cerr << "Invalid TypeSupport" << std::endl; + return false; + } + + efd::DomainParticipantQos pqos = efd::PARTICIPANT_QOS_DEFAULT; + pqos.name(_name); + auto factory = efd::DomainParticipantFactory::get_instance(); + _impl->_participant = factory->create_participant(0, pqos); + if (_impl->_participant == nullptr) { + std::cerr << "Failed to create DomainParticipant" << std::endl; + return false; + } + _impl->_type.register_type(_impl->_participant); + + efd::SubscriberQos subqos = efd::SUBSCRIBER_QOS_DEFAULT; + _impl->_subscriber = _impl->_participant->create_subscriber(subqos, nullptr); + if (_impl->_subscriber == nullptr) { + std::cerr << "Failed to create Subscriber" << std::endl; + return false; + } + + efd::TopicQos tqos = efd::TOPIC_QOS_DEFAULT; + const std::string base { "rt/carla/" }; + std::string topic_name = base; + if (!_parent.empty()) + topic_name += _parent + "/"; + topic_name += _name; + topic_name += "/" + topic_name_tail; + _impl->_topic = _impl->_participant->create_topic(topic_name, _impl->_type->getName(), tqos); + if (_impl->_topic == nullptr) { + std::cerr << "Failed to create Topic" << std::endl; + return false; + } + + efd::DataReaderQos rqos = efd::DATAREADER_QOS_DEFAULT; + efd::DataReaderListener* listener = (efd::DataReaderListener*)_impl->_listener._impl.get(); + _impl->_datareader = _impl->_subscriber->create_datareader(_impl->_topic, rqos, listener); + if (_impl->_datareader == nullptr) { + std::cerr << "Failed to create DataReader" << std::endl; + return false; + } + + callback = topic_callback; + + return true; + } + + template + void TopicSubscription::Spin() { + if (IsAlive()) { + if (HasNewMessage() && callback.has_value()) { + auto callback_func = *callback; + callback_func(GetMessage()); + } + } + } + + template + bool TopicSubscription::Read() { + efd::SampleInfo info; + eprosima::fastrtps::types::ReturnCode_t rcode = _impl->_datareader->take_next_sample(&_impl->_event, &info); + if (rcode == erc::ReturnCodeValue::RETCODE_OK) { + return true; + } + if (rcode == erc::ReturnCodeValue::RETCODE_ERROR) { + std::cerr << "RETCODE_ERROR" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_UNSUPPORTED) { + std::cerr << "RETCODE_UNSUPPORTED" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_BAD_PARAMETER) { + std::cerr << "RETCODE_BAD_PARAMETER" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_PRECONDITION_NOT_MET) { + std::cerr << "RETCODE_PRECONDITION_NOT_MET" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_OUT_OF_RESOURCES) { + std::cerr << "RETCODE_OUT_OF_RESOURCES" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ENABLED) { + std::cerr << "RETCODE_NOT_ENABLED" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_IMMUTABLE_POLICY) { + std::cerr << "RETCODE_IMMUTABLE_POLICY" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_INCONSISTENT_POLICY) { + std::cerr << "RETCODE_INCONSISTENT_POLICY" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_ALREADY_DELETED) { + std::cerr << "RETCODE_ALREADY_DELETED" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_TIMEOUT) { + std::cerr << "RETCODE_TIMEOUT" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_NO_DATA) { + std::cerr << "RETCODE_NO_DATA" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_ILLEGAL_OPERATION) { + std::cerr << "RETCODE_ILLEGAL_OPERATION" << std::endl; + return false; + } + if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ALLOWED_BY_SECURITY) { + std::cerr << "RETCODE_NOT_ALLOWED_BY_SECURITY" << std::endl; + return false; + } + std::cerr << "UNKNOWN" << std::endl; + return false; + } + + template + void TopicSubscription::ForwardMessage(MessageType message) { + _impl->_message = message; + _impl->_new_message = true; + } + + template + void TopicSubscription::DestroySubscriber() { + _impl->_alive = false; + } + + template + MessageType TopicSubscription::GetMessage() { + _impl->_new_message = false; + return _impl->_message; + } + + template + bool TopicSubscription::IsAlive() { + return _impl->_alive; + } + + template + bool TopicSubscription::HasNewMessage() { + return _impl->_new_message; + } + + template + void* TopicSubscription::GetOwner() { + return _impl->_owningActor; + } + + template + TopicSubscription::TopicSubscription(void* owningActor, const char* ros_name, const char* parent) : + _impl(std::make_shared>()) { + _impl->_listener.SetOwner(this); + _impl->_owningActor = owningActor; + _name = ros_name; + _parent = parent; + } + + template + TopicSubscription::~TopicSubscription() { + if (!_impl) + return; + + if (_impl->_subscriber && _impl->_datareader) + _impl->_subscriber->delete_datareader(_impl->_datareader); + + if (_impl->_participant && _impl->_subscriber) + _impl->_participant->delete_subscriber(_impl->_subscriber); + + if (_impl->_participant && _impl->_topic) + _impl->_participant->delete_topic(_impl->_topic); + + if (_impl->_participant) { + auto efd_instance = efd::DomainParticipantFactory::get_instance(); + if (efd_instance) + efd_instance->delete_participant(_impl->_participant); + } + + } + + template + TopicSubscription::TopicSubscription(const TopicSubscription& other) { + _frame_id = other._frame_id; + _name = other._name; + _parent = other._parent; + _impl = other._impl; + _impl->_listener.SetOwner(this); + } + + template + TopicSubscription& TopicSubscription::operator=(const TopicSubscription& other) { + _frame_id = other._frame_id; + _name = other._name; + _parent = other._parent; + _impl = other._impl; + _impl->_listener.SetOwner(this); + + return *this; + } + + template + TopicSubscription::TopicSubscription(TopicSubscription&& other) { + _frame_id = std::move(other._frame_id); + _name = std::move(other._name); + _parent = std::move(other._parent); + _impl = std::move(other._impl); + _impl->_listener.SetOwner(this); + } + + template + TopicSubscription& TopicSubscription::operator=(TopicSubscription&& other) { + _frame_id = std::move(other._frame_id); + _name = std::move(other._name); + _parent = std::move(other._parent); + _impl = std::move(other._impl); + _impl->_listener.SetOwner(this); + + return *this; + } + } + } + + \ No newline at end of file