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.

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

Use rosgraph_msgs.msg.Clock for TimeSource #304

Merged
merged 1 commit into from
Apr 5, 2019
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
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
1 change: 1 addition & 0 deletions rclpy/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,7 @@

<exec_depend>ament_index_python</exec_depend>
<exec_depend>builtin_interfaces</exec_depend>
<exec_depend>rosgraph_msgs</exec_depend>

<test_depend>ament_cmake_pytest</test_depend>
<test_depend>ament_lint_auto</test_depend>
Expand Down
6 changes: 3 additions & 3 deletions rclpy/rclpy/time_source.py
Original file line number Diff line number Diff line change
Expand Up @@ -12,12 +12,12 @@
# See the License for the specific language governing permissions and
# limitations under the License.

import builtin_interfaces.msg
from rcl_interfaces.msg import SetParametersResult
from rclpy.clock import ClockType
from rclpy.clock import ROSClock
from rclpy.parameter import Parameter
from rclpy.time import Time
import rosgraph_msgs.msg

CLOCK_TOPIC = '/clock'

Expand Down Expand Up @@ -55,7 +55,7 @@ def ros_time_is_active(self, enabled):
def _subscribe_to_clock_topic(self):
if self._clock_sub is None and self._node is not None:
self._clock_sub = self._node.create_subscription(
builtin_interfaces.msg.Time,
rosgraph_msgs.msg.Clock,
CLOCK_TOPIC,
self.clock_callback
)
Expand Down Expand Up @@ -102,7 +102,7 @@ def attach_clock(self, clock):

def clock_callback(self, msg):
# Cache the last message in case a new clock is attached.
time_from_msg = Time.from_msg(msg)
time_from_msg = Time.from_msg(msg.clock)
self._last_time_set = time_from_msg
for clock in self._associated_clocks:
clock.set_ros_time_override(time_from_msg)
Expand Down
14 changes: 7 additions & 7 deletions rclpy/test/test_time_source.py
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,6 @@
import unittest
from unittest.mock import Mock

import builtin_interfaces.msg
import rclpy
from rclpy.clock import Clock
from rclpy.clock import ClockChange
Expand All @@ -28,6 +27,7 @@
from rclpy.time import Time
from rclpy.time_source import CLOCK_TOPIC
from rclpy.time_source import TimeSource
import rosgraph_msgs.msg

from .mock_compat import __name__ as _ # noqa: ignore=F401

Expand All @@ -44,11 +44,11 @@ def tearDown(self):
rclpy.shutdown(context=self.context)

def publish_clock_messages(self):
clock_pub = self.node.create_publisher(builtin_interfaces.msg.Time, CLOCK_TOPIC)
clock_pub = self.node.create_publisher(rosgraph_msgs.msg.Clock, CLOCK_TOPIC)
cycle_count = 0
time_msg = builtin_interfaces.msg.Time()
time_msg = rosgraph_msgs.msg.Clock()
while rclpy.ok(context=self.context) and cycle_count < 5:
time_msg.sec = cycle_count
time_msg.clock.sec = cycle_count
clock_pub.publish(time_msg)
cycle_count += 1
executor = rclpy.executors.SingleThreadedExecutor(context=self.context)
Expand All @@ -57,11 +57,11 @@ def publish_clock_messages(self):
time.sleep(1)

def publish_reversed_clock_messages(self):
clock_pub = self.node.create_publisher(builtin_interfaces.msg.Time, CLOCK_TOPIC)
clock_pub = self.node.create_publisher(rosgraph_msgs.msg.Clock, CLOCK_TOPIC)
cycle_count = 0
time_msg = builtin_interfaces.msg.Time()
time_msg = rosgraph_msgs.msg.Clock()
while rclpy.ok(context=self.context) and cycle_count < 5:
time_msg.sec = 6 - cycle_count
time_msg.clock.sec = 6 - cycle_count
clock_pub.publish(time_msg)
cycle_count += 1
executor = rclpy.executors.SingleThreadedExecutor(context=self.context)
Expand Down