fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-24 03:53:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

microRTPS: allow timesync using ROS time

Browse Source
This commit is contained in:
TSC21
2021-08-12 08:44:53 +02:00
committed by Nuno Marques
parent e762d57222
commit 6d4f65a47a
4 changed files with 169 additions and 45 deletions
Download Patch File Download Diff File Expand all files Collapse all files
msg/templates/urtps/microRTPS_timesync.cpp.em
+54 -7
View File
@@ -68,6 +68,9 @@ except AttributeError:
TimeSync::TimeSync(bool debug)
: _offset_ns(-1),
@[if ros2_distro]@
_timesync_node(std::make_shared<rclcpp::Node>("timesync_node")),
@[end if]@
_skew_ns_per_sync(0.0),
_num_samples(0),
_request_reset_counter(0),
@@ -82,6 +85,12 @@ void TimeSync::start(TimesyncPublisher *pub)
{
stop();
@[if ros2_distro]@
auto spin_node = [this]() {
rclcpp::spin(_timesync_node);
};
@[end if]@
auto run = [this, pub]() {
while (!_request_stop) {
timesync_msg_t msg = newTimesyncMsg();
@@ -92,6 +101,9 @@ void TimeSync::start(TimesyncPublisher *pub)
}
};
_request_stop = false;
@[if ros2_distro]@
_timesync_node_thread.reset(new std::thread(spin_node));
@[end if]@
_send_timesync_thread.reset(new std::thread(run));
}
@@ -99,6 +111,9 @@ void TimeSync::stop()
{
_request_stop = true;
@[if ros2_distro]@
if (_timesync_node_thread && _timesync_node_thread->joinable()) { _timesync_node_thread->join(); }
@[end if]@
if (_send_timesync_thread && _send_timesync_thread->joinable()) { _send_timesync_thread->join(); }
}
@@ -108,17 +123,29 @@ void TimeSync::reset()
_request_reset_counter = 0;
}
int64_t TimeSync::getTimeNSec()
@[if ros2_distro]@
int64_t TimeSync::getROSTimeNSec()
{
return _timesync_node->now().nanoseconds();
}
int64_t TimeSync::getROSTimeUSec()
{
return RCL_NS_TO_US(getROSTimeNSec());
}
@[else]@
int64_t TimeSync::getSteadyTimeNSec()
{
auto time = std::chrono::steady_clock::now();
return std::chrono::time_point_cast<std::chrono::nanoseconds>(time).time_since_epoch().count();
}
int64_t TimeSync::getTimeUSec()
int64_t TimeSync::getSteadyTimeUSec()
{
auto time = std::chrono::steady_clock::now();
return std::chrono::time_point_cast<std::chrono::microseconds>(time).time_since_epoch().count();
}
@[end if]@
bool TimeSync::addMeasurement(int64_t local_t1_ns, int64_t remote_t2_ns, int64_t local_t3_ns)
{
@@ -187,15 +214,27 @@ void TimeSync::processTimesyncMsg(timesync_msg_t *msg, TimesyncPublisher *pub)
_last_remote_msg_seq = getMsgSeq(msg);
if (getMsgTC1(msg) > 0) {
if (!addMeasurement(getMsgTS1(msg), getMsgTC1(msg), getTimeNSec())) {
@[if ros2_distro]@
if (!addMeasurement(getMsgTS1(msg), getMsgTC1(msg), getROSTimeNSec())) {
@[else]@
if (!addMeasurement(getMsgTS1(msg), getMsgTC1(msg), getSteadyTimeNSec())) {
@[end if]@
if (_debug) { std::cerr << "\033[1;33m[ micrortps__timesync ]\tOffset not updated\033[0m" << std::endl; }
}
} else if (getMsgTC1(msg) == 0) {
setMsgTimestamp(msg, getTimeUSec());
@[if ros2_distro]@
setMsgTimestamp(msg, getROSTimeUSec());
@[else]@
setMsgTimestamp(msg, getSteadyTimeUSec());
@[end if]@
setMsgSysID(msg, 0);
setMsgSeq(msg, getMsgSeq(msg) + 1);
setMsgTC1(msg, getTimeNSec());
@[if ros2_distro]@
setMsgTC1(msg, getROSTimeNSec());
@[else]@
setMsgTC1(msg, getSteadyTimeNSec());
@[end if]@
pub->publish(msg);
}
@@ -206,11 +245,19 @@ timesync_msg_t TimeSync::newTimesyncMsg()
{
timesync_msg_t msg{};
setMsgTimestamp(&msg, getTimeUSec());
@[if ros2_distro]@
setMsgTimestamp(&msg, getROSTimeUSec());
@[else]@
setMsgTimestamp(&msg, getSteadyTimeUSec());
@[end if]@
setMsgSysID(&msg, 0);
setMsgSeq(&msg, _last_msg_seq);
setMsgTC1(&msg, 0);
setMsgTS1(&msg, getTimeNSec());
@[if ros2_distro]@
setMsgTS1(&msg, getROSTimeNSec());
@[else]@
setMsgTS1(&msg, getSteadyTimeNSec());
@[end if]@
_last_msg_seq++;