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

ptp: Add support for PTP timestamps with UTC/TAI configuration params and diagnostics. #43

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

ptp: Add support for PTP timestamps with UTC/TAI configuration params and diagnostics. #43

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
6308691
Account for PTP timestamps being 0 before valid ptp lock.
jackg0 Jun 4, 2021
0ed5ea8
Add optional ptp time offset parameter.
jackg0 Jun 15, 2021
2ce2ad4
Change param to match units elsewhere.
jackg0 Jun 15, 2021
c3e2396
Add ptp status diagnostic.
jackg0 Jan 25, 2022
9117b5c
Add function for timestamp conversion
jackg0 Jan 25, 2022
ff64bb4
Fix comments in PR.
jackg0 Jan 28, 2022
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
16 changes: 13 additions & 3 deletions multisense_ros/include/multisense_ros/camera.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -348,11 +348,21 @@ class Camera {
//
// Diagnostics
diagnostic_updater::Updater diagnostic_updater_;
void deviceInfoDiagnostic(diagnostic_updater::DiagnosticStatusWrapper &stat);
void deviceStatusDiagnostic(diagnostic_updater::DiagnosticStatusWrapper &stat);
void deviceInfoDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat);
void deviceStatusDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat);
void ptpStatusDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat);

void diagnosticTimerCallback(const ros::TimerEvent &);
void diagnosticTimerCallback(const ros::TimerEvent&);
ros::Timer diagnostic_trigger_;

//
// Timestamping and timesync settings
ros::Time imageTimestampToRosTime(uint32_t time_secs, uint32_t time_microsecs);

bool ptp_time_sync_ = false;
bool network_time_sync_ = false;
std::atomic_bool ptp_time_stamp_in_use_;
int32_t ptp_time_offset_secs_ = 0;
};

}
Expand Down
67 changes: 56 additions & 11 deletions multisense_ros/src/camera.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -302,7 +302,8 @@ Camera::Camera(Channel* driver, const std::string& tf_prefix) :
pointcloud_max_range_(15.0),
last_frame_id_(-1),
border_clip_type_(BorderClip::NONE),
border_clip_value_(0.0)
border_clip_value_(0.0),
ptp_time_stamp_in_use_(false)
{
//
// Query device and version information from sensor
Expand All @@ -319,6 +320,12 @@ Camera::Camera(Channel* driver, const std::string& tf_prefix) :
return;
}

//
// Get timesync parameters
device_nh_.getParam("ptp_time_offset_secs", ptp_time_offset_secs_);
device_nh_.getParam("ptp_time_sync", ptp_time_sync_);
device_nh_.getParam("network_time_sync", network_time_sync_);

//
// Get the camera config

Expand Down Expand Up @@ -735,6 +742,7 @@ Camera::Camera(Channel* driver, const std::string& tf_prefix) :
diagnostic_updater_.setHardwareID(device_info_.name + " " + std::to_string(device_info_.hardwareRevision));
diagnostic_updater_.add("device_info", this, &Camera::deviceInfoDiagnostic);
diagnostic_updater_.add("device_status", this, &Camera::deviceStatusDiagnostic);
diagnostic_updater_.add("ptp_status", this, &Camera::ptpStatusDiagnostic);
diagnostic_trigger_ = device_nh_.createTimer(ros::Duration(1), &Camera::diagnosticTimerCallback, this);
}

Expand Down Expand Up @@ -827,8 +835,7 @@ void Camera::histogramCallback(const image::Header& header)
Status status = driver_->getImageHistogram(header.frameId, mh);
if (Status_Ok == status) {
rh.frame_count = header.frameId;
rh.time_stamp = ros::Time(header.timeSeconds,
1000 * header.timeMicroSeconds);
rh.time_stamp = imageTimestampToRosTime(header.timeSeconds, header.timeMicroSeconds);
rh.width = header.width;
rh.height = header.height;
switch(header.source) {
Expand All @@ -855,7 +862,7 @@ void Camera::jpegImageCallback(const image::Header& header)
return;
}

const ros::Time t = ros::Time(header.timeSeconds, 1000 * header.timeMicroSeconds);
ros::Time t = imageTimestampToRosTime(header.timeSeconds, header.timeMicroSeconds);

const uint32_t height = header.height;
const uint32_t width = header.width;
Expand Down Expand Up @@ -925,7 +932,7 @@ void Camera::disparityImageCallback(const image::Header& header)

const uint32_t imageSize = (header.width * header.height * header.bitsPerPixel) / 8;

const ros::Time t = ros::Time(header.timeSeconds, 1000 * header.timeMicroSeconds);
ros::Time t = imageTimestampToRosTime(header.timeSeconds, header.timeMicroSeconds);

switch(header.source) {
case Source_Disparity:
Expand Down Expand Up @@ -1094,7 +1101,7 @@ void Camera::monoCallback(const image::Header& header)
return;
}

ros::Time t = ros::Time(header.timeSeconds, 1000 * header.timeMicroSeconds);
ros::Time t = imageTimestampToRosTime(header.timeSeconds, header.timeMicroSeconds);

switch(header.source) {
case Source_Luma_Left:
Expand Down Expand Up @@ -1202,7 +1209,7 @@ void Camera::rectCallback(const image::Header& header)
return;
}

ros::Time t = ros::Time(header.timeSeconds, 1000 * header.timeMicroSeconds);
ros::Time t = imageTimestampToRosTime(header.timeSeconds, header.timeMicroSeconds);

switch(header.source) {
case Source_Luma_Rectified_Left:
Expand Down Expand Up @@ -1334,7 +1341,7 @@ void Camera::depthCallback(const image::Header& header)
return;
}

const ros::Time t(header.timeSeconds, 1000 * header.timeMicroSeconds);
ros::Time t = imageTimestampToRosTime(header.timeSeconds, header.timeMicroSeconds);

const float bad_point = std::numeric_limits<float>::quiet_NaN();
const uint32_t depthSize = header.height * header.width * sizeof(float);
Expand Down Expand Up @@ -1507,7 +1514,7 @@ void Camera::pointCloudCallback(const image::Header& header)
return;
}

const ros::Time t(header.timeSeconds, 1000 * header.timeMicroSeconds);
ros::Time t = imageTimestampToRosTime(header.timeSeconds, header.timeMicroSeconds);

//
// Resize our corresponding pointclouds if we plan on publishing them
Expand Down Expand Up @@ -1760,7 +1767,7 @@ void Camera::rawCamDataCallback(const image::Header& header)
raw_cam_data_.gain = left_luma_rect.gain;
raw_cam_data_.exposure_time = left_luma_rect.exposure;
raw_cam_data_.frame_count = left_luma_rect.frameId;
raw_cam_data_.time_stamp = ros::Time(left_luma_rect.timeSeconds, 1000 * left_luma_rect.timeMicroSeconds);
raw_cam_data_.time_stamp = imageTimestampToRosTime(header.timeSeconds, header.timeMicroSeconds);
raw_cam_data_.width = left_luma_rect.width;
raw_cam_data_.height = left_luma_rect.height;

Expand Down Expand Up @@ -1789,7 +1796,7 @@ void Camera::colorImageCallback(const image::Header& header)
return;
}

const ros::Time t(header.timeSeconds, 1000 * header.timeMicroSeconds);
ros::Time t = imageTimestampToRosTime(header.timeSeconds, header.timeMicroSeconds);

switch (header.source)
{
Expand Down Expand Up @@ -2080,6 +2087,25 @@ void Camera::groundSurfaceSplineCallback(const ground_surface::Header& header)
ground_surface_spline_pub_.publish(ground_surface_utilities::eigenToPointcloud(eigen_pcl, frame_id_origin_));
}

ros::Time Camera::imageTimestampToRosTime(uint32_t time_secs, uint32_t time_microsecs)
{
// When camera has ptp enabled and there is no ptp lock,
// the image timestamp equals 0
if (time_secs == 0)
{
return ros::Time::now();
}

auto time_stamp = ros::Time(time_secs, 1000 * time_microsecs);
if (ptp_time_sync_ && time_secs > std::abs(ptp_time_offset_secs_))
{
time_stamp += ros::Duration(ptp_time_offset_secs_);
ptp_time_stamp_in_use_ = true;
}

return time_stamp;
}

void Camera::updateConfig(const image::Config& config)
{
stereo_calibration_manager_->updateConfig(config);
Expand Down Expand Up @@ -2264,6 +2290,25 @@ void Camera::deviceStatusDiagnostic(diagnostic_updater::DiagnosticStatusWrapper&
}
}

void Camera::ptpStatusDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat)
{
stat.add("ptp enabled", ptp_time_sync_);
stat.add("ptp status", ptp_time_stamp_in_use_);
if (!ptp_time_sync_)
{
stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "PTP status: Disabled.");
return;
}

if (ptp_time_stamp_in_use_)
{
stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "PTP status: OK");
} else {
stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "PTP status: Uncalibrated");
}
ptp_time_stamp_in_use_ = false;
}

void Camera::diagnosticTimerCallback(const ros::TimerEvent&)
{
diagnostic_updater_.update();
Expand Down