From 16f13a8d94bd950050320019ad00167c5e3af8d3 Mon Sep 17 00:00:00 2001 From: Parth-Sanghvi Date: Thu, 18 Feb 2021 08:47:20 -0800 Subject: [PATCH 1/3] Error Forwarding for Realsense connections --- .../src/realsense_node_factory.cpp | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/realsense2_camera/src/realsense_node_factory.cpp b/realsense2_camera/src/realsense_node_factory.cpp index 248df921e7..6be247dc6a 100644 --- a/realsense2_camera/src/realsense_node_factory.cpp +++ b/realsense2_camera/src/realsense_node_factory.cpp @@ -91,6 +91,15 @@ void RealSenseNodeFactory::getDevice(rs2::device_list list) if (0 == list.size()) { ROS_WARN("No RealSense devices were found!"); + ros::Publisher chatter_pub = nh.advertise("realsense_missoncontrol_health", 1000); + ros::Rate loop_rate(10); + std_msgs::String msg; + std::stringstream ss; + ss << "ERROR, Connection Check"; + msg.data = ss.str(); + chatter_pub.publish(msg); + ros::spinOnce(); + loop_rate.sleep(); } else { @@ -223,6 +232,15 @@ void RealSenseNodeFactory::change_device_callback(rs2::event_information& info) if (info.was_removed(_device)) { ROS_ERROR("The device has been disconnected!"); + ros::Publisher chatter_pub = nh.advertise("realsense_missoncontrol_health", 1000); + ros::Rate loop_rate(10); + std_msgs::String msg; + std::stringstream ss; + ss << "ERROR, Connection Check"; + msg.data = ss.str(); + chatter_pub.publish(msg); + ros::spinOnce(); + loop_rate.sleep(); _realSenseNode.reset(nullptr); _device = rs2::device(); } From c056f8af40f9caaabd540c5f4145960fac574420 Mon Sep 17 00:00:00 2001 From: Parth Sanghvi <53913628+Parth-Sanghvi@users.noreply.github.com> Date: Fri, 19 Feb 2021 13:03:16 -0800 Subject: [PATCH 2/3] Update realsense_node_factory.cpp --- realsense2_camera/src/realsense_node_factory.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/realsense2_camera/src/realsense_node_factory.cpp b/realsense2_camera/src/realsense_node_factory.cpp index 6be247dc6a..01ce24c415 100644 --- a/realsense2_camera/src/realsense_node_factory.cpp +++ b/realsense2_camera/src/realsense_node_factory.cpp @@ -92,14 +92,14 @@ void RealSenseNodeFactory::getDevice(rs2::device_list list) { ROS_WARN("No RealSense devices were found!"); ros::Publisher chatter_pub = nh.advertise("realsense_missoncontrol_health", 1000); - ros::Rate loop_rate(10); - std_msgs::String msg; - std::stringstream ss; - ss << "ERROR, Connection Check"; - msg.data = ss.str(); - chatter_pub.publish(msg); - ros::spinOnce(); - loop_rate.sleep(); + ros::Rate loop_rate(10); + std_msgs::String msg; + std::stringstream ss; + ss << "ERROR, Connection Check"; + msg.data = ss.str(); + chatter_pub.publish(msg); + ros::spinOnce(); + loop_rate.sleep(); } else { From 9fe14010c9b17cc4d62b06970649f1f59991fed4 Mon Sep 17 00:00:00 2001 From: Ed Friese Date: Tue, 23 Feb 2021 22:12:29 +0000 Subject: [PATCH 3/3] keep health publisher alive for lifetime of realsense node --- .../include/realsense_node_factory.h | 3 +- .../src/realsense_node_factory.cpp | 42 +++++++++---------- 2 files changed, 22 insertions(+), 23 deletions(-) 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 6be247dc6a..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,16 +91,13 @@ void RealSenseNodeFactory::getDevice(rs2::device_list list) { if (0 == list.size()) { - ROS_WARN("No RealSense devices were found!"); - ros::Publisher chatter_pub = nh.advertise("realsense_missoncontrol_health", 1000); - ros::Rate loop_rate(10); - std_msgs::String msg; - std::stringstream ss; - ss << "ERROR, Connection Check"; - msg.data = ss.str(); - chatter_pub.publish(msg); - ros::spinOnce(); - loop_rate.sleep(); + 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 { @@ -141,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()) @@ -218,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) { @@ -231,17 +229,15 @@ void RealSenseNodeFactory::change_device_callback(rs2::event_information& info) { if (info.was_removed(_device)) { - ROS_ERROR("The device has been disconnected!"); - ros::Publisher chatter_pub = nh.advertise("realsense_missoncontrol_health", 1000); - ros::Rate loop_rate(10); - std_msgs::String msg; std::stringstream ss; - ss << "ERROR, Connection Check"; - msg.data = ss.str(); - chatter_pub.publish(msg); - ros::spinOnce(); - loop_rate.sleep(); - _realSenseNode.reset(nullptr); + 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) @@ -288,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()) { {