Merge pull request #331 from mateusz-lichota/wait-for-rpc-connection

Add waiting for RPC server to become available
FS-Driverless · Sep 15, 2022 · 7f52b83 · 7f52b83
2 parents c0b5580 + a72e77d
commit 7f52b83
Show file tree

Hide file tree

Showing 11 changed files with 62 additions and 37 deletions.
diff --git a/AirSim/AirLib/include/api/RpcLibClientBase.hpp b/AirSim/AirLib/include/api/RpcLibClientBase.hpp
@@ -26,7 +26,7 @@ class RpcLibClientBase {
     RpcLibClientBase(const string& ip_address = "localhost", uint16_t port = RpcLibPort, float timeout_sec = 60);
     virtual ~RpcLibClientBase();    //required for pimpl
 
-    void confirmConnection();
+    void confirmConnection(double timeout);
     void reset();
 
     ConnectionState getConnectionState();
@@ -107,6 +107,10 @@ class RpcLibClientBase {
     const void* getClient() const;
 
 private:
+    std::string ip_address_;
+    uint16_t port_;
+    float timeout_sec_;
+
     struct impl;
     std::unique_ptr<impl> pimpl_;
 };

diff --git a/AirSim/AirLib/src/api/RpcLibClientBase.cpp b/AirSim/AirLib/src/api/RpcLibClientBase.cpp
@@ -58,9 +58,9 @@ struct RpcLibClientBase::impl {
 
 typedef msr::airlib_rpclib::RpcLibAdapatorsBase RpcLibAdapatorsBase;
 
-RpcLibClientBase::RpcLibClientBase(const string&  ip_address, uint16_t port, float timeout_sec)
+RpcLibClientBase::RpcLibClientBase(const string&  ip_address, uint16_t port, float timeout_sec) : ip_address_(ip_address), port_(port), timeout_sec_(timeout_sec)
 {
-    pimpl_.reset(new impl(ip_address, port, timeout_sec));
+    pimpl_.reset(new impl(ip_address_, port_, timeout_sec_));
 }
 
 RpcLibClientBase::~RpcLibClientBase()
@@ -111,11 +111,21 @@ void RpcLibClientBase::reset()
     pimpl_->client.call("reset");
 }
 
-void RpcLibClientBase::confirmConnection()
+void RpcLibClientBase::confirmConnection(double timeout)
 {
     ClockBase* clock = ClockFactory::get();
 
-    clock->sleep_for(1);
+    double delay = 0.2;
+    for (double elapsed = 0.0; elapsed <= timeout; elapsed += delay) {
+        pimpl_.reset(new impl(ip_address_, port_, timeout_sec_));
+        if (getConnectionState() == RpcLibClientBase::ConnectionState::Connected)
+        {
+            break;
+        }
+        clock->sleep_for(delay);
+    }
+
+
     if (getConnectionState() != RpcLibClientBase::ConnectionState::Connected)
     {
         throw std::runtime_error("Failed connecting to RPC server (airsim). Is the simulator running?");

diff --git a/ros/src/fsds_ros_bridge/include/airsim_ros_wrapper.h b/ros/src/fsds_ros_bridge/include/airsim_ros_wrapper.h
@@ -74,10 +74,10 @@ struct SimpleMatrix
 class AirsimROSWrapper
 {
 public:
-    AirsimROSWrapper(const ros::NodeHandle& nh, const ros::NodeHandle& nh_private, const std::string& host_ip);
+    AirsimROSWrapper(const ros::NodeHandle& nh, const ros::NodeHandle& nh_private, const std::string& host_ip, double timeout_sec);
     ~AirsimROSWrapper(){};
 
-    void initialize_airsim();
+    void initialize_airsim(double timeout);
     void initialize_ros();
     void publish_track();
     void initialize_statistics();
@@ -223,4 +223,4 @@ class AirsimROSWrapper
     /// ROS subscribers
     ros::Subscriber control_cmd_sub;
     ros::Subscriber finished_signal_sub_;
-};
+};
diff --git a/ros/src/fsds_ros_bridge/scripts/cameralauncher.py b/ros/src/fsds_ros_bridge/scripts/cameralauncher.py
@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/env python3
 import roslaunch
 from os.path import expanduser
 import json 
@@ -62,4 +62,4 @@ def signal_handler(sig, frame):
   launch.spin()
 finally:
   # After Ctrl+C, stop all nodes from running
-  launch.stop()
+  launch.stop()
diff --git a/ros/src/fsds_ros_bridge/src/airsim_ros_wrapper.cpp b/ros/src/fsds_ros_bridge/src/airsim_ros_wrapper.cpp
@@ -4,17 +4,19 @@
 // PLUGINLIB_EXPORT_CLASS(AirsimROSWrapper, nodelet::Nodelet)
 #include "common/AirSimSettings.hpp"
 
-AirsimROSWrapper::AirsimROSWrapper(const ros::NodeHandle& nh, const ros::NodeHandle& nh_private, const std::string& host_ip) : nh_(nh),
+AirsimROSWrapper::AirsimROSWrapper(const ros::NodeHandle& nh, const ros::NodeHandle& nh_private, const std::string& host_ip, double timeout_sec) : nh_(nh),
                                                                                                                                nh_private_(nh_private),
                                                                                                                                lidar_async_spinner_(1, &lidar_timer_cb_queue_), // same as above, but for lidar
-                                                                                                                               airsim_client_(host_ip),
-                                                                                                                               airsim_client_lidar_(host_ip)
+                                                                                                                               airsim_client_(host_ip, RpcLibPort, timeout_sec),
+                                                                                                                               airsim_client_lidar_(host_ip, RpcLibPort, timeout_sec)
 {
+    initialize_airsim(timeout_sec);
     try {
-        airsim_client_.confirmConnection();
+        ROS_INFO_STREAM("READING SETTINGS");
         std::string settings_text = airsim_client_.getSettingsString();
-        msr::airlib::AirSimSettings::initializeSettings(settings_text);
+        ROS_INFO_STREAM("SETTINGS READ");
 
+        msr::airlib::AirSimSettings::initializeSettings(settings_text);
         msr::airlib::AirSimSettings::singleton().load();
         for (const auto &warning : msr::airlib::AirSimSettings::singleton().warning_messages)
         {
@@ -35,14 +37,15 @@ AirsimROSWrapper::AirsimROSWrapper(const ros::NodeHandle& nh, const ros::NodeHan
     ROS_INFO_STREAM("AirsimROSWrapper Initialized!");
 }
 
-void AirsimROSWrapper::initialize_airsim()
+void AirsimROSWrapper::initialize_airsim(double timeout)
 {
     // todo do not reset if already in air?
     try
     {
+        double elapsed = 0.0;
         ROS_INFO_STREAM("Waiting for connection - ");
-        airsim_client_.confirmConnection();
-        airsim_client_lidar_.confirmConnection();
+        airsim_client_.confirmConnection(timeout);
+        airsim_client_lidar_.confirmConnection(timeout);
         ROS_INFO_STREAM("Connected to the simulator!");
     }
     catch (rpc::rpc_error& e)
@@ -257,8 +260,6 @@ void AirsimROSWrapper::create_ros_pubs_from_settings_json()
     }
 
     reset_srvr_ = nh_.advertiseService("reset", &AirsimROSWrapper::reset_srv_cb, this);
-
-    initialize_airsim();
 }
 
 ros::Time AirsimROSWrapper::make_ts(uint64_t unreal_ts)

diff --git a/ros/src/fsds_ros_bridge/src/fsds_ros_bridge.cpp b/ros/src/fsds_ros_bridge/src/fsds_ros_bridge.cpp
@@ -9,8 +9,11 @@ int main(int argc, char ** argv)
     ros::NodeHandle nh_private("~");
 
     std::string host_ip = "localhost";
+    double timeout_sec = 10;
     nh_private.getParam("host_ip", host_ip);
-    AirsimROSWrapper airsim_ros_wrapper(nh, nh_private, host_ip);
+    nh_private.getParam("timeout", timeout_sec);
+
+    AirsimROSWrapper airsim_ros_wrapper(nh, nh_private, host_ip, timeout_sec);
 
     if (airsim_ros_wrapper.is_used_lidar_timer_cb_queue_)
     {

diff --git a/ros/src/fsds_ros_bridge/src/fsds_ros_bridge_camera.cpp b/ros/src/fsds_ros_bridge/src/fsds_ros_bridge_camera.cpp
@@ -162,9 +162,12 @@ int main(int argc, char ** argv)
     msr::airlib::CarRpcLibClient client(host_ip, RpcLibPort, 5);
     airsim_api = &client;
 
+    double timeout_sec = 10.0;
+    nh.getParam("timeout", timeout_sec);
+
     try {
         std::cout << "Waiting for connection - " << std::endl;
-        airsim_api->confirmConnection();
+        airsim_api->confirmConnection(timeout_sec);
         std::cout << "Connected to the simulator!" << std::endl;
     } catch (const std::exception &e) {
         std::string msg = e.what();

diff --git a/ros2/src/fsds_ros2_bridge/include/airsim_ros_wrapper.h b/ros2/src/fsds_ros2_bridge/include/airsim_ros_wrapper.h
@@ -75,10 +75,10 @@ struct SimpleMatrix
 class AirsimROSWrapper
 {
 public:
-    AirsimROSWrapper(const std::shared_ptr<rclcpp::Node>& nh, const std::string& host_ip);
+    AirsimROSWrapper(const std::shared_ptr<rclcpp::Node>& nh, const std::string& host_ip, double timeout_sec);
     ~AirsimROSWrapper(){};
 
-    void initialize_airsim();
+    void initialize_airsim(double timeout_sec);
     void initialize_ros();
     void publish_track();
     void initialize_statistics();
@@ -182,6 +182,7 @@ class AirsimROSWrapper
     bool competition_mode_;
     rclcpp::Time go_timestamp_;
 
+    std::string host_ip_;
     msr::airlib::CarRpcLibClient airsim_client_;
     msr::airlib::CarRpcLibClient airsim_client_lidar_;
 

diff --git a/ros2/src/fsds_ros2_bridge/src/airsim_ros_wrapper.cpp b/ros2/src/fsds_ros2_bridge/src/airsim_ros_wrapper.cpp
@@ -6,14 +6,14 @@
 
 using dseconds = std::chrono::duration<double>;
 
-AirsimROSWrapper::AirsimROSWrapper(const std::shared_ptr<rclcpp::Node>& nh, const std::string& host_ip) 
+AirsimROSWrapper::AirsimROSWrapper(const std::shared_ptr<rclcpp::Node>& nh, const std::string& host_ip, double timeout_sec) 
                                                                                                   : nh_(nh),
-                                                                                                    airsim_client_(host_ip),
-                                                                                                    airsim_client_lidar_(host_ip),
+                                                                                                    airsim_client_(host_ip, RpcLibPort, timeout_sec),
+                                                                                                    airsim_client_lidar_(host_ip, RpcLibPort, timeout_sec),
                                                                                                     static_tf_pub_(this->nh_)
 {
+    initialize_airsim(timeout_sec);
     try {
-        airsim_client_.confirmConnection();
         std::string settings_text = airsim_client_.getSettingsString();
         msr::airlib::AirSimSettings::initializeSettings(settings_text);
 
@@ -37,14 +37,16 @@ AirsimROSWrapper::AirsimROSWrapper(const std::shared_ptr<rclcpp::Node>& nh, cons
     RCLCPP_INFO(nh_->get_logger(), "AirsimROSWrapper Initialized!");
 }
 
-void AirsimROSWrapper::initialize_airsim()
+void AirsimROSWrapper::initialize_airsim(double timeout)
 {
     // todo do not reset if already in air?
     try
-    {
-        RCLCPP_INFO(nh_->get_logger(), "Waiting for connection");
-        airsim_client_.confirmConnection();
-        airsim_client_lidar_.confirmConnection();
+    {   
+        double elapsed = 0.0;
+        RCLCPP_INFO_STREAM(nh_->get_logger(), "Waiting for connection");
+
+        airsim_client_.confirmConnection(timeout);
+        airsim_client_lidar_.confirmConnection(timeout);
         RCLCPP_INFO(nh_->get_logger(), "Connected to the simulator!");
     }
     catch (rpc::rpc_error& e)
@@ -270,8 +272,6 @@ void AirsimROSWrapper::create_ros_pubs_from_settings_json()
     }
 
     reset_srvr_ = nh_->create_service<fs_msgs::srv::Reset>("reset", std::bind(&AirsimROSWrapper::reset_srv_cb, this, std::placeholders::_1, std::placeholders::_2));
-
-    initialize_airsim();
 }
 
 rclcpp::Time AirsimROSWrapper::make_ts(uint64_t unreal_ts) const

diff --git a/ros2/src/fsds_ros2_bridge/src/fsds_ros2_bridge.cpp b/ros2/src/fsds_ros2_bridge/src/fsds_ros2_bridge.cpp
@@ -8,8 +8,9 @@ int main(int argc, char ** argv)
     std::shared_ptr<rclcpp::Node> node = rclcpp::Node::make_shared("fsds_ros2_bridge"); 
 
     std::string host_ip = node->declare_parameter<std::string>("host_ip", "localhost");
+    double timeout_sec = node->declare_parameter<double>("timeout", 10.0);
 
-    AirsimROSWrapper airsim_ros_wrapper(node, host_ip);
+    AirsimROSWrapper airsim_ros_wrapper(node, host_ip, timeout_sec);
 
     // if (airsim_ros_wrapper.is_used_lidar_timer_cb_queue_)
     // {

diff --git a/ros2/src/fsds_ros2_bridge/src/fsds_ros2_bridge_camera.cpp b/ros2/src/fsds_ros2_bridge/src/fsds_ros2_bridge_camera.cpp
@@ -188,9 +188,11 @@ int main(int argc, char ** argv)
     msr::airlib::CarRpcLibClient client(host_ip, RpcLibPort, 5);
     airsim_api = &client;
 
+    double timeout_sec = nh->declare_parameter<double>("timeout", 10.0);
+
     try {
         std::cout << "Waiting for connection - " << std::endl;
-        airsim_api->confirmConnection();
+        airsim_api->confirmConnection(timeout_sec);
         std::cout << "Connected to the simulator!" << std::endl;
     } catch (const std::exception &e) {
         std::string msg = e.what();