added cmake target
added definition in buildcarla added defines to avoid compile demo in ros
This commit is contained in:
parent
2bbe3c5e8b
commit
678e8b48c0
|
@ -39,7 +39,9 @@
|
|||
|
||||
#include "subscribers/CarlaSubscriber.h"
|
||||
#include "subscribers/CarlaEgoVehicleControlSubscriber.h"
|
||||
#include "subscribers/BasicSubscriber.h"
|
||||
#if defined(WITH_ROS2_DEMO)
|
||||
#include "subscribers/BasicSubscriber.h"
|
||||
#endif
|
||||
|
||||
#include <vector>
|
||||
|
||||
|
@ -69,8 +71,7 @@ enum ESensors {
|
|||
InstanceSegmentationCamera,
|
||||
WorldObserver,
|
||||
CameraGBufferUint8,
|
||||
CameraGBufferFloat,
|
||||
YourNewSensor
|
||||
CameraGBufferFloat
|
||||
};
|
||||
|
||||
void ROS2::Enable(bool enable) {
|
||||
|
@ -78,8 +79,10 @@ void ROS2::Enable(bool enable) {
|
|||
log_info("ROS2 enabled: ", _enabled);
|
||||
_clock_publisher = std::make_shared<CarlaClockPublisher>("clock", "");
|
||||
_clock_publisher->Init();
|
||||
#if defined(WITH_ROS2_DEMO)
|
||||
_basic_publisher = std::make_shared<BasicPublisher>("basic_publisher", "");
|
||||
_basic_publisher->Init();
|
||||
#endif
|
||||
}
|
||||
|
||||
void ROS2::SetFrame(uint64_t frame) {
|
||||
|
@ -99,6 +102,7 @@ void ROS2::SetFrame(uint64_t frame) {
|
|||
RemoveActorCallback(actor);
|
||||
}
|
||||
}
|
||||
#if defined(WITH_ROS2_DEMO)
|
||||
if (_basic_subscriber)
|
||||
{
|
||||
void* actor = _basic_subscriber->GetActor();
|
||||
|
@ -115,6 +119,7 @@ void ROS2::SetFrame(uint64_t frame) {
|
|||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
void ROS2::SetTimestamp(double timestamp) {
|
||||
|
@ -125,9 +130,10 @@ void ROS2::SetTimestamp(double timestamp) {
|
|||
_nanoseconds = static_cast<uint32_t>(fractional * multiplier);
|
||||
_clock_publisher->SetData(_seconds, _nanoseconds);
|
||||
_clock_publisher->Publish();
|
||||
#if defined(WITH_ROS2_DEMO)
|
||||
_basic_publisher->SetData("Hello from Carla!");
|
||||
_basic_publisher->Publish();
|
||||
//log_info("ROS2 new timestamp: ", _timestamp);
|
||||
#endif
|
||||
}
|
||||
|
||||
void ROS2::AddActorRosName(void *actor, std::string ros_name) {
|
||||
|
@ -194,6 +200,7 @@ std::string ROS2::GetActorParentRosName(void *actor) {
|
|||
return std::string("");
|
||||
}
|
||||
|
||||
#if defined(WITH_ROS2_DEMO)
|
||||
void ROS2::AddBasicSubscriberCallback(void* actor, std::string ros_name, ActorMessageCallback callback) {
|
||||
_actor_message_callbacks.insert({actor, std::move(callback)});
|
||||
|
||||
|
@ -206,6 +213,7 @@ void ROS2::RemoveBasicSubscriberCallback(void* actor) {
|
|||
_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)});
|
||||
|
@ -503,24 +511,6 @@ std::pair<std::shared_ptr<CarlaPublisher>, std::shared_ptr<CarlaTransformPublish
|
|||
case ESensors::CameraGBufferFloat: {
|
||||
std::cerr << "Camera GBuffer float does not have an available publisher" << std::endl;
|
||||
} break;
|
||||
case ESensors::YourNewSensor: {
|
||||
if (ros_name == "ros_name__") {
|
||||
ros_name.pop_back();
|
||||
ros_name.pop_back();
|
||||
ros_name += string_id;
|
||||
UpdateActorRosName(actor, ros_name);
|
||||
}
|
||||
std::shared_ptr<BasicPublisher> new_publisher = std::make_shared<BasicPublisher>(ros_name.c_str(), parent_ros_name.c_str());
|
||||
if (new_publisher->Init()) {
|
||||
_publishers.insert({actor, new_publisher});
|
||||
publisher = new_publisher;
|
||||
}
|
||||
std::shared_ptr<CarlaTransformPublisher> new_transform = std::make_shared<CarlaTransformPublisher>(ros_name.c_str(), parent_ros_name.c_str());
|
||||
if (new_transform->Init()) {
|
||||
_transforms.insert({actor, new_transform});
|
||||
transform = new_transform;
|
||||
}
|
||||
}
|
||||
default: {
|
||||
std::cerr << "Unknown sensor type" << std::endl;
|
||||
}
|
||||
|
@ -893,10 +883,12 @@ void ROS2::Shutdown() {
|
|||
element.second.reset();
|
||||
}
|
||||
_clock_publisher.reset();
|
||||
_basic_publisher.reset();
|
||||
_controller.reset();
|
||||
_basic_subscriber.reset();
|
||||
_enabled = false;
|
||||
#if defined(WITH_ROS2_DEMO)
|
||||
_basic_publisher.reset();
|
||||
_basic_subscriber.reset();
|
||||
#endif
|
||||
}
|
||||
|
||||
} // namespace ros2
|
||||
|
|
|
@ -72,10 +72,12 @@ class ROS2
|
|||
std::string GetActorParentRosName(void *actor);
|
||||
|
||||
// callbacks
|
||||
void AddBasicSubscriberCallback(void* actor, std::string ros_name, ActorMessageCallback callback);
|
||||
void RemoveBasicSubscriberCallback(void* actor);
|
||||
void AddActorCallback(void* actor, std::string ros_name, ActorCallback callback);
|
||||
void RemoveActorCallback(void* actor);
|
||||
#if defined(WITH_ROS2_DEMO)
|
||||
void RemoveBasicSubscriberCallback(void* actor);
|
||||
void AddBasicSubscriberCallback(void* actor, std::string ros_name, ActorMessageCallback callback);
|
||||
#endif
|
||||
|
||||
// enabling streams to publish
|
||||
void EnableStream(carla::streaming::detail::stream_id_type id) { _publish_stream.insert(id); }
|
||||
|
@ -160,15 +162,16 @@ void ProcessDataFromCollisionSensor(
|
|||
std::unordered_map<void *, std::string> _actor_ros_name;
|
||||
std::unordered_map<void *, std::vector<void*> > _actor_parent_ros_name;
|
||||
std::shared_ptr<CarlaEgoVehicleControlSubscriber> _controller;
|
||||
std::shared_ptr<BasicSubscriber> _basic_subscriber;
|
||||
std::shared_ptr<CarlaClockPublisher> _clock_publisher;
|
||||
std::shared_ptr<BasicPublisher> _basic_publisher;
|
||||
std::unordered_map<void *, std::shared_ptr<CarlaPublisher>> _publishers;
|
||||
std::unordered_map<void *, std::shared_ptr<CarlaTransformPublisher>> _transforms;
|
||||
std::unordered_set<carla::streaming::detail::stream_id_type> _publish_stream;
|
||||
std::unordered_map<void *, ActorCallback> _actor_callbacks;
|
||||
#if defined(WITH_ROS2_DEMO)
|
||||
std::shared_ptr<BasicSubscriber> _basic_subscriber;
|
||||
std::shared_ptr<BasicPublisher> _basic_publisher;
|
||||
std::unordered_map<void *, ActorMessageCallback> _actor_message_callbacks;
|
||||
|
||||
#endif
|
||||
};
|
||||
|
||||
} // namespace ros2
|
||||
|
|
|
@ -482,6 +482,22 @@ carla_add_custom_target (
|
|||
VERBATIM
|
||||
)
|
||||
|
||||
if (ENABLE_ROS2)
|
||||
carla_add_custom_target (
|
||||
launch-ros2-demo
|
||||
COMMENT
|
||||
"Launching Carla Unreal..."
|
||||
COMMAND
|
||||
${UNREAL_EDITOR_PATH}
|
||||
${CARLA_UE_PROJECT_PATH}
|
||||
-${CARLA_UNREAL_RHI}
|
||||
--ros2-demo
|
||||
${LAUNCH_ARGS}
|
||||
USES_TERMINAL
|
||||
VERBATIM
|
||||
)
|
||||
endif()
|
||||
|
||||
add_dependencies (
|
||||
launch
|
||||
carla-unreal-editor
|
||||
|
|
|
@ -216,13 +216,14 @@ FCarlaActor* UActorDispatcher::RegisterActor(
|
|||
ActorROS2Handler Handler(UEActor, RosName);
|
||||
std::visit(Handler, Data);
|
||||
});
|
||||
|
||||
#if defined(WITH_ROS2_DEMO)
|
||||
ROS2->AddBasicSubscriberCallback(static_cast<void*>(&Actor), RosName, [RosName](void *Actor, carla::ros2::ROS2MessageCallbackData Data) -> void
|
||||
{
|
||||
AActor *UEActor = reinterpret_cast<AActor *>(Actor);
|
||||
ActorROS2Handler Handler(UEActor, RosName);
|
||||
std::visit(Handler, Data);
|
||||
});
|
||||
#endif
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
@ -24,6 +24,9 @@ public class Carla :
|
|||
[CommandLine("-ros2")]
|
||||
bool EnableRos2 = false;
|
||||
|
||||
[CommandLine("-ros2-demo")]
|
||||
bool EnableRos2Demo = false;
|
||||
|
||||
[CommandLine("-osm2odr")]
|
||||
bool EnableOSM2ODR = false;
|
||||
|
||||
|
@ -48,6 +51,11 @@ public class Carla :
|
|||
{
|
||||
EnableRos2 = true;
|
||||
}
|
||||
else if (optionTrimmed.Equals("ROS2_DEMO"))
|
||||
{
|
||||
EnableRos2 = true;
|
||||
EnableRos2Demo = true;
|
||||
}
|
||||
else if(optionTrimmed.Equals("OSM2ODR"))
|
||||
{
|
||||
EnableOSM2ODR = true;
|
||||
|
@ -154,6 +162,7 @@ public class Carla :
|
|||
if (EnableRos2)
|
||||
{
|
||||
TestOptionalFeature(EnableRos2, "Ros2 support", "WITH_ROS2");
|
||||
TestOptionalFeature(EnableRos2Demo, "Ros2 demo", "WITH_ROS2_DEMO");
|
||||
|
||||
string CarlaPluginSourcePath = Path.GetFullPath(ModuleDirectory);
|
||||
string CarlaPluginBinariesLinuxPath = Path.Combine(CarlaPluginSourcePath, "..", "..", "Binaries", "Linux");
|
||||
|
|
Loading…
Reference in New Issue