-
Notifications
You must be signed in to change notification settings - Fork 18
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.
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
Add connection timeout and respawn options #105
base: master
Are you sure you want to change the base?
Changes from 1 commit
d6934c3
ab3b2d2
4165a53
0bef68c
File filter
Filter by extension
Conversations
Jump to
Diff view
Diff view
There are no files selected for viewing
Original file line number | Diff line number | Diff line change |
---|---|---|
|
@@ -53,16 +53,18 @@ int main(int argc, char** argvPP) | |
// | ||
// Get parameters from ROS/command-line | ||
|
||
std::string sensor_ip; | ||
std::string tf_prefix; | ||
int sensor_mtu; | ||
int head_id; | ||
std::string sensor_ip; | ||
std::string tf_prefix; | ||
int sensor_mtu; | ||
int head_id; | ||
double timeout_s; | ||
|
||
|
||
nh_private_.param<std::string>("sensor_ip", sensor_ip, "10.66.171.21"); | ||
nh_private_.param<std::string>("tf_prefix", tf_prefix, "multisense"); | ||
nh_private_.param<int>("sensor_mtu", sensor_mtu, 1500); | ||
nh_private_.param<int>("head_id", head_id, -1); | ||
nh_private_.param<double>("camera_timeout_s", timeout_s, -1.0); | ||
|
||
Channel *d = NULL; | ||
|
||
|
@@ -121,7 +123,37 @@ int main(int argc, char** argvPP) | |
std::placeholders::_1), | ||
std::bind(&multisense_ros::Camera::groundSurfaceSplineDrawParametersChanged, &camera, | ||
std::placeholders::_1)); | ||
ros::spin(); | ||
|
||
ros::Rate rate(50); | ||
ros::Time last_status{ros::Time::now()}; | ||
const ros::Duration timeout{timeout_s}; | ||
|
||
ros::Time last_warning{}; | ||
ros::Duration warn_delay{0.5}; | ||
while (ros::ok()) { | ||
ros::spinOnce(); | ||
|
||
system::StatusMessage statusMessage; | ||
const auto status_result = d->getDeviceStatus(statusMessage); | ||
if (Status_Ok != status_result) { | ||
|
||
if (ros::Time::now() - last_warning > warn_delay) { | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Seems like this might be too chatty when a single status message is dropped |
||
ROS_WARN("multisense_ros: lost connection with camera"); | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. This warning is fairly aggressive. Consider softening the language. |
||
last_warning = ros::Time::now(); | ||
} | ||
|
||
if (timeout > ros::Duration{0} && ros::Time::now() - last_status > timeout) { | ||
ROS_ERROR("multisense_ros: shutting down due to connection timeout"); | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Consider changing this to something like "reinitializing connection with the MultiSense" There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. I'm not sure what less harsh comment would be applicable. During shutdown, we only respawn if the respawn argument was also passed, so I don't quite want to say that here. After this comment, we also exit with an error code, so it seems acceptable to be harsh |
||
Channel::Destroy(d); | ||
return -5; | ||
} | ||
|
||
} else { | ||
last_status = ros::Time::now(); | ||
} | ||
|
||
rate.sleep(); | ||
} | ||
} | ||
|
||
Channel::Destroy(d); | ||
|
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
This will only get updated at 1Hz under the hood.