diff --git a/Source/RapyutaSimulationPlugins/Private/Robots/RRRobotROS2Interface.cpp b/Source/RapyutaSimulationPlugins/Private/Robots/RRRobotROS2Interface.cpp index 7bf12817..db79c54a 100644 --- a/Source/RapyutaSimulationPlugins/Private/Robots/RRRobotROS2Interface.cpp +++ b/Source/RapyutaSimulationPlugins/Private/Robots/RRRobotROS2Interface.cpp @@ -51,6 +51,7 @@ void URRRobotROS2Interface::InitInterfaces() URRUObjectUtils::CreateChildComponent(Robot, *FString::Printf(TEXT("%sOdom"), *GetName())); OdomComponent->bPublishOdomTf = bPublishOdomTf; OdomComponent->PublicationFrequencyHz = OdomPublicationFrequencyHz; + OdomComponent->QoS = OdomQoS; OdomComponent->RootOffset = Robot->RootOffset; } } diff --git a/Source/RapyutaSimulationPlugins/Private/Sensors/RRBaseOdomComponent.cpp b/Source/RapyutaSimulationPlugins/Private/Sensors/RRBaseOdomComponent.cpp index 0e4d4751..c00cdadb 100644 --- a/Source/RapyutaSimulationPlugins/Private/Sensors/RRBaseOdomComponent.cpp +++ b/Source/RapyutaSimulationPlugins/Private/Sensors/RRBaseOdomComponent.cpp @@ -6,6 +6,7 @@ URRBaseOdomComponent::URRBaseOdomComponent() { MsgClass = UROS2OdomMsg::StaticClass(); TopicName = TEXT("odom"); + QoS = UROS2QoS::SensorData; PublicationFrequencyHz = 30; FrameId = TEXT("odom"); //default frame id SensorPublisherClass = URRROS2OdomPublisher::StaticClass(); diff --git a/Source/RapyutaSimulationPlugins/Private/Sensors/RRPoseSensorManager.cpp b/Source/RapyutaSimulationPlugins/Private/Sensors/RRPoseSensorManager.cpp index 5610b378..44319ef0 100644 --- a/Source/RapyutaSimulationPlugins/Private/Sensors/RRPoseSensorManager.cpp +++ b/Source/RapyutaSimulationPlugins/Private/Sensors/RRPoseSensorManager.cpp @@ -31,11 +31,10 @@ void URRPoseSensorManager::OnComponentCreated() void URRPoseSensorManager::InitalizeWithROS2(UROS2NodeComponent* InROS2Node, const FString& InPublisherName, - const FString& InTopicName, - const UROS2QoS InQoS) + const FString& InTopicName) { UE_LOG_WITH_INFO_NAMED(LogRapyutaCore, Warning, TEXT("%s"), *ReferenceTag); - Super::InitalizeWithROS2(InROS2Node, InPublisherName, InTopicName, InQoS); + Super::InitalizeWithROS2(InROS2Node, InPublisherName, InTopicName); MapOriginPoseSensor->InitalizeWithROS2(InROS2Node); ServerSimState = CastChecked(UGameplayStatics::GetActorOfClass(GetWorld(), ASimulationState::StaticClass())); } diff --git a/Source/RapyutaSimulationPlugins/Private/Sensors/RRROS2BaseSensorComponent.cpp b/Source/RapyutaSimulationPlugins/Private/Sensors/RRROS2BaseSensorComponent.cpp index e7e9ce5c..3042e9c5 100644 --- a/Source/RapyutaSimulationPlugins/Private/Sensors/RRROS2BaseSensorComponent.cpp +++ b/Source/RapyutaSimulationPlugins/Private/Sensors/RRROS2BaseSensorComponent.cpp @@ -11,8 +11,7 @@ URRROS2BaseSensorComponent::URRROS2BaseSensorComponent() void URRROS2BaseSensorComponent::InitalizeWithROS2(UROS2NodeComponent* InROS2Node, const FString& InPublisherName, - const FString& InTopicName, - const UROS2QoS InQoS) + const FString& InTopicName) { // NOTE: Here, [SensorPublisher] is expected to finish custom configuring before being added to [InROS2Node]'s Publishers and init. // Thus [UROS2NodeComponent::CreatePublisher()] is not used. @@ -21,11 +20,19 @@ void URRROS2BaseSensorComponent::InitalizeWithROS2(UROS2NodeComponent* InROS2Nod CreatePublisher(InPublisherName); PreInitializePublisher(InROS2Node, InTopicName); // [SensorPublisher] is added to [InROS2Node]'s Publishers here-in - InitializePublisher(InROS2Node, InQoS); + InitializePublisher(InROS2Node, QoS); // Start getting sensor data Run(); } +void URRROS2BaseSensorComponent::InitalizeWithROS2(UROS2NodeComponent* InROS2Node, + const FString& InPublisherName, + const FString& InTopicName, + const UROS2QoS InQoS) +{ + QoS = InQoS; + InitalizeWithROS2(InROS2Node, InPublisherName, InTopicName); +} void URRROS2BaseSensorComponent::CreatePublisher(const FString& InPublisherName) { diff --git a/Source/RapyutaSimulationPlugins/Private/Sensors/RRROS2OverlapSensorComponent.cpp b/Source/RapyutaSimulationPlugins/Private/Sensors/RRROS2OverlapSensorComponent.cpp index 9b1b6a3b..e35e83e6 100644 --- a/Source/RapyutaSimulationPlugins/Private/Sensors/RRROS2OverlapSensorComponent.cpp +++ b/Source/RapyutaSimulationPlugins/Private/Sensors/RRROS2OverlapSensorComponent.cpp @@ -18,10 +18,9 @@ URRROS2OverlapSensorComponent::URRROS2OverlapSensorComponent() void URRROS2OverlapSensorComponent::InitalizeWithROS2(UROS2NodeComponent* InROS2Node, const FString& InPublisherName, - const FString& InTopicName, - const UROS2QoS InQoS) + const FString& InTopicName) { - Super::InitalizeWithROS2(InROS2Node, InPublisherName, InTopicName, InQoS); + Super::InitalizeWithROS2(InROS2Node, InPublisherName, InTopicName); EventPublisher = InROS2Node->CreatePublisher(EventTopicName, UROS2Publisher::StaticClass(), UROS2OverlapEventMsg::StaticClass()); } diff --git a/Source/RapyutaSimulationPlugins/Private/Tools/RRROS2OdomPublisher.cpp b/Source/RapyutaSimulationPlugins/Private/Tools/RRROS2OdomPublisher.cpp index 19e1cf9a..93880707 100644 --- a/Source/RapyutaSimulationPlugins/Private/Tools/RRROS2OdomPublisher.cpp +++ b/Source/RapyutaSimulationPlugins/Private/Tools/RRROS2OdomPublisher.cpp @@ -9,7 +9,7 @@ URRROS2OdomPublisher::URRROS2OdomPublisher() { MsgClass = UROS2OdomMsg::StaticClass(); - QoS = UROS2QoS::KeepLast; + QoS = UROS2QoS::SensorData; SetDefaultDelegates(); //use UpdateMessage as update delegate } diff --git a/Source/RapyutaSimulationPlugins/Public/Robots/RRRobotROS2Interface.h b/Source/RapyutaSimulationPlugins/Public/Robots/RRRobotROS2Interface.h index 40edfdfe..b7ce13fe 100644 --- a/Source/RapyutaSimulationPlugins/Public/Robots/RRRobotROS2Interface.h +++ b/Source/RapyutaSimulationPlugins/Public/Robots/RRRobotROS2Interface.h @@ -97,6 +97,10 @@ class RAPYUTASIMULATIONPLUGINS_API URRRobotROS2Interface : public URRBaseROS2Int UPROPERTY(EditAnywhere, BlueprintReadWrite, Replicated) float OdomPublicationFrequencyHz = 30; + UPROPERTY(EditAnywhere, BlueprintReadWrite, Replicated) + UROS2QoS OdomQoS = UROS2QoS::SensorData; + + //! Movement command topic. If empty is given, subscriber will not be initiated. UPROPERTY(EditAnywhere, BlueprintReadWrite, Replicated) FString CmdVelTopicName = TEXT("cmd_vel"); diff --git a/Source/RapyutaSimulationPlugins/Public/Sensors/RRPoseSensorManager.h b/Source/RapyutaSimulationPlugins/Public/Sensors/RRPoseSensorManager.h index 71ca9b20..0f827a0c 100644 --- a/Source/RapyutaSimulationPlugins/Public/Sensors/RRPoseSensorManager.h +++ b/Source/RapyutaSimulationPlugins/Public/Sensors/RRPoseSensorManager.h @@ -70,8 +70,7 @@ class RAPYUTASIMULATIONPLUGINS_API URRPoseSensorManager : public URRROS2EntitySt */ virtual void InitalizeWithROS2(UROS2NodeComponent* InROS2Node, const FString& InPublisherName = EMPTY_STR, - const FString& InTopicName = EMPTY_STR, - const UROS2QoS InQoS = UROS2QoS::SensorData) override; + const FString& InTopicName = EMPTY_STR) override; /** * @brief Calculate relative pose with #URRGeneralUtils and update #Data diff --git a/Source/RapyutaSimulationPlugins/Public/Sensors/RRROS2BaseSensorComponent.h b/Source/RapyutaSimulationPlugins/Public/Sensors/RRROS2BaseSensorComponent.h index 8525b2fc..1ead4585 100644 --- a/Source/RapyutaSimulationPlugins/Public/Sensors/RRROS2BaseSensorComponent.h +++ b/Source/RapyutaSimulationPlugins/Public/Sensors/RRROS2BaseSensorComponent.h @@ -101,10 +101,25 @@ class RAPYUTASIMULATIONPLUGINS_API URRROS2BaseSensorComponent : public USceneCom * @sa [ROS 2 QoS](https://docs.ros.org/en/rolling/Concepts/About-Quality-of-Service-Settings.html) */ UFUNCTION(BlueprintCallable) + virtual void InitalizeWithROS2(UROS2NodeComponent* InROS2Node, + const FString& InPublisherName, + const FString& InTopicName, + const UROS2QoS InQoS); + + /** + * @brief Create and initialize publisher and start sensor update by calling + * #CreatePublisher, #PreInitializePublisher, #InitializePublisher and #Run with #QoS + * + * @param InROS2Node ROS2Node which this publisher belongs to + * @param InPublisherName Publisher component name + * @param InTopicName Topic name + * * + * @sa [UROS2NodeComponent](https://rclue.readthedocs.io/en/devel/doxygen_generated/html/d1/d79/_r_o_s2_node_component_8h.html) + * @sa [ROS 2 QoS](https://docs.ros.org/en/rolling/Concepts/About-Quality-of-Service-Settings.html) + */ virtual void InitalizeWithROS2(UROS2NodeComponent* InROS2Node, const FString& InPublisherName = TEXT(""), - const FString& InTopicName = TEXT(""), - const UROS2QoS InQoS = UROS2QoS::SensorData); + const FString& InTopicName = TEXT("")); /** * @brief Create a Publisher with #SensorPublisherClass. @@ -184,6 +199,9 @@ class RAPYUTASIMULATIONPLUGINS_API URRROS2BaseSensorComponent : public USceneCom UPROPERTY(EditAnywhere, BlueprintReadWrite) int32 PublicationFrequencyHz = 30; + UPROPERTY(EditAnywhere, BlueprintReadWrite) + UROS2QoS QoS = UROS2QoS::SensorData; + //! Append namespace to #FrameId or not. UPROPERTY(EditAnywhere, BlueprintReadWrite) bool bAppendNodeNamespace = true; diff --git a/Source/RapyutaSimulationPlugins/Public/Sensors/RRROS2OverlapSensorComponent.h b/Source/RapyutaSimulationPlugins/Public/Sensors/RRROS2OverlapSensorComponent.h index 1b43b5c9..b2370d42 100644 --- a/Source/RapyutaSimulationPlugins/Public/Sensors/RRROS2OverlapSensorComponent.h +++ b/Source/RapyutaSimulationPlugins/Public/Sensors/RRROS2OverlapSensorComponent.h @@ -43,12 +43,10 @@ class RAPYUTASIMULATIONPLUGINS_API URRROS2OverlapSensorComponent : public URRROS * @param InROS2Node * @param InPublisherName * @param InTopicName - * @param InQoS */ virtual void InitalizeWithROS2(UROS2NodeComponent* InROS2Node, const FString& InPublisherName = EMPTY_STR, - const FString& InTopicName = EMPTY_STR, - const UROS2QoS InQoS = UROS2QoS::SensorData) override; + const FString& InTopicName = EMPTY_STR) override; void BeginPlay() override;