Skip to content

Commit 3771d46

Browse files
pijaropijaro
authored
Merge pull request #33 from RobotecAI/time_source
Time sources
2 parents ae62f2f + d923747 commit 3771d46

File tree

8 files changed

+250
-28
lines changed

8 files changed

+250
-28
lines changed

‎src/Ros2ForUnity/Scripts/ROS2UnityComponent.cs‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -45,6 +45,8 @@ public bool Ok()
4545
{
4646
lock (mutex)
4747
{
48+
if (ros2forUnity == null)
49+
LazyConstruct();
4850
return (nodes != null && ros2forUnity.Ok());
4951
}
5052
}

‎src/Ros2ForUnity/Scripts/Time/DotnetTimeSource.cs‎

Lines changed: 75 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,75 @@
1+
// Copyright 2022 Robotec.ai.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
using System;
16+
using System.Diagnostics;
17+
18+
namespace ROS2
19+
{
20+
21+
/// <summary>
22+
/// DateTime based clock that has resolution increased using Stopwatch.
23+
/// DateTime is used to synchronize since Stopwatch tends to drift.
24+
/// </summary>
25+
public class DotnetTimeSource : ITimeSource
26+
{
27+
private readonly double maxUnsyncedSeconds = 10;
28+
29+
private Stopwatch stopwatch = new Stopwatch();
30+
31+
private readonly object mutex = new object();
32+
33+
private double systemTimeIntervalStart = 0;
34+
35+
private double stopwatchStartTimeStamp;
36+
37+
private double TotalSystemTimeSeconds()
38+
{
39+
return TimeSpan.FromTicks(DateTime.UtcNow.Ticks).TotalSeconds;
40+
}
41+
42+
private void UpdateSystemTime()
43+
{
44+
systemTimeIntervalStart = TotalSystemTimeSeconds();
45+
stopwatchStartTimeStamp = Stopwatch.GetTimestamp();
46+
}
47+
48+
public DotnetTimeSource()
49+
{
50+
UpdateSystemTime();
51+
}
52+
53+
public void GetTime(out int seconds, out uint nanoseconds)
54+
{
55+
lock(mutex) // Threading
56+
{
57+
double endTimestamp = Stopwatch.GetTimestamp();
58+
var durationInSeconds = endTimestamp - stopwatchStartTimeStamp;
59+
double timeOffset = 0;
60+
if (durationInSeconds >= maxUnsyncedSeconds)
61+
{ // acquire DateTime to sync
62+
UpdateSystemTime();
63+
}
64+
else
65+
{ // use Stopwatch offset
66+
timeOffset = durationInSeconds;
67+
}
68+
69+
TimeUtils.TimeFromTotalSeconds(systemTimeIntervalStart + timeOffset, out seconds, out nanoseconds);
70+
}
71+
}
72+
}
73+
74+
} // namespace ROS2
75+

‎src/Ros2ForUnity/Scripts/Time/ITimeSource.cs‎

Lines changed: 26 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,26 @@
1+
// Copyright 2022 Robotec.ai.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
namespace ROS2
16+
{
17+
18+
/// <summary>
19+
/// Interace for acquiring time
20+
/// </summary>
21+
public interface ITimeSource
22+
{
23+
public void GetTime(out int seconds, out uint nanoseconds);
24+
}
25+
26+
} // namespace ROS2

‎src/Ros2ForUnity/Scripts/ROS2Clock.cs‎ renamed to ‎src/Ros2ForUnity/Scripts/Time/ROS2Clock.cs‎

Lines changed: 17 additions & 28 deletions
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
1-
// Copyright 2019-2021 Robotec.ai.
1+
// Copyright 2019-2022 Robotec.ai.
22
//
33
// Licensed under the Apache License, Version 2.0 (the "License");
44
// you may not use this file except in compliance with the License.
@@ -19,57 +19,46 @@ namespace ROS2
1919
{
2020

2121
/// <summary>
22-
/// A ros2 clock wrapper class that can acquire ros2 time on demand. It does not include a publisher.
23-
/// (Not every clock needs to be associated with a publisher)
22+
/// A ros2 clock class that for interfacing between a time source (unity or ros2 system time) and ros2cs messages, structs.
2423
/// </summary>
2524
public class ROS2Clock
2625
{
27-
private int seconds;
28-
private uint nanoseconds;
29-
private ROS2.Clock clock;
26+
private ITimeSource _timeSource;
3027

31-
public ROS2Clock()
32-
{
33-
clock = new ROS2.Clock();
28+
public ROS2Clock() : this(new ROS2TimeSource())
29+
{ // By default, use ROS2TimeSource
3430
}
3531

36-
~ROS2Clock()
32+
public ROS2Clock(ITimeSource ts)
3733
{
38-
clock.Dispose();
34+
_timeSource = ts;
3935
}
4036

4137
public void UpdateClockMessage(ref rosgraph_msgs.msg.Clock clockMessage)
4238
{
43-
GetRosTime();
39+
int seconds;
40+
uint nanoseconds;
41+
_timeSource.GetTime(out seconds, out nanoseconds);
4442
clockMessage.Clock_.Sec = seconds;
4543
clockMessage.Clock_.Nanosec = nanoseconds;
4644
}
4745

4846
public void UpdateROSClockTime(builtin_interfaces.msg.Time time)
4947
{
50-
GetRosTime();
51-
time.Nanosec = nanoseconds;
48+
int seconds;
49+
uint nanoseconds;
50+
_timeSource.GetTime(out seconds, out nanoseconds);
5251
time.Sec = seconds;
52+
time.Nanosec = nanoseconds;
5353
}
5454

5555
public void UpdateROSTimestamp(ref ROS2.MessageWithHeader message)
5656
{
57-
GetRosTime();
57+
int seconds;
58+
uint nanoseconds;
59+
_timeSource.GetTime(out seconds, out nanoseconds);
5860
message.UpdateHeaderTime(seconds, nanoseconds);
5961
}
60-
61-
private void GetRosTime()
62-
{
63-
if (!ROS2.Ros2cs.Ok())
64-
{
65-
Debug.LogWarning("Cannot update ros time, ros2 either not initialized or shut down already");
66-
return;
67-
}
68-
69-
long nanosec = (long)(clock.Now.Seconds * 1e9);
70-
seconds = (int)(nanosec / 1000000000);
71-
nanoseconds = (uint)(nanosec % 1000000000);
72-
}
7362
}
7463

7564
} // namespace ROS2

‎src/Ros2ForUnity/Scripts/ROS2Clock.cs.meta‎ renamed to ‎src/Ros2ForUnity/Scripts/Time/ROS2Clock.cs.meta‎

File renamed without changes.

‎src/Ros2ForUnity/Scripts/Time/ROS2TimeSource.cs‎

Lines changed: 54 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,54 @@
1+
// Copyright 2022 Robotec.ai.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
using UnityEngine;
16+
17+
namespace ROS2
18+
{
19+
20+
/// <summary>
21+
/// ros2 time source (system time by default).
22+
/// </summary>
23+
public class ROS2TimeSource : ITimeSource
24+
{
25+
private ROS2.Clock clock;
26+
27+
public void GetTime(out int seconds, out uint nanoseconds)
28+
{
29+
if (!ROS2.Ros2cs.Ok())
30+
{
31+
seconds = 0;
32+
nanoseconds = 0;
33+
Debug.LogWarning("Cannot acquire valid ros time, ros either not initialized or shut down already");
34+
return;
35+
}
36+
37+
if (clock == null)
38+
{ // Create clock which uses system time by default (unless use_sim_time is set in ros2)
39+
clock = new ROS2.Clock();
40+
}
41+
42+
TimeUtils.TimeFromTotalSeconds(clock.Now.Seconds, out seconds, out nanoseconds);
43+
}
44+
45+
~ROS2TimeSource()
46+
{
47+
if (clock != null)
48+
{
49+
clock.Dispose();
50+
}
51+
}
52+
}
53+
54+
} // namespace ROS2

‎src/Ros2ForUnity/Scripts/Time/TimeUtils.cs‎

Lines changed: 31 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,31 @@
1+
// Copyright 2022 Robotec.ai.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
namespace ROS2
16+
{
17+
18+
/// <summary>
19+
/// Interace for acquiring time
20+
/// </summary>
21+
internal static class TimeUtils
22+
{
23+
public static void TimeFromTotalSeconds(in double secondsIn, out int seconds, out uint nanoseconds)
24+
{
25+
long nanosec = (long)(secondsIn * 1e9);
26+
seconds = (int)(nanosec / 1000000000);
27+
nanoseconds = (uint)(nanosec % 1000000000);
28+
}
29+
}
30+
31+
} // namespace ROS2

‎src/Ros2ForUnity/Scripts/Time/UnityTimeSource.cs‎

Lines changed: 45 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,45 @@
1+
// Copyright 2022 Robotec.ai.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
using System;
16+
using System.Threading;
17+
using UnityEngine;
18+
19+
namespace ROS2
20+
{
21+
22+
/// <summary>
23+
/// Acquires Unity time. Note that Time API only allows main thread access,
24+
/// but this class object also stores last acquired value for other threads.
25+
/// This is done without a warning, so the class will not behave as expected
26+
/// when not used by main thread.
27+
/// </summary>
28+
public class UnityTimeSource : ITimeSource
29+
{
30+
private Thread mainThread;
31+
private double lastReadingSecs;
32+
33+
public UnityTimeSource()
34+
{
35+
mainThread = Thread.CurrentThread;
36+
}
37+
38+
public void GetTime(out int seconds, out uint nanoseconds)
39+
{
40+
lastReadingSecs = mainThread.Equals(Thread.CurrentThread) ? Time.timeAsDouble : lastReadingSecs;
41+
TimeUtils.TimeFromTotalSeconds(lastReadingSecs, out seconds, out nanoseconds);
42+
}
43+
}
44+
45+
} // namespace ROS2

0 commit comments

Comments
 (0)