[22873] Setup ROS 2 Easy Mode at participant creation

include/fastdds/rtps/RTPSDomain.hpp

@@ -110,6 +110,26 @@ class RTPSDomain
             const RTPSParticipantAttributes& attrs,
             RTPSParticipantListener* plisten = nullptr);
 
+    /**
+     * @brief Create a RTPSParticipant as default server or client if ROS_MASTER_URI environment variable is set.
+     * It also configures ROS 2 Easy Mode IP if ROS2_EASY_MODE_URI environment variable is set
+     * and it was empty in the input attributes.
+     *
+     * @param domain_id DomainId to be used by the RTPSParticipant.
+     * @param enabled True if the RTPSParticipant should be enabled on creation. False if it will be enabled later with RTPSParticipant::enable()
+     * @param attrs RTPSParticipant Attributes.
+     * @param plisten Pointer to the ParticipantListener.
+     * @return Pointer to the RTPSParticipant.
+     *
+     * \warning The returned pointer is invalidated after a call to removeRTPSParticipant() or stopAll(),
+     *          so its use may result in undefined behaviour.
+     */
+    static RTPSParticipant* create_client_server_participant(
+            uint32_t domain_id,
+            bool enabled,
+            const RTPSParticipantAttributes& attrs,
+            RTPSParticipantListener* plisten = nullptr);
+
     /**
      * Create a RTPSWriter in a participant.
      *

src/cpp/fastdds/domain/DomainParticipantImpl.cpp

@@ -291,23 +291,16 @@ ReturnCode_t DomainParticipantImpl::enable()
     utils::set_attributes_from_qos(rtps_attr, qos_);
     rtps_attr.participantID = participant_id_;
 
-    // If DEFAULT_ROS2_MASTER_URI is specified then try to create default client if
-    // that already exists.
-    RTPSParticipant* part = RTPSDomainImpl::clientServerEnvironmentCreationOverride(
+    RTPSParticipant* part = RTPSDomain::createParticipant(
         domain_id_,
         false,
         rtps_attr,
         &rtps_listener_);
 
     if (part == nullptr)
     {
-        part = RTPSDomain::createParticipant(domain_id_, false, rtps_attr, &rtps_listener_);
-
-        if (part == nullptr)
-        {
-            EPROSIMA_LOG_ERROR(DOMAIN_PARTICIPANT, "Problem creating RTPSParticipant");
-            return RETCODE_ERROR;
-        }
+        EPROSIMA_LOG_ERROR(DOMAIN_PARTICIPANT, "Problem creating RTPSParticipant");
+        return RETCODE_ERROR;
     }
 
     guid_ = part->getGuid();

src/cpp/rtps/RTPSDomain.cpp

@@ -118,7 +118,7 @@ RTPSParticipant* RTPSDomain::createParticipant(
         const RTPSParticipantAttributes& attrs,
         RTPSParticipantListener* listen)
 {
-    return RTPSDomainImpl::createParticipant(domain_id, true, attrs, listen);
+    return RTPSDomain::createParticipant(domain_id, true, attrs, listen);
 }
 
 RTPSParticipant* RTPSDomain::createParticipant(
@@ -127,7 +127,36 @@ RTPSParticipant* RTPSDomain::createParticipant(
         const RTPSParticipantAttributes& attrs,
         RTPSParticipantListener* listen)
 {
-    return RTPSDomainImpl::createParticipant(domain_id, enabled, attrs, listen);
+    RTPSParticipant* part = nullptr;
+
+    // Try to create a participant with the default server-client setup.
+    part = RTPSDomainImpl::create_client_server_participant(domain_id, enabled, attrs, listen);
+
+    if (!part)
+    {
+        // Try to create the participant with the input attributes if the auto server-client setup failed
+        // or was omitted.
+        part = RTPSDomainImpl::createParticipant(domain_id, enabled, attrs, listen);
+        if (!part)
+        {
+            EPROSIMA_LOG_ERROR(RTPS_DOMAIN, "Unable to create the participant");
+        }
+    }
+    else
+    {
+        EPROSIMA_LOG_INFO(RTPS_DOMAIN, "Auto default server-client setup: Client created.");
+    }
+
+    return part;
+}
+
+RTPSParticipant* RTPSDomain::create_client_server_participant(
+        uint32_t domain_id,
+        bool enabled,
+        const RTPSParticipantAttributes& attrs,
+        RTPSParticipantListener* plisten /* = nullptr */)
+{
+    return RTPSDomainImpl::create_client_server_participant(domain_id, enabled, attrs, plisten);
 }
 
 bool RTPSDomain::removeRTPSParticipant(
@@ -313,6 +342,62 @@ RTPSParticipant* RTPSDomainImpl::createParticipant(
     return p;
 }
 
+RTPSParticipant* RTPSDomainImpl::create_client_server_participant(
+        uint32_t domain_id,
+        bool enabled,
+        const RTPSParticipantAttributes& attrs,
+        RTPSParticipantListener* plisten)
+{
+    RTPSParticipant* part = nullptr;
+    RTPSParticipantAttributes env_attrs = attrs;
+
+    // Fill participant attributes using set environment variables.
+    // Note: If ROS2_EASY_MODE is configured and it is not set in the input participant attributes, it will be set.
+    // In other case, the previous easy_mode_ip value will be kept and ROS2_EASY_MODE will be ignored.
+    if (client_server_environment_attributes_override(domain_id, env_attrs))
+    {
+        part = createParticipant(domain_id, enabled, env_attrs, plisten);
+
+        if (!part)
+        {
+            // Unable to create auto server-client default participants
+            EPROSIMA_LOG_ERROR(RTPS_DOMAIN, "Auto default server-client setup: Unable to create the client.");
+            return nullptr;
+        }
+        else
+        {
+            // Launch the discovery server daemon if Easy Mode is enabled
+            if (!env_attrs.easy_mode_ip.empty())
+            {
+                if (!run_easy_mode_discovery_server(domain_id, env_attrs.easy_mode_ip))
+                {
+                    EPROSIMA_LOG_ERROR(RTPS_DOMAIN, "Error launching Easy Mode discovery server daemon");
+                    // Remove the client participant
+                    removeRTPSParticipant(part);
+                    part = nullptr;
+                    return nullptr;
+                }
+
+                EPROSIMA_LOG_INFO(RTPS_DOMAIN, "Easy Mode discovery server launched successfully");
+            }
+
+            EPROSIMA_LOG_INFO(RTPS_DOMAIN, "Auto default server-client setup: Default client created.");
+
+            // At this point, Discovery Protocol has changed from SIMPLE to CLIENT or SUPER_CLIENT.
+            // Set client_override_ flag to true (Simple Participant turned into a Client Participant).
+            part->mp_impl->client_override(true);
+
+            return part;
+        }
+    }
+    else
+    {
+        EPROSIMA_LOG_WARNING(RTPS_DOMAIN,
+                "ParticipantAttributes not overriden. Skipping auto server-client default setup.");
+        return nullptr;
+    }
+}
+
 bool RTPSDomainImpl::removeRTPSParticipant(
         RTPSParticipant* p)
 {
@@ -518,36 +603,52 @@ bool RTPSDomainImpl::removeRTPSReader(
     return false;
 }
 
-RTPSParticipant* RTPSDomainImpl::clientServerEnvironmentCreationOverride(
+bool RTPSDomainImpl::client_server_environment_attributes_override(
         uint32_t domain_id,
-        bool enabled,
-        const RTPSParticipantAttributes& att,
-        RTPSParticipantListener* listen)
+        RTPSParticipantAttributes& att)
 {
     // Check the specified discovery protocol: if other than simple it has priority over ros environment variable
     if (att.builtin.discovery_config.discoveryProtocol != DiscoveryProtocol::SIMPLE)
     {
         EPROSIMA_LOG_INFO(RTPS_DOMAIN, "Detected non simple discovery protocol attributes."
                 << " Ignoring auto default client-server setup.");
-        return nullptr;
+        return false;
     }
 
     // We only make the attributes copy when we are sure is worth
     // Is up to the caller guarantee the att argument is not modified during the call
     RTPSParticipantAttributes client_att(att);
 
-    // Check whether we need to initialize in easy mode
-    const std::string& ros_easy_mode_env_value = ros_easy_mode_env();
+    /* Check whether we need to initialize in easy mode */
+
+    // Get the IP of the remote discovery server.
+    // 1. Check if it is configured in RTPSParticipantAttributes
+    // 2. If not, check if it is configured in the environment variable
+    std::string ros_easy_mode_ip_env = ros_easy_mode_env();
 
-    if (ros_easy_mode_env_value.empty())
+    if (!att.easy_mode_ip.empty())
+    {
+        if (!ros_easy_mode_ip_env.empty())
+        {
+            EPROSIMA_LOG_WARNING(RTPSDOMAIN, "Easy mode IP is configured both in RTPSParticipantAttributes and "
+                    << ROS2_EASY_MODE_URI << " environment variable, ignoring the latter.");
+        }
+        client_att.easy_mode_ip = att.easy_mode_ip;
+    }
+    else
+    {
+        client_att.easy_mode_ip = ros_easy_mode_ip_env;
+    }
+
+    if (client_att.easy_mode_ip.empty())
     {
         // Retrieve the info from the environment variable
         LocatorList_t& server_list = client_att.builtin.discovery_config.m_DiscoveryServers;
         if (load_environment_server_info(server_list) && server_list.empty())
         {
             // It's not an error, the environment variable may not be set. Any issue with environment
             // variable syntax is EPROSIMA_LOG_ERROR already
-            return nullptr;
+            return false;
         }
 
         // Check if some address requires the UDPv6, TCPv4 or TCPv6 transport
@@ -648,46 +749,11 @@ RTPSParticipant* RTPSDomainImpl::clientServerEnvironmentCreationOverride(
             EPROSIMA_LOG_WARNING(RTPS_DOMAIN, "An XML profile for 'service' was found. When using ROS2_EASY_MODE, please ensure"
                     " the max_blocking_time is configured with a value higher than the default.");
         }
-
-        SystemCommandBuilder sys_command;
-        int res = sys_command.executable(FAST_DDS_DEFAULT_CLI_SCRIPT_NAME)
-                        .verb(FAST_DDS_DEFAULT_CLI_DISCOVERY_VERB)
-                        .verb(FAST_DDS_DEFAULT_CLI_AUTO_VERB)
-                        .arg("-d")
-                        .value(std::to_string(domain_id))
-                        .value(ros_easy_mode_env_value + ":" + std::to_string(domain_id))
-                        .build_and_call();
-#ifndef _WIN32
-        // Adecuate Python subprocess return
-        res = WEXITSTATUS(res);
-#endif // _WIN32
-
-        if (res != SystemCommandBuilder::SystemCommandResult::SUCCESS)
-        {
-            if (res == SystemCommandBuilder::SystemCommandResult::BAD_PARAM)
-            {
-                EPROSIMA_LOG_ERROR("DOMAIN", "ROS2_EASY_MODE IP connection conflicts with a previous one.");
-            }
-            else
-            {
-                EPROSIMA_LOG_ERROR(DOMAIN, "Auto discovery server client setup. Unable to spawn daemon.");
-            }
-            return nullptr;
-        }
     }
 
-    RTPSParticipant* part = createParticipant(domain_id, enabled, client_att, listen);
-    if (nullptr != part)
-    {
-        // Client successfully created
-        EPROSIMA_LOG_INFO(RTPS_DOMAIN, "Auto default server-client setup. Default client created.");
-        part->mp_impl->client_override(true);
-        return part;
-    }
+    att = client_att;
 
-    // Unable to create auto server-client default participants
-    EPROSIMA_LOG_ERROR(RTPS_DOMAIN, "Auto default server-client setup. Unable to create the client.");
-    return nullptr;
+    return true;
 }
 
 uint32_t RTPSDomainImpl::getNewId()
@@ -934,6 +1000,39 @@ fastdds::dds::xtypes::TypeObjectRegistry& RTPSDomainImpl::type_object_registry_o
     return get_instance()->type_object_registry_;
 }
 
+bool RTPSDomainImpl::run_easy_mode_discovery_server(
+        uint32_t domain_id,
+        const std::string& ip)
+{
+    SystemCommandBuilder sys_command;
+    int res = sys_command.executable(FAST_DDS_DEFAULT_CLI_SCRIPT_NAME)
+                    .verb(FAST_DDS_DEFAULT_CLI_DISCOVERY_VERB)
+                    .verb(FAST_DDS_DEFAULT_CLI_AUTO_VERB)
+                    .arg("-d")
+                    .value(std::to_string(domain_id))
+                    .value(ip + ":" + std::to_string(domain_id))
+                    .build_and_call();
+#ifndef _WIN32
+    // Adecuate Python subprocess return
+    res = WEXITSTATUS(res);
+#endif // _WIN32
+
+    if (res != SystemCommandBuilder::SystemCommandResult::SUCCESS)
+    {
+        if (res == SystemCommandBuilder::SystemCommandResult::BAD_PARAM)
+        {
+            EPROSIMA_LOG_ERROR("DOMAIN", "ROS2_EASY_MODE IP connection conflicts with a previous one.");
+        }
+        else
+        {
+            EPROSIMA_LOG_ERROR(DOMAIN, "Auto discovery server client setup. Unable to spawn daemon.");
+        }
+        return false;
+    }
+
+    return true;
+}
+
 } // namespace rtps
 } // namespace fastdds
 } // namespace eprosima

src/cpp/rtps/RTPSDomainImpl.hpp

@@ -90,6 +90,27 @@ class RTPSDomainImpl
             const RTPSParticipantAttributes& attrs,
             RTPSParticipantListener* plisten);
 
+    /**
+     * @brief Create a RTPSParticipant as default server or client if ROS_MASTER_URI environment variable is set.
+     * It also configures ROS 2 Easy Mode IP if the following conditions are met:
+     * 1. ROS2_EASY_MODE_URI environment variable is set.
+     * 2. `easy_mode_ip` member of the input RTPSParticipantAttributes is an empty string.
+     *
+     * @param domain_id DomainId to be used by the RTPSParticipant.
+     * @param enabled True if the RTPSParticipant should be enabled on creation. False if it will be enabled later with RTPSParticipant::enable()
+     * @param attrs RTPSParticipant Attributes.
+     * @param plisten Pointer to the ParticipantListener.
+     * @return Pointer to the RTPSParticipant.
+     *
+     * \warning The returned pointer is invalidated after a call to removeRTPSParticipant() or stopAll(),
+     *          so its use may result in undefined behaviour.
+     */
+    static RTPSParticipant* create_client_server_participant(
+            uint32_t domain_id,
+            bool enabled,
+            const RTPSParticipantAttributes& attrs,
+            RTPSParticipantListener* plisten);
+
     /**
      * Remove a RTPSWriter.
      * @param writer Pointer to the writer you want to remove.
@@ -115,21 +136,20 @@ class RTPSDomainImpl
             RTPSParticipant* p);
 
     /**
-     * Creates a RTPSParticipant as default server or client if ROS_MASTER_URI environment variable is set.
+     * Fills RTPSParticipantAttributes to create a RTPSParticipant as default server or client
+     * if ROS_MASTER_URI environment variable is set.
+     * It also configures ROS 2 Easy Mode IP if ROS2_EASY_MODE_URI environment variable is set
+     * and it was empty in the input attributes.
+     *
      * @param domain_id DDS domain associated
-     * @param enabled True if the RTPSParticipant should be enabled on creation. False if it will be enabled later with RTPSParticipant::enable()
-     * @param attrs RTPSParticipant Attributes.
-     * @param listen Pointer to the ParticipantListener.
-     * @return Pointer to the RTPSParticipant.
+     * @param [in, out] attrs RTPSParticipant Attributes.
+     * @return True if the attributes were successfully modified,
+     * false if an error occurred or environment variable not set
      *
-     * \warning The returned pointer is invalidated after a call to removeRTPSParticipant() or stopAll(),
-     *          so its use may result in undefined behaviour.
      */
-    static RTPSParticipant* clientServerEnvironmentCreationOverride(
+    static bool client_server_environment_attributes_override(
             uint32_t domain_id,
-            bool enabled,
-            const RTPSParticipantAttributes& attrs,
-            RTPSParticipantListener* listen /*= nullptr*/);
+            RTPSParticipantAttributes& attrs);
 
     /**
      * Create a RTPSWriter in a participant.
@@ -255,6 +275,18 @@ class RTPSDomainImpl
      */
     static fastdds::dds::xtypes::TypeObjectRegistry& type_object_registry_observer();
 
+    /**
+     * @brief Run the Easy Mode discovery server using the Fast DDS CLI command
+     *
+     * @param domain_id Domain ID to use for the discovery server
+     * @param easy_mode_ip IP address to use for the discovery server
+     *
+     * @return True if the server was successfully started, false otherwise.
+     */
+    static bool run_easy_mode_discovery_server(
+            uint32_t domain_id,
+            const std::string& easy_mode_ip);
+
 private:
 
     /**