Skip to content

Commit

Permalink
Merge pull request #352 from ruvu/feature/diagnostics
Browse files Browse the repository at this point in the history 
Feature/diagnostics
  • Loading branch information
@icarpis
icarpis authored Mar 29, 2018
2 parents 9489eaa + fd266c1 commit 859cb61
Show file tree
Hide file tree
Showing 5 changed files with 44 additions and 9 deletions.
1 change: 1 addition & 0 deletions realsense2_camera/.gitignore
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1 @@
CMakeLists.txt.user
1 change: 1 addition & 0 deletions realsense2_camera/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,7 @@ find_package(catkin REQUIRED COMPONENTS
image_transport
tf
dynamic_reconfigure
diagnostic_updater
)

if(BUILD_WITH_OPENMP)
Expand Down
33 changes: 30 additions & 3 deletions realsense2_camera/include/base_realsense_node.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,9 @@
#include <realsense2_camera/rs415_paramsConfig.h>
#include <realsense2_camera/rs435_paramsConfig.h>

#include <diagnostic_updater/diagnostic_updater.h>
#include <diagnostic_updater/update_functions.h>


namespace realsense2_camera
{
Expand All @@ -24,6 +27,30 @@ namespace realsense2_camera
base_depth_count
};

struct FrequencyDiagnostics
{
FrequencyDiagnostics(double expected_frequency, std::string name, std::string hardware_id) :
expected_frequency_(expected_frequency),
frequency_status_(diagnostic_updater::FrequencyStatusParam(&expected_frequency_, &expected_frequency_)),
diagnostic_updater_(ros::NodeHandle(), ros::NodeHandle("~"), ros::this_node::getName() + "_" + name)
{
ROS_INFO("Expected frequency for %s = %.5f", name.c_str(), expected_frequency_);
diagnostic_updater_.setHardwareID(hardware_id);
diagnostic_updater_.add(frequency_status_);
}

void update()
{
frequency_status_.tick();
diagnostic_updater_.update();
}

double expected_frequency_;
diagnostic_updater::FrequencyStatus frequency_status_;
diagnostic_updater::Updater diagnostic_updater_;
};
typedef std::pair<image_transport::Publisher, std::shared_ptr<FrequencyDiagnostics>> ImagePublisherWithFrequencyDiagnostics;

class BaseRealSenseNode : public InterfaceRealSenseNode
{
public:
Expand Down Expand Up @@ -76,7 +103,7 @@ namespace realsense2_camera
const stream_index_pair& stream,
std::map<stream_index_pair, cv::Mat>& images,
const std::map<stream_index_pair, ros::Publisher>& info_publishers,
const std::map<stream_index_pair, image_transport::Publisher>& image_publishers,
const std::map<stream_index_pair, ImagePublisherWithFrequencyDiagnostics>& image_publishers,
std::map<stream_index_pair, int>& seq,
std::map<stream_index_pair, sensor_msgs::CameraInfo>& camera_info,
const std::map<stream_index_pair, std::string>& optical_frame_id,
Expand Down Expand Up @@ -110,7 +137,7 @@ namespace realsense2_camera
std::map<stream_index_pair, std::string> _stream_name;
tf2_ros::StaticTransformBroadcaster _static_tf_broadcaster;

std::map<stream_index_pair, image_transport::Publisher> _image_publishers;
std::map<stream_index_pair, ImagePublisherWithFrequencyDiagnostics> _image_publishers;
std::map<stream_index_pair, ros::Publisher> _imu_publishers;
std::map<stream_index_pair, int> _image_format;
std::map<stream_index_pair, rs2_format> _format;
Expand Down Expand Up @@ -141,7 +168,7 @@ namespace realsense2_camera
std::map<stream_index_pair, sensor_msgs::CameraInfo> _depth_aligned_camera_info;
std::map<stream_index_pair, int> _depth_aligned_seq;
std::map<stream_index_pair, ros::Publisher> _depth_aligned_info_publisher;
std::map<stream_index_pair, image_transport::Publisher> _depth_aligned_image_publishers;
std::map<stream_index_pair, ImagePublisherWithFrequencyDiagnostics> _depth_aligned_image_publishers;
std::map<stream_index_pair, std::string> _depth_aligned_frame_id;
std::map<stream_index_pair, ros::Publisher> _depth_to_other_extrinsics_publishers;
std::map<stream_index_pair, rs2_extrinsics> _depth_to_other_extrinsics;
Expand Down
2 changes: 2 additions & 0 deletions realsense2_camera/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,7 @@
<build_depend>image_transport</build_depend>
<build_depend>tf</build_depend>
<build_depend>dynamic_reconfigure</build_depend>
<build_depend>diagnostic_updater</build_depend>
<build_depend>librealsense2</build_depend>
<run_depend>image_transport</run_depend>
<run_depend>cv_bridge</run_depend>
Expand All @@ -34,6 +35,7 @@
<run_depend>message_runtime</run_depend>
<run_depend>tf</run_depend>
<run_depend>dynamic_reconfigure</run_depend>
<run_depend>diagnostic_updater</run_depend>
<run_depend>librealsense2</run_depend>
<export>
<nodelet plugin="${prefix}/nodelet_plugins.xml" />
Expand Down
16 changes: 10 additions & 6 deletions realsense2_camera/src/base_realsense_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -303,7 +303,8 @@ void BaseRealSenseNode::setupPublishers()
image_raw << _stream_name[stream] << "/image_" << ((rectified_image)?"rect_":"") << "raw";
camera_info << _stream_name[stream] << "/camera_info";

_image_publishers[stream] = image_transport.advertise(image_raw.str(), 1);
std::shared_ptr<FrequencyDiagnostics> frequency_diagnostics(new FrequencyDiagnostics(_fps[stream], _stream_name[stream], _serial_no));
_image_publishers[stream] = {image_transport.advertise(image_raw.str(), 1), frequency_diagnostics};
_info_publisher[stream] = _node_handle.advertise<sensor_msgs::CameraInfo>(camera_info.str(), 1);

if (_align_depth && (stream != DEPTH))
Expand All @@ -312,7 +313,9 @@ void BaseRealSenseNode::setupPublishers()
aligned_image_raw << "aligned_depth_to_" << _stream_name[stream] << "/image_raw";
aligned_camera_info << "aligned_depth_to_" << _stream_name[stream] << "/camera_info";

_depth_aligned_image_publishers[stream] = image_transport.advertise(aligned_image_raw.str(), 1);
std::string aligned_stream_name = "aligned_depth_to_" + _stream_name[stream];
std::shared_ptr<FrequencyDiagnostics> frequency_diagnostics(new FrequencyDiagnostics(_fps[stream], aligned_stream_name, _serial_no));
_depth_aligned_image_publishers[stream] = {image_transport.advertise(aligned_image_raw.str(), 1), frequency_diagnostics};
_depth_aligned_info_publisher[stream] = _node_handle.advertise<sensor_msgs::CameraInfo>(aligned_camera_info.str(), 1);
}

Expand Down Expand Up @@ -452,7 +455,7 @@ void BaseRealSenseNode::publishAlignedDepthToOthers(rs2::frame depth_frame, cons
auto& info_publisher = _depth_aligned_info_publisher.at(sip);
auto& image_publisher = _depth_aligned_image_publishers.at(sip);
if(0 != info_publisher.getNumSubscribers() ||
0 != image_publisher.getNumSubscribers())
0 != image_publisher.first.getNumSubscribers())
{
auto from_image_frame = depth_frame.as<rs2::video_frame>();
auto& out_vec = _aligned_depth_images[sip];
Expand Down Expand Up @@ -1197,7 +1200,7 @@ void BaseRealSenseNode::publishFrame(rs2::frame f, const ros::Time& t,
const stream_index_pair& stream,
std::map<stream_index_pair, cv::Mat>& images,
const std::map<stream_index_pair, ros::Publisher>& info_publishers,
const std::map<stream_index_pair, image_transport::Publisher>& image_publishers,
const std::map<stream_index_pair, ImagePublisherWithFrequencyDiagnostics>& image_publishers,
std::map<stream_index_pair, int>& seq,
std::map<stream_index_pair, sensor_msgs::CameraInfo>& camera_info,
const std::map<stream_index_pair, std::string>& optical_frame_id,
Expand All @@ -1214,7 +1217,7 @@ void BaseRealSenseNode::publishFrame(rs2::frame f, const ros::Time& t,
auto& info_publisher = info_publishers.at(stream);
auto& image_publisher = image_publishers.at(stream);
if(0 != info_publisher.getNumSubscribers() ||
0 != image_publisher.getNumSubscribers())
0 != image_publisher.first.getNumSubscribers())
{
auto width = 0;
auto height = 0;
Expand Down Expand Up @@ -1242,7 +1245,8 @@ void BaseRealSenseNode::publishFrame(rs2::frame f, const ros::Time& t,
cam_info.header.seq = seq[stream];
info_publisher.publish(cam_info);

image_publisher.publish(img);
image_publisher.first.publish(img);
image_publisher.second->update();
ROS_DEBUG("%s stream published", rs2_stream_to_string(f.get_profile().stream_type()));
}
}
Expand Down

0 comments on commit 859cb61

Please sign in to comment.