Skip to content

ROS1 - add wait_for_device_timeout parameter #2156

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 1 commit into from
Nov 15, 2021
Merged

ROS1 - add wait_for_device_timeout parameter #2156

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 3 additions & 1 deletion realsense2_camera/launch/includes/nodelet.launch.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -105,6 +105,7 @@
<arg name="linear_accel_cov" default="0.01"/>
<arg name="initial_reset" default="false"/>
<arg name="reconnect_timeout" default= "6.0"/>
<arg name="wait_for_device_timeout" default= "-1.0"/>
<arg name="unite_imu_method" default="none"/> <!-- Options are: [none, copy, linear_interpolation] -->


Expand Down Expand Up @@ -209,7 +210,8 @@
<param name="clip_distance" type="double" value="$(arg clip_distance)"/>
<param name="linear_accel_cov" type="double" value="$(arg linear_accel_cov)"/>
<param name="initial_reset" type="bool" value="$(arg initial_reset)"/>
<param name="reconnect_timeout" type="double" value="$(arg reconnect_timeout)"/>
<param name="reconnect_timeout" type="double" value="$(arg reconnect_timeout)"/>
<param name="wait_for_device_timeout" type="double" value="$(arg wait_for_device_timeout)"/>
<param name="unite_imu_method" type="str" value="$(arg unite_imu_method)"/>

</node>
Expand Down
2 changes: 2 additions & 0 deletions realsense2_camera/launch/rs_camera.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -60,6 +60,7 @@
<arg name="linear_accel_cov" default="0.01"/>
<arg name="initial_reset" default="false"/>
<arg name="reconnect_timeout" default="6.0"/>
<arg name="wait_for_device_timeout" default="-1.0"/>
<arg name="unite_imu_method" default=""/>
<arg name="topic_odom_in" default="odom_in"/>
<arg name="calib_odom_file" default=""/>
Expand Down Expand Up @@ -131,6 +132,7 @@
<arg name="linear_accel_cov" value="$(arg linear_accel_cov)"/>
<arg name="initial_reset" value="$(arg initial_reset)"/>
<arg name="reconnect_timeout" value="$(arg reconnect_timeout)"/>
<arg name="wait_for_device_timeout" value="$(arg wait_for_device_timeout)"/>
<arg name="unite_imu_method" value="$(arg unite_imu_method)"/>
<arg name="topic_odom_in" value="$(arg topic_odom_in)"/>
<arg name="calib_odom_file" value="$(arg calib_odom_file)"/>
Expand Down
22 changes: 20 additions & 2 deletions realsense2_camera/src/realsense_node_factory.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<double> 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<int>(reconnect_timeout*1e3));
ros::Time first_try_time = ros::Time::now();
while (_is_alive && !_device)
{
getDevice(_ctx.query_devices());
Expand All @@ -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<std::chrono::seconds>(timespan).count());
actual_timespan = std::chrono::milliseconds (static_cast<int>(std::min(max_timespan_secs, time_to_timeout) * 1e3));
}
}
std::this_thread::sleep_for(actual_timespan);
}
}
});
Expand Down