diff --git a/realsense2_camera/launch/includes/nodelet.launch.xml b/realsense2_camera/launch/includes/nodelet.launch.xml index 82fa3203f5..8452b4259d 100644 --- a/realsense2_camera/launch/includes/nodelet.launch.xml +++ b/realsense2_camera/launch/includes/nodelet.launch.xml @@ -105,6 +105,7 @@ + @@ -209,7 +210,8 @@ - + + diff --git a/realsense2_camera/launch/rs_camera.launch b/realsense2_camera/launch/rs_camera.launch index b721247bf4..75b0026bef 100644 --- a/realsense2_camera/launch/rs_camera.launch +++ b/realsense2_camera/launch/rs_camera.launch @@ -60,6 +60,7 @@ + @@ -131,6 +132,7 @@ + diff --git a/realsense2_camera/src/realsense_node_factory.cpp b/realsense2_camera/src/realsense_node_factory.cpp index c86488ce85..3eb7e96183 100644 --- a/realsense2_camera/src/realsense_node_factory.cpp +++ b/realsense2_camera/src/realsense_node_factory.cpp @@ -292,7 +292,10 @@ void RealSenseNodeFactory::initialize(const ros::WallTimerEvent &ignored) { double reconnect_timeout; privateNh.param("reconnect_timeout", reconnect_timeout, 6.0); - std::chrono::duration timespan(reconnect_timeout); + double wait_for_device_timeout; + privateNh.param("wait_for_device_timeout", wait_for_device_timeout, -1.0); + std::chrono::milliseconds timespan(static_cast(reconnect_timeout*1e3)); + ros::Time first_try_time = ros::Time::now(); while (_is_alive && !_device) { getDevice(_ctx.query_devices()); @@ -304,7 +307,22 @@ void RealSenseNodeFactory::initialize(const ros::WallTimerEvent &ignored) } else { - std::this_thread::sleep_for(timespan); + std::chrono::milliseconds actual_timespan(timespan); + if (wait_for_device_timeout > 0) + { + auto time_to_timeout(wait_for_device_timeout - (ros::Time::now() - first_try_time).toSec()); + if (time_to_timeout < 0) + { + ROS_ERROR_STREAM("wait for device timeout of " << wait_for_device_timeout << " secs expired"); + exit(1); + } + else + { + double max_timespan_secs(std::chrono::duration_cast(timespan).count()); + actual_timespan = std::chrono::milliseconds (static_cast(std::min(max_timespan_secs, time_to_timeout) * 1e3)); + } + } + std::this_thread::sleep_for(actual_timespan); } } });