Repository: cyberbotics/webots_ros2
Issue: Improper conversion to rclpy.Time for nanoseconds 

In `webots_node`, time is converted from unix timestamp format to `rclpy.Time`

https://github.com/cyberbotics/webots_ros2/blob/40385d93f49812357145cb9b285758b8fcbcbf55/webots_ros2_core/webots_ros2_core/webots_node.py#L93

However, there seems to be an error in the conversion as stated nanoseconds are in fact microseconds. Therefore, the line should be :

```python
nanoseconds = int((sim_time - seconds) * 1e9)
``` 

Changed files:
webots_ros2_epuck/webots_ros2_epuck/driver.py, webots_ros2_core/test/test_flake8.py, webots_ros2_core/webots_ros2_core/trajectory_follower.py, webots_ros2_core/webots_ros2_core/joint_state_publisher.py, webots_ros2_core/webots_ros2_core/webots_node.py, webots_ros2_core/webots_ros2_core/tf_publisher.py, webots_ros2_demos/webots_ros2_demos/follow_joint_trajectory_client.py, webots_ros2_core/webots_ros2_core/laser_publisher.py

diff --git a/webots_ros2_core/test/test_flake8.py b/webots_ros2_core/test/test_flake8.py
index c79ba0d..ffa6a77 100644
--- a/webots_ros2_core/test/test_flake8.py
+++ b/webots_ros2_core/test/test_flake8.py
@@ -21,5 +21,5 @@ import pytest
 @pytest.mark.flake8
 @pytest.mark.linter
 def test_flake8():
-    rc = main(argv=[])
+    rc = main(argv=['--linelength', '128'])
     assert rc == 0, 'Found errors'
diff --git a/webots_ros2_core/webots_ros2_core/joint_state_publisher.py b/webots_ros2_core/webots_ros2_core/joint_state_publisher.py
index 2165e35..3b91327 100644
--- a/webots_ros2_core/webots_ros2_core/joint_state_publisher.py
+++ b/webots_ros2_core/webots_ros2_core/joint_state_publisher.py
@@ -19,7 +19,7 @@ import sys
 from webots_ros2_core.utils import append_webots_python_lib_to_path
 
 from sensor_msgs.msg import JointState
-from builtin_interfaces.msg import Time
+from rclpy.time import Time
 
 try:
     append_webots_python_lib_to_path()
@@ -57,10 +57,8 @@ class JointStatePublisher():
 
     def publish(self):
         """Publish the 'joint_states' topic with up to date value."""
-        seconds = int(self.robot.getTime())
-        nanoseconds = int((self.robot.getTime() - seconds) * 1.0e+6)
         msg = JointState()
-        msg.header.stamp = Time(sec=seconds, nanosec=nanoseconds)
+        msg.header.stamp = Time(seconds=self.robot.getTime()).to_msg()
         msg.header.frame_id = 'From simulation state data'
         msg.name = [s + self.jointPrefix for s in self.jointNames]
         msg.position = []
diff --git a/webots_ros2_core/webots_ros2_core/laser_publisher.py b/webots_ros2_core/webots_ros2_core/laser_publisher.py
index cd6a256..393dd13 100644
--- a/webots_ros2_core/webots_ros2_core/laser_publisher.py
+++ b/webots_ros2_core/webots_ros2_core/laser_publisher.py
@@ -20,7 +20,7 @@ from webots_ros2_core.utils import append_webots_python_lib_to_path
 
 from tf2_msgs.msg import TFMessage
 from sensor_msgs.msg import LaserScan
-from builtin_interfaces.msg import Time
+from rclpy.time import Time
 from geometry_msgs.msg import TransformStamped
 
 
@@ -89,17 +89,14 @@ class LaserPublisher():
 
     def publish(self, lidar, transforms):
         """Publish the laser scan topics with up to date value."""
-        nextTime = self.robot.getTime() + 0.001 * self.timestep
-        nextSec = int(nextTime)
-        # rounding prevents precision issues that can cause problems with ROS timers
-        nextNanosec = int(round(1000 * (nextTime - nextSec)) * 1.0e+6)
+        stamp = Time(seconds=self.robot.getTime() + 0.001 * self.timestep).to_msg()
         for i in range(lidar.getNumberOfLayers()):
             name = self.prefix + lidar.getName() + '_scan'
             if lidar.getNumberOfLayers() > 1:
                 name += '_' + str(i)
             # publish the lidar to scan transform
             transformStamped = TransformStamped()
-            transformStamped.header.stamp = Time(sec=nextSec, nanosec=nextNanosec)
+            transformStamped.header.stamp = stamp
             transformStamped.header.frame_id = self.prefix + lidar.getName()
             transformStamped.child_frame_id = name
             q1 = transforms3d.quaternions.axangle2quat([0, 1, 0], -1.5708)
@@ -117,7 +114,7 @@ class LaserPublisher():
             transforms.append(transformStamped)
             # publish the actual laser scan
             msg = LaserScan()
-            msg.header.stamp = Time(sec=self.node.sec, nanosec=self.node.nanosec)
+            msg.header.stamp = stamp
             msg.header.frame_id = name
             msg.angle_min = -0.5 * lidar.getFov()
             msg.angle_max = 0.5 * lidar.getFov()
diff --git a/webots_ros2_core/webots_ros2_core/tf_publisher.py b/webots_ros2_core/webots_ros2_core/tf_publisher.py
index bbf9884..8f7b129 100644
--- a/webots_ros2_core/webots_ros2_core/tf_publisher.py
+++ b/webots_ros2_core/webots_ros2_core/tf_publisher.py
@@ -21,7 +21,7 @@ from webots_ros2_core.webots_node import WebotsNode
 
 from tf2_msgs.msg import TFMessage
 from geometry_msgs.msg import TransformStamped
-from builtin_interfaces.msg import Time
+from rclpy.time import Time
 
 
 class TfPublisher(WebotsNode):
@@ -42,16 +42,13 @@ class TfPublisher(WebotsNode):
     def tf_publisher_callback(self):
         # Publish TF for the next step
         # we use one step in advance to make sure no sensor data are published before
+        stamp = Time(seconds=self.robot.getTime() + 0.001 * self.timestep).to_msg()
         tFMessage = TFMessage()
-        nextTime = self.robot.getTime() + 0.001 * self.timestep
-        nextSec = int(nextTime)
-        # rounding prevents precision issues that can cause problems with ROS timers
-        nextNanosec = int(round(1000 * (nextTime - nextSec)) * 1.0e+6)
         for name in self.nodes:
             position = self.nodes[name].getPosition()
             orientation = self.nodes[name].getOrientation()
             transformStamped = TransformStamped()
-            transformStamped.header.stamp = Time(sec=nextSec, nanosec=nextNanosec)
+            transformStamped.header.stamp = stamp
             transformStamped.header.frame_id = 'map'
             transformStamped.child_frame_id = name
             transformStamped.transform.translation.x = position[0]
diff --git a/webots_ros2_core/webots_ros2_core/trajectory_follower.py b/webots_ros2_core/webots_ros2_core/trajectory_follower.py
index 259858a..db09e5e 100644
--- a/webots_ros2_core/webots_ros2_core/trajectory_follower.py
+++ b/webots_ros2_core/webots_ros2_core/trajectory_follower.py
@@ -22,7 +22,8 @@ from webots_ros2_core.utils import append_webots_python_lib_to_path
 
 from control_msgs.action import FollowJointTrajectory
 from trajectory_msgs.msg import JointTrajectoryPoint
-from builtin_interfaces.msg import Duration
+from rclpy.duration import Duration
+from rclpy.time import Time
 
 from rclpy.action import ActionServer, CancelResponse, GoalResponse
 
@@ -64,10 +65,8 @@ def within_tolerance(a_vec, b_vec, tol_vec):
 
 def interp_cubic(p0, p1, t_abs):
     """Perform a cubic interpolation between two trajectory points."""
-    t0 = p0.time_from_start.sec + p0.time_from_start.nanosec * 1.0e-6
-    t1 = p1.time_from_start.sec + p1.time_from_start.nanosec * 1.0e-6
-    T = t1 - t0
-    t = t_abs - t0
+    T = (p1.time_from_start - p0.time_from_start).nanoseconds / 1e9
+    t = (t_abs - p0.time_from_start).nanoseconds / 1e9
     q = [0] * len(p0.positions)
     qdot = [0] * len(p0.positions)
     qddot = [0] * len(p0.positions)
@@ -82,17 +81,15 @@ def interp_cubic(p0, p1, t_abs):
         q[i] = a + b * t + c * t**2 + d * t**3
         qdot[i] = b + 2 * c * t + 3 * d * t**2
         qddot[i] = 2 * c + 6 * d * t
-    return JointTrajectoryPoint(positions=q, velocities=qdot, accelerations=qddot,
-                                time_from_start=Duration(sec=int(t_abs), nanosec=int(int(t_abs) *
-                                                                                     1.0e+6)))
+    return JointTrajectoryPoint(positions=q, velocities=qdot, accelerations=qddot, time_from_start=t_abs.to_msg())
 
 
 def interp_linear(p0, p1, t_abs):
     """Perform a linear interpolation between two trajectory points."""
-    t0 = p0.time_from_start.sec + p0.time_from_start.nanosec * 1.0e-6
-    t1 = p1.time_from_start.sec + p1.time_from_start.nanosec * 1.0e-6
+    t0 = Duration.from_msg(p0.time_from_start).nanoseconds / 1e9
+    t1 = Duration.from_msg(p1.time_from_start).nanoseconds / 1e9
     T = t1 - t0
-    t = t_abs - t0
+    t = t_abs.nanoseconds / 1e9 - t0
     ratio = max(min((t / T), 1), 0)
     q = [0] * len(p0.positions)
     qdot = [0] * len(p0.positions)
@@ -101,9 +98,7 @@ def interp_linear(p0, p1, t_abs):
         q[i] = (1.0 - ratio) * p0.positions[i] + ratio * p1.positions[i]
         qdot[i] = (1.0 - ratio) * p0.velocities[i] + ratio * p1.velocities[i]
         qddot[i] = (1.0 - ratio) * p0.accelerations[i] + ratio * p1.accelerations[i]
-    return JointTrajectoryPoint(positions=q, velocities=qdot, accelerations=qddot,
-                                time_from_start=Duration(sec=int(t_abs), nanosec=int(int(t_abs) *
-                                                                                     1.0e+6)))
+    return JointTrajectoryPoint(positions=q, velocities=qdot, accelerations=qddot, time_from_start=t_abs.to_msg())
 
 
 def sample_trajectory(trajectory, t):
@@ -114,16 +109,14 @@ def sample_trajectory(trajectory, t):
     the time t is the time since the trajectory was started.
     """
     # First point
-    if t <= 0.0:
+    if t <= Duration(seconds=0):
         return copy.deepcopy(trajectory.points[0])
     # Last point
-    if t >= (trajectory.points[-1].time_from_start.sec +
-             trajectory.points[-1].time_from_start.nanosec * 1.0e-6):
+    if t >= Duration.from_msg(trajectory.points[-1].time_from_start):
         return copy.deepcopy(trajectory.points[-1])
     # Finds the (middle) segment containing t
     i = 0
-    while (trajectory.points[i + 1].time_from_start.sec +
-           trajectory.points[i + 1].time_from_start.nanosec * 1.0e-6) < t:
+    while Duration.from_msg(trajectory.points[i + 1].time_from_start) < t:
         i += 1
     return interp_linear(trajectory.points[i], trajectory.points[i + 1], t)
 
@@ -152,7 +145,7 @@ class TrajectoryFollower():
     def __init__(self, robot, node, jointPrefix, goal_time_tolerance=None):
         self.robot = robot
         self.node = node
-        self.previousTime = robot.getTime()
+        self.previousTime = Time(seconds=robot.getTime())
         self.timestep = int(robot.getBasicTimeStep())
         # Parse motor and position sensors
         self.motors = {}
@@ -208,7 +201,7 @@ class TrajectoryFollower():
             return GoalResponse.REJECT
 
         # Inserts the current setpoint at the head of the trajectory
-        now = self.robot.getTime()
+        now = Time(seconds=self.robot.getTime())
         positions = []
         velocities = []
         accelerations = []
@@ -220,7 +213,7 @@ class TrajectoryFollower():
             positions=positions,
             velocities=velocities,
             accelerations=accelerations,
-            time_from_start=Duration())
+            time_from_start=Duration().to_msg())
         goal_handle.trajectory.points.insert(0, point0)
 
         # Add this trajectory to the list if not conflicting with a current trajectory
@@ -253,14 +246,14 @@ class TrajectoryFollower():
         result = FollowJointTrajectory.Result()
         while self.robot:
             # Update position and velocites
-            now = self.robot.getTime()
+            now = Time(seconds=self.robot.getTime())
             position = {}
             velocity = {}
             timeDifference = now - self.previousTime
             for name in self.sensors.keys():
                 position[name] = self.sensors[name].getValue()
-                if timeDifference > 0.0:
-                    velocity[name] = (position[name] - self.position[name]) / timeDifference
+                if timeDifference > Duration(seconds=0):
+                    velocity[name] = (position[name] - self.position[name]) / (timeDifference.nanoseconds * 1e9)
                 else:
                     velocity[name] = self.velocity[name]
 
@@ -279,9 +272,8 @@ class TrajectoryFollower():
             if not trajectory:
                 break
             # Apply trajectory
-            lastPointStart = trajectory.jointTrajectory.points[-1].time_from_start
-            if (now - trajectory.startTime) <= (lastPointStart.sec +
-                                                lastPointStart.nanosec * 1.0e-6):
+            lastPointStart = Duration.from_msg(trajectory.jointTrajectory.points[-1].time_from_start)
+            if (now - trajectory.startTime) <= lastPointStart:
                 # Sending intermediate points
                 trajectory.lastPointSent = False
                 setpoint = sample_trajectory(trajectory.jointTrajectory,
@@ -295,8 +287,7 @@ class TrajectoryFollower():
             elif not trajectory.lastPointSent:
                 # All intermediate points sent, sending last point to make sure we reach the goal
                 trajectory.lastPointSent = True
-                setpoint = sample_trajectory(trajectory.jointTrajectory,
-                                             lastPointStart.sec + lastPointStart.nanosec * 1.0e-6)
+                setpoint = sample_trajectory(trajectory.jointTrajectory, lastPointStart)
                 for name in trajectory.jointTrajectory.joint_names:
                     index = trajectory.jointTrajectory.joint_names.index(name)
                     set_position_in_limit(self.motors[name], setpoint.positions[index])
diff --git a/webots_ros2_core/webots_ros2_core/webots_node.py b/webots_ros2_core/webots_ros2_core/webots_node.py
index f294614..42758a7 100644
--- a/webots_ros2_core/webots_ros2_core/webots_node.py
+++ b/webots_ros2_core/webots_ros2_core/webots_node.py
@@ -28,7 +28,6 @@ from rosgraph_msgs.msg import Clock
 
 from rclpy.node import Node
 from rclpy.parameter import Parameter
-from builtin_interfaces.msg import Time
 
 try:
     append_webots_python_lib_to_path()
@@ -86,9 +85,3 @@ class WebotsNode(Node):
         self.step(request.value)
         response.success = True
         return response
-
-    def now(self):
-        sim_time = self.robot.getTime()
-        seconds = int(sim_time)
-        nanoseconds = int((sim_time - seconds) * 1.0e+6)
-        return Time(sec=seconds, nanosec=nanoseconds)
diff --git a/webots_ros2_demos/webots_ros2_demos/follow_joint_trajectory_client.py b/webots_ros2_demos/webots_ros2_demos/follow_joint_trajectory_client.py
index 5acf432..3f81d92 100644
--- a/webots_ros2_demos/webots_ros2_demos/follow_joint_trajectory_client.py
+++ b/webots_ros2_demos/webots_ros2_demos/follow_joint_trajectory_client.py
@@ -69,12 +69,13 @@ class followJointTrajectoryClient(Node):
         goal_msg = FollowJointTrajectory.Goal()
         goal_msg.trajectory.joint_names = trajectory['joint_names']
         for point in trajectory['points']:
-            duration = Duration(sec=int(point['time_from_start']['sec']),
-                                nanosec=int(int(point['time_from_start']['nanosec']) * 1.0e+6))
             trajectoryPoint = JointTrajectoryPoint(positions=point['positions'],
                                                    velocities=point['velocities'],
                                                    accelerations=point['accelerations'],
-                                                   time_from_start=duration)
+                                                   time_from_start=Duration(
+                                                       sec=point['time_from_start']['sec'],
+                                                       nanosec=point['time_from_start']['nanosec']
+                                                    ))
             goal_msg.trajectory.points.append(trajectoryPoint)
 
         self.get_logger().info('Sending goal request...')
diff --git a/webots_ros2_epuck/webots_ros2_epuck/driver.py b/webots_ros2_epuck/webots_ros2_epuck/driver.py
index fdefb5b..03d197a 100644
--- a/webots_ros2_epuck/webots_ros2_epuck/driver.py
+++ b/webots_ros2_epuck/webots_ros2_epuck/driver.py
@@ -17,6 +17,7 @@
 from functools import partial
 from math import pi, cos, sin
 import rclpy
+from rclpy.time import Time
 from std_msgs.msg import Bool, Int32
 from tf2_ros import StaticTransformBroadcaster
 from sensor_msgs.msg import Range, Image, CameraInfo, Imu, LaserScan, Illuminance
@@ -119,7 +120,7 @@ class EPuckDriver(WebotsDifferentialDriveNode):
                 self.ground_sensor_publishers[idx] = self.create_publisher(Range, '/' + idx, 1)
 
                 ground_sensor_transform = TransformStamped()
-                ground_sensor_transform.header.stamp = self.now()
+                ground_sensor_transform.header.stamp = Time(seconds=self.robot.getTime()).to_msg()
                 ground_sensor_transform.header.frame_id = "base_link"
                 ground_sensor_transform.child_frame_id = "gs" + str(i)
                 ground_sensor_transform.transform.rotation = euler_to_quaternion(0, pi/2, 0)
@@ -141,7 +142,7 @@ class EPuckDriver(WebotsDifferentialDriveNode):
             self.distance_sensor_publishers['ps{}'.format(i)] = sensor_publisher
 
             distance_sensor_transform = TransformStamped()
-            distance_sensor_transform.header.stamp = self.now()
+            distance_sensor_transform.header.stamp = Time(seconds=self.robot.getTime()).to_msg()
             distance_sensor_transform.header.frame_id = "base_link"
             distance_sensor_transform.child_frame_id = "ps" + str(i)
             distance_sensor_transform.transform.rotation = euler_to_quaternion(0, 0, DISTANCE_SENSOR_ANGLE[i])
@@ -157,7 +158,7 @@ class EPuckDriver(WebotsDifferentialDriveNode):
             self.tof_sensor.enable(self.timestep)
             self.tof_publisher = self.create_publisher(Range, '/tof', 1)
             tof_transform = TransformStamped()
-            tof_transform.header.stamp = self.now()
+            tof_transform.header.stamp = Time(seconds=self.robot.getTime()).to_msg()
             tof_transform.header.frame_id = "base_link"
             tof_transform.child_frame_id = "tof"
             tof_transform.transform.rotation.x = 0.0
@@ -217,7 +218,7 @@ class EPuckDriver(WebotsDifferentialDriveNode):
             self.light_sensors.append(light_sensor)
 
             light_transform = TransformStamped()
-            light_transform.header.stamp = self.now()
+            light_transform.header.stamp = Time(seconds=self.robot.getTime()).to_msg()
             light_transform.header.frame_id = "base_link"
             light_transform.child_frame_id = "ls" + str(i)
             light_transform.transform.rotation = euler_to_quaternion(0, 0, DISTANCE_SENSOR_ANGLE[i])
@@ -228,7 +229,7 @@ class EPuckDriver(WebotsDifferentialDriveNode):
 
         # Static tf broadcaster: Laser
         laser_transform = TransformStamped()
-        laser_transform.header.stamp = self.now()
+        laser_transform.header.stamp = Time(seconds=self.robot.getTime()).to_msg()
         laser_transform.header.frame_id = "base_link"
         laser_transform.child_frame_id = "laser_scanner"
         laser_transform.transform.rotation.x = 0.0
@@ -255,7 +256,7 @@ class EPuckDriver(WebotsDifferentialDriveNode):
 
     def step_callback(self):
         self.robot.step(self.timestep)
-        stamp = self.now()
+        stamp = Time(seconds=self.robot.getTime()).to_msg()
 
         self.publish_distance_data(stamp)
         self.publish_light_data(stamp)