Skip to content
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

Jump to bottom

Add connection timeout and respawn options #105

Open
wants to merge 4 commits into
base: master
Choose a base branch
from
Open

Add connection timeout and respawn options #105

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
d6934c3
add options for timeout on disconnect and respawn if node dies
QuentinTorg Oct 23, 2024
ab3b2d2
clean up error messages slightly
QuentinTorg Oct 25, 2024
4165a53
increase delay time slightly to reduce likelihood that we print for a…
QuentinTorg Oct 25, 2024
0bef68c
Merge remote-tracking branch 'origin/master' into qtorgerson/respawn
QuentinTorg Oct 25, 2024
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
15 changes: 9 additions & 6 deletions multisense_bringup/multisense.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -27,36 +27,39 @@
<arg name="launch_color_laser_publisher" default="false" doc="Set true to launch the laser colorization publisher for SL cameras" />
<arg name="nodes_prefix" default="$(arg namespace)" doc="Prefix used for naming the nodes on launch" />
<arg name="tf_prefix" default="$(arg namespace)" doc="Prefix used for transform names" />
<arg name="camera_timeout_s" default="-1" doc="Camera driver will shut down if it does not hear from the camera after this many seconds. (Disabled when &lt;= 0)" />
<arg name="respawn" default="false" doc="Respawn the multisense node if the camera disconnects" />

<!-- Robot state publisher -->
<group if = "$(arg launch_robot_state_publisher)">
<param name="robot_description"
command="$(find xacro)/xacro '$(find multisense_description)/urdf/multisense$(arg sensor)/standalone.urdf.xacro' name:=$(arg namespace)"/>
<node pkg="robot_state_publisher" type="robot_state_publisher" name="$(arg nodes_prefix)_state_publisher">
command="$(find xacro)/xacro '$(find multisense_description)/urdf/multisense$(arg sensor)/standalone.urdf.xacro' name:=$(arg namespace)" />
<node pkg="robot_state_publisher" type="robot_state_publisher" name="$(arg nodes_prefix)_state_publisher" respawn="$(arg respawn)" >
<param name="publish_frequency" type="double" value="50.0" />
<remap from="joint_states" to="/$(arg namespace)/joint_states" />
</node>
</group>

<!-- ROS Driver -->
<node pkg="multisense_ros" ns="$(arg namespace)" type="ros_driver" name="$(arg nodes_prefix)_driver" output="screen">
<node pkg="multisense_ros" ns="$(arg namespace)" type="ros_driver" name="$(arg nodes_prefix)_driver" output="screen" respawn="$(arg respawn)" >
<param name="sensor_ip" value="$(arg ip_address)" />
<param name="sensor_mtu" value="$(arg mtu)" />
<param name="tf_prefix" value="$(arg tf_prefix)" />
<param name="head_id" value="$(arg head_id)" />
<param name="camera_timeout_s" value="$(arg camera_timeout_s)" />
</node>

<!-- Color Laser PointCloud Publisher -->
<group if = "$(arg launch_color_laser_publisher)">
<node pkg="multisense_ros" ns="$(arg namespace)" type="color_laser_publisher" name="$(arg nodes_prefix)_color_laser_publisher" output="screen">
<node pkg="multisense_ros" ns="$(arg namespace)" type="color_laser_publisher" name="$(arg nodes_prefix)_color_laser_publisher" output="screen" respawn="$(arg respawn)" >
<remap from="image_rect_color" to="/$(arg namespace)/left/image_rect_color" />
<remap from="lidar_points2" to="/$(arg namespace)/lidar_points2" />
<remap from="camera_info" to="/$(arg namespace)/left/image_rect_color/camera_info" />
</node>
</group>

<node if="$(arg show_rviz)" type="rviz" name="rviz" pkg="rviz" required="false"
launch-prefix="bash -c 'sleep 5; $0 $@' "
args="-d $(arg rviz_config)"/>
launch-prefix="bash -c 'sleep 5; $0 $@' "
args="-d $(arg rviz_config)"/>

</launch>
27 changes: 17 additions & 10 deletions multisense_bringup/remote_head.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -36,23 +36,26 @@
<arg name="head3_tf_prefix" default="$(arg head3_namespace)" />

<arg name="launch_robot_state_publisher" default="true" />
<arg name="camera_timeout_s" default="-1" doc="Camera driver will shut down if it does not hear from the camera after this many seconds. (Disabled when &lt;= 0)" />
<arg name="respawn" default="false" doc="Respawn the multisense node if the camera disconnects" />

<!-- Remote Head VPB Launch-->
<group if = "$(arg launch_vpb)">

<!-- ROS Driver -->
<node pkg="multisense_ros" ns="$(arg vpb_namespace)" type="ros_driver" name="$(arg vpb_nodes_prefix)_driver" output="screen" required="true">
<node pkg="multisense_ros" ns="$(arg vpb_namespace)" type="ros_driver" name="$(arg vpb_nodes_prefix)_driver" output="screen" required="true" respawn="$(arg respawn)" >
<param name="sensor_ip" value="$(arg ip_address)" />
<param name="sensor_mtu" value="$(arg mtu)" />
<param name="tf_prefix" value="$(arg vpb_tf_prefix)" />
<param name="head_id" value="-1" />
<param name="camera_timeout_s" value="$(arg camera_timeout_s)" />
</node>

<!-- Robot state publisher -->
<group if = "$(arg launch_robot_state_publisher)">
<param name="robot_description"
command="$(find xacro)/xacro '$(find multisense_description)/urdf/multisense$(arg vpb_sensor)/standalone.urdf.xacro' name:=$(arg vpb_namespace)"/>
<node pkg="robot_state_publisher" type="robot_state_publisher" name="$(arg vpb_nodes_prefix)_state_publisher" required="true">
<node pkg="robot_state_publisher" type="robot_state_publisher" name="$(arg vpb_nodes_prefix)_state_publisher" required="true" respawn="$(arg respawn)" >
<param name="publish_frequency" type="double" value="50.0" />
<remap from="joint_states" to="/$(arg vpb_namespace)/joint_states" />
</node>
Expand All @@ -65,18 +68,19 @@
<group if = "$(arg launch_head0)">

<!-- ROS Driver -->
<node pkg="multisense_ros" ns="$(arg head0_namespace)" type="ros_driver" name="$(arg head0_nodes_prefix)_driver" output="screen" required="true">
<node pkg="multisense_ros" ns="$(arg head0_namespace)" type="ros_driver" name="$(arg head0_nodes_prefix)_driver" output="screen" required="true" respawn="$(arg respawn)" >
<param name="sensor_ip" value="$(arg ip_address)" />
<param name="sensor_mtu" value="$(arg mtu)" />
<param name="tf_prefix" value="$(arg head0_tf_prefix)" />
<param name="head_id" value="0" />
<param name="camera_timeout_s" value="$(arg camera_timeout_s)" />
</node>

<!-- Robot state publisher -->
<group if = "$(arg launch_robot_state_publisher)">
<param name="robot_description"
command="$(find xacro)/xacro '$(find multisense_description)/urdf/multisense$(arg head0_sensor)/standalone.urdf.xacro' name:=$(arg head0_namespace)"/>
<node pkg="robot_state_publisher" type="robot_state_publisher" name="$(arg head0_nodes_prefix)_state_publisher" required="true">
<node pkg="robot_state_publisher" type="robot_state_publisher" name="$(arg head0_nodes_prefix)_state_publisher" required="true" respawn="$(arg respawn)" >
<param name="publish_frequency" type="double" value="50.0" />
<remap from="joint_states" to="/$(arg head0_namespace)/joint_states" />
</node>
Expand All @@ -89,18 +93,19 @@
<group if = "$(arg launch_head1)">

<!-- ROS Driver -->
<node pkg="multisense_ros" ns="$(arg head1_namespace)" type="ros_driver" name="$(arg head1_nodes_prefix)_driver" output="screen" required="true">
<node pkg="multisense_ros" ns="$(arg head1_namespace)" type="ros_driver" name="$(arg head1_nodes_prefix)_driver" output="screen" required="true" respawn="$(arg respawn)" >
<param name="sensor_ip" value="$(arg ip_address)" />
<param name="sensor_mtu" value="$(arg mtu)" />
<param name="tf_prefix" value="$(arg head1_tf_prefix)" />
<param name="head_id" value="1" />
<param name="camera_timeout_s" value="$(arg camera_timeout_s)" />
</node>

<!-- Robot state publisher -->
<group if = "$(arg launch_robot_state_publisher)">
<param name="robot_description"
command="$(find xacro)/xacro '$(find multisense_description)/urdf/multisense$(arg head1_sensor)/standalone.urdf.xacro' name:=$(arg head1_namespace)"/>
<node pkg="robot_state_publisher" type="robot_state_publisher" name="$(arg head1_nodes_prefix)_state_publisher" required="true">
<node pkg="robot_state_publisher" type="robot_state_publisher" name="$(arg head1_nodes_prefix)_state_publisher" required="true" respawn="$(arg respawn)" >
<param name="publish_frequency" type="double" value="50.0" />
<remap from="joint_states" to="/$(arg head1_namespace)/joint_states" />
</node>
Expand All @@ -113,18 +118,19 @@
<group if = "$(arg launch_head2)">

<!-- ROS Driver -->
<node pkg="multisense_ros" ns="$(arg head2_namespace)" type="ros_driver" name="$(arg head2_nodes_prefix)_driver" output="screen" required="true">
<node pkg="multisense_ros" ns="$(arg head2_namespace)" type="ros_driver" name="$(arg head2_nodes_prefix)_driver" output="screen" required="true" respawn="$(arg respawn)" >
<param name="sensor_ip" value="$(arg ip_address)" />
<param name="sensor_mtu" value="$(arg mtu)" />
<param name="tf_prefix" value="$(arg head2_tf_prefix)" />
<param name="head_id" value="2" />
<param name="camera_timeout_s" value="$(arg camera_timeout_s)" />
</node>

<!-- Robot state publisher -->
<group if = "$(arg launch_robot_state_publisher)">
<param name="robot_description"
command="$(find xacro)/xacro '$(find multisense_description)/urdf/multisense$(arg head2_sensor)/standalone.urdf.xacro' name:=$(arg head2_namespace)"/>
<node pkg="robot_state_publisher" type="robot_state_publisher" name="$(arg head2_nodes_prefix)_state_publisher" required="true">
<node pkg="robot_state_publisher" type="robot_state_publisher" name="$(arg head2_nodes_prefix)_state_publisher" required="true" respawn="$(arg respawn)" >
<param name="publish_frequency" type="double" value="50.0" />
<remap from="joint_states" to="/$(arg head2_namespace)/joint_states" />
</node>
Expand All @@ -137,18 +143,19 @@
<group if = "$(arg launch_head3)">

<!-- ROS Driver -->
<node pkg="multisense_ros" ns="$(arg head3_namespace)" type="ros_driver" name="$(arg head3_nodes_prefix)_driver" output="screen" required="true">
<node pkg="multisense_ros" ns="$(arg head3_namespace)" type="ros_driver" name="$(arg head3_nodes_prefix)_driver" output="screen" required="true" respawn="$(arg respawn)" >
<param name="sensor_ip" value="$(arg ip_address)" />
<param name="sensor_mtu" value="$(arg mtu)" />
<param name="tf_prefix" value="$(arg head3_tf_prefix)" />
<param name="head_id" value="3" />
<param name="camera_timeout_s" value="$(arg camera_timeout_s)" />
</node>

<!-- Robot state publisher -->
<group if = "$(arg launch_robot_state_publisher)">
<param name="robot_description"
command="$(find xacro)/xacro '$(find multisense_description)/urdf/multisense$(arg head3_sensor)/standalone.urdf.xacro' name:=$(arg head3_namespace)"/>
<node pkg="robot_state_publisher" type="robot_state_publisher" name="$(arg head3_nodes_prefix)_state_publisher" required="true">
<node pkg="robot_state_publisher" type="robot_state_publisher" name="$(arg head3_nodes_prefix)_state_publisher" required="true" respawn="$(arg respawn)" >
<param name="publish_frequency" type="double" value="50.0" />
<remap from="joint_states" to="/$(arg head3_namespace)/joint_states" />
</node>
Expand Down
42 changes: 37 additions & 5 deletions multisense_ros/src/ros_driver.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down Expand Up @@ -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);
Copy link
Contributor

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.

if (Status_Ok != status_result) {

if (ros::Time::now() - last_warning > warn_delay) {
Copy link
Contributor

Choose a reason for hiding this comment

The 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");
Copy link
Contributor

Choose a reason for hiding this comment

The 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");
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Consider changing this to something like "reinitializing connection with the MultiSense"

Copy link
Collaborator Author

Choose a reason for hiding this comment

The 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);
Expand Down