Skip to content

Commit

Permalink
Add connection timeout and respawn options (#105)
Browse files Browse the repository at this point in the history
  • Loading branch information
@QuentinTorg
QuentinTorg authored Oct 29, 2024
1 parent b4901aa commit 22ed8f3
Show file tree
Hide file tree
Showing 3 changed files with 64 additions and 21 deletions.
15 changes: 9 additions & 6 deletions multisense_bringup/multisense.launch
View file
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. Values &lt;3 may result in false positives)" />
<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
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. Values &lt;3 may result in false positives)" />
<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
43 changes: 38 additions & 5 deletions multisense_ros/src/ros_driver.cpp
View file
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;
int 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<int>("camera_timeout_s", timeout_s, -1);

Channel *d = NULL;

Expand Down Expand Up @@ -121,7 +123,38 @@ 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{static_cast<double>(timeout_s)};

ros::Time last_warning{};
ros::Duration warn_delay{1.1};
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) {
ROS_WARN("multisense_ros: missed camera status message");
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");
Channel::Destroy(d);
return -5;
}

} else {
last_warning = ros::Time::now();
last_status = ros::Time::now();
}

rate.sleep();
}
}

Channel::Destroy(d);
Expand Down

0 comments on commit 22ed8f3

Please sign in to comment.