diff --git a/realsense2_camera/include/realsense_node_factory.h b/realsense2_camera/include/realsense_node_factory.h index ff9f9c01bc..4d0f494ac8 100644 --- a/realsense2_camera/include/realsense_node_factory.h +++ b/realsense2_camera/include/realsense_node_factory.h @@ -35,7 +35,7 @@ namespace realsense2_camera const stream_index_pair GYRO{RS2_STREAM_GYRO, 0}; const stream_index_pair ACCEL{RS2_STREAM_ACCEL, 0}; const stream_index_pair POSE{RS2_STREAM_POSE, 0}; - const stream_index_pair CONFIDENCE{RS2_STREAM_CONFIDENCE, 0}; + const stream_index_pair CONFIDENCE{RS2_STREAM_CONFIDENCE, 0}; const std::vector IMAGE_STREAMS = {DEPTH, INFRA0, INFRA1, INFRA2, COLOR, @@ -79,6 +79,7 @@ namespace realsense2_camera std::thread _query_thread; bool _is_alive; ros::ServiceServer toggle_sensor_srv; + ros::Publisher health_pub; }; }//end namespace diff --git a/realsense2_camera/src/realsense_node_factory.cpp b/realsense2_camera/src/realsense_node_factory.cpp index 248df921e7..f6b9cba3f7 100644 --- a/realsense2_camera/src/realsense_node_factory.cpp +++ b/realsense2_camera/src/realsense_node_factory.cpp @@ -11,6 +11,7 @@ #include #include #include +#include using namespace realsense2_camera; @@ -90,7 +91,13 @@ void RealSenseNodeFactory::getDevice(rs2::device_list list) { if (0 == list.size()) { - ROS_WARN("No RealSense devices were found!"); + std::stringstream ss; + ss << "ERROR: No Realsense device found"; + ROS_ERROR_STREAM(ss.str()); + + std_msgs::String msg; + msg.data = ss.str(); + health_pub.publish(msg); } else { @@ -132,7 +139,7 @@ void RealSenseNodeFactory::getDevice(rs2::device_list list) } else { - ROS_INFO_STREAM("Device with port number " << port_id << " was found."); + ROS_INFO_STREAM("Device with port number " << port_id << " was found."); } bool found_device_type(true); if (!_device_type.empty()) @@ -209,7 +216,7 @@ void RealSenseNodeFactory::getDevice(rs2::device_list list) ROS_INFO("Resetting device..."); _device.hardware_reset(); _device = rs2::device(); - + } catch(const std::exception& ex) { @@ -222,8 +229,15 @@ void RealSenseNodeFactory::change_device_callback(rs2::event_information& info) { if (info.was_removed(_device)) { - ROS_ERROR("The device has been disconnected!"); - _realSenseNode.reset(nullptr); + std::stringstream ss; + ss << "ERROR: Realsense device removed"; + ROS_ERROR_STREAM(ss.str()); + + std_msgs::String msg; + msg.data = ss.str(); + health_pub.publish(msg); + + _realSenseNode.reset(nullptr); _device = rs2::device(); } if (!_device) @@ -270,6 +284,8 @@ void RealSenseNodeFactory::onInit() std::string rosbag_filename(""); privateNh.param("rosbag_filename", rosbag_filename, std::string("")); + health_pub = nh.advertise("health", 1, true); + if (!rosbag_filename.empty()) { {