From 6d21d2b77c767c61bec04b62d954013a389006bc Mon Sep 17 00:00:00 2001 From: Mateusz Lichota Date: Sat, 10 Sep 2022 21:47:43 +0200 Subject: [PATCH 1/3] Add waiting for RPC server to become available --- .../AirLib/include/api/RpcLibClientBase.hpp | 6 ++++- AirSim/AirLib/src/api/RpcLibClientBase.cpp | 18 +++++++++++---- .../include/airsim_ros_wrapper.h | 4 ++-- .../include/airsim_ros_wrapper.h | 5 +++-- .../src/airsim_ros_wrapper.cpp | 22 +++++++++---------- .../fsds_ros2_bridge/src/fsds_ros2_bridge.cpp | 3 ++- .../src/fsds_ros2_bridge_camera.cpp | 4 +++- 7 files changed, 40 insertions(+), 22 deletions(-) diff --git a/AirSim/AirLib/include/api/RpcLibClientBase.hpp b/AirSim/AirLib/include/api/RpcLibClientBase.hpp index a69236bb..d21792a1 100644 --- 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 pimpl_; }; diff --git a/AirSim/AirLib/src/api/RpcLibClientBase.cpp b/AirSim/AirLib/src/api/RpcLibClientBase.cpp index f7292339..f0660dbd 100644 --- 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 index 29c73d58..50e7613b 100644 --- a/ros/src/fsds_ros_bridge/include/airsim_ros_wrapper.h +++ b/ros/src/fsds_ros_bridge/include/airsim_ros_wrapper.h @@ -77,7 +77,7 @@ class AirsimROSWrapper AirsimROSWrapper(const ros::NodeHandle& nh, const ros::NodeHandle& nh_private, const std::string& host_ip); ~AirsimROSWrapper(){}; - void initialize_airsim(); + void initialize_airsim(TTimeDelta 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_; -}; \ No newline at end of file +}; diff --git a/ros2/src/fsds_ros2_bridge/include/airsim_ros_wrapper.h b/ros2/src/fsds_ros2_bridge/include/airsim_ros_wrapper.h index eb7d211b..f7273a85 100644 --- 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& nh, const std::string& host_ip); + AirsimROSWrapper(const std::shared_ptr& 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 index 071f8647..fd33f20e 100644 --- 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; -AirsimROSWrapper::AirsimROSWrapper(const std::shared_ptr& nh, const std::string& host_ip) +AirsimROSWrapper::AirsimROSWrapper(const std::shared_ptr& 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& 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("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 index 98f8457a..dc9d80fe 100644 --- 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 node = rclcpp::Node::make_shared("fsds_ros2_bridge"); std::string host_ip = node->declare_parameter("host_ip", "localhost"); + double timeout_sec = node->declare_parameter("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 index ec731dae..5cb24ab9 100644 --- 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("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(); From f10d4ff60e04cef5e707d3e13db33e1b624a3649 Mon Sep 17 00:00:00 2001 From: Wouter Heerwegh Date: Wed, 14 Sep 2022 16:58:54 +0200 Subject: [PATCH 2/3] Add timeout to ROS1 (to be tested) --- .../fsds_ros_bridge/include/airsim_ros_wrapper.h | 2 +- .../fsds_ros_bridge/scripts/cameralauncher.py | 4 ++-- .../fsds_ros_bridge/src/airsim_ros_wrapper.cpp | 16 ++++++++-------- ros/src/fsds_ros_bridge/src/fsds_ros_bridge.cpp | 5 ++++- .../src/fsds_ros_bridge_camera.cpp | 5 ++++- 5 files changed, 19 insertions(+), 13 deletions(-) diff --git a/ros/src/fsds_ros_bridge/include/airsim_ros_wrapper.h b/ros/src/fsds_ros_bridge/include/airsim_ros_wrapper.h index 50e7613b..86c16b7a 100644 --- a/ros/src/fsds_ros_bridge/include/airsim_ros_wrapper.h +++ b/ros/src/fsds_ros_bridge/include/airsim_ros_wrapper.h @@ -74,7 +74,7 @@ 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(TTimeDelta timeout); diff --git a/ros/src/fsds_ros_bridge/scripts/cameralauncher.py b/ros/src/fsds_ros_bridge/scripts/cameralauncher.py index 01d89d92..80c49f77 100755 --- 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() \ No newline at end of file + 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 index 4ef2846e..0b8169c6 100644 --- a/ros/src/fsds_ros_bridge/src/airsim_ros_wrapper.cpp +++ b/ros/src/fsds_ros_bridge/src/airsim_ros_wrapper.cpp @@ -4,12 +4,13 @@ // 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(); std::string settings_text = airsim_client_.getSettingsString(); @@ -35,14 +36,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 +259,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 index 031f1a35..f512b20c 100644 --- 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 index 95cf7a63..d98eb0fe 100644 --- 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(); From a72e77d472de937bf9264a23b8aba4746afb13de Mon Sep 17 00:00:00 2001 From: Wouter Heerwegh Date: Wed, 14 Sep 2022 17:06:42 +0200 Subject: [PATCH 3/3] Rest of changes --- ros/src/fsds_ros_bridge/include/airsim_ros_wrapper.h | 2 +- ros/src/fsds_ros_bridge/src/airsim_ros_wrapper.cpp | 5 +++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/ros/src/fsds_ros_bridge/include/airsim_ros_wrapper.h b/ros/src/fsds_ros_bridge/include/airsim_ros_wrapper.h index 86c16b7a..a3cf7efb 100644 --- a/ros/src/fsds_ros_bridge/include/airsim_ros_wrapper.h +++ b/ros/src/fsds_ros_bridge/include/airsim_ros_wrapper.h @@ -77,7 +77,7 @@ class AirsimROSWrapper AirsimROSWrapper(const ros::NodeHandle& nh, const ros::NodeHandle& nh_private, const std::string& host_ip, double timeout_sec); ~AirsimROSWrapper(){}; - void initialize_airsim(TTimeDelta timeout); + void initialize_airsim(double timeout); void initialize_ros(); void publish_track(); void initialize_statistics(); diff --git a/ros/src/fsds_ros_bridge/src/airsim_ros_wrapper.cpp b/ros/src/fsds_ros_bridge/src/airsim_ros_wrapper.cpp index 0b8169c6..0ad18511 100644 --- a/ros/src/fsds_ros_bridge/src/airsim_ros_wrapper.cpp +++ b/ros/src/fsds_ros_bridge/src/airsim_ros_wrapper.cpp @@ -12,10 +12,11 @@ AirsimROSWrapper::AirsimROSWrapper(const ros::NodeHandle& nh, const ros::NodeHan { 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) {