make sensor component msg qos configurable from robot (#366) (#370)

* make sensor component msg qos configurable from robot

* Update RRROS2OverlapSensorComponent.h

update documentation

Co-authored-by: yuokamoto <[email protected]>

Author: github-actions[bot]

Source/RapyutaSimulationPlugins/Private/Robots/RRRobotROS2Interface.cpp

@@ -51,6 +51,7 @@ void URRRobotROS2Interface::InitInterfaces()
                 URRUObjectUtils::CreateChildComponent<URRBaseOdomComponent>(Robot, *FString::Printf(TEXT("%sOdom"), *GetName()));
             OdomComponent->bPublishOdomTf = bPublishOdomTf;
             OdomComponent->PublicationFrequencyHz = OdomPublicationFrequencyHz;
+            OdomComponent->QoS = OdomQoS;
             OdomComponent->RootOffset = Robot->RootOffset;
         }
     }

Source/RapyutaSimulationPlugins/Private/Sensors/RRBaseOdomComponent.cpp

@@ -6,6 +6,7 @@ URRBaseOdomComponent::URRBaseOdomComponent()
 {
     SensorPublisherClass = URRROS2OdomPublisher::StaticClass();
     TopicName = TEXT("odom");
+    QoS = UROS2QoS::SensorData;
     PublicationFrequencyHz = 30;
     FrameId = TEXT("odom");    //default frame id
 }

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<ASimulationState>(UGameplayStatics::GetActorOfClass(GetWorld(), ASimulationState::StaticClass()));
 }

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)
 {

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());
 }

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
 }
 

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");

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

Source/RapyutaSimulationPlugins/Public/Sensors/RRROS2BaseSensorComponent.h

@@ -108,10 +108,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.
@@ -191,6 +206,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;

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;