Skip to content
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
52 changes: 33 additions & 19 deletions rclpy/rclpy/time_source.py
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,8 @@
# See the License for the specific language governing permissions and
# limitations under the License.

import weakref

from rcl_interfaces.msg import SetParametersResult
from rclpy.clock import ClockType
from rclpy.clock import ROSClock
Expand All @@ -27,7 +29,7 @@ class TimeSource:

def __init__(self, *, node=None):
self._clock_sub = None
self._node = None
self._node_weak_ref = None
self._associated_clocks = []
# Zero time is a special value that means time is uninitialzied
self._last_time_set = Time(clock_type=ClockType.ROS_TIME)
Expand All @@ -49,27 +51,31 @@ def ros_time_is_active(self, enabled):
if enabled:
self._subscribe_to_clock_topic()
else:
if self._clock_sub is not None and self._node is not None:
self._node.destroy_subscription(self._clock_sub)
self._clock_sub = None
if self._clock_sub is not None:
node = self._get_node()
if node is not None:
node.destroy_subscription(self._clock_sub)
self._clock_sub = None

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(
rosgraph_msgs.msg.Clock,
CLOCK_TOPIC,
self.clock_callback,
10
)
if self._clock_sub is None:
node = self._get_node()
if node is not None:
self._clock_sub = node.create_subscription(
rosgraph_msgs.msg.Clock,
CLOCK_TOPIC,
self.clock_callback,
10
)

def attach_node(self, node):
from rclpy.node import Node
if not isinstance(node, Node):
raise TypeError('Node must be of type rclpy.node.Node')
# Remove an existing node.
if self._node is not None:
if self._node_weak_ref is not None:
self.detach_node()
self._node = node
self._node_weak_ref = weakref.ref(node)

if not node.has_parameter(USE_SIM_TIME_NAME):
node.declare_parameter(USE_SIM_TIME_NAME, False)
Expand All @@ -92,11 +98,12 @@ def attach_node(self, node):
def detach_node(self):
# Remove the subscription to the clock topic.
if self._clock_sub is not None:
if self._node is None:
node = self._get_node()
if node is None:
raise RuntimeError('Unable to destroy previously created clock subscription')
self._node.destroy_subscription(self._clock_sub)
node.destroy_subscription(self._clock_sub)
self._clock_sub = None
self._node = None
self._node_weak_ref = None

def attach_clock(self, clock):
if not isinstance(clock, ROSClock):
Expand All @@ -119,9 +126,16 @@ def _on_parameter_event(self, parameter_list):
if parameter.type_ == Parameter.Type.BOOL:
self.ros_time_is_active = parameter.value
else:
self._node.get_logger().error(
'{} parameter set to something besides a bool'
.format(USE_SIM_TIME_NAME))
node = self._get_node()
if node:
node.get_logger().error(
'{} parameter set to something besides a bool'
.format(USE_SIM_TIME_NAME))
break

return SetParametersResult(successful=True)

def _get_node(self):
if self._node_weak_ref is not None:
return self._node_weak_ref()
return None
Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I would think this case is worth raising for (trying to do an operation that requires a node, but the time source is not attached to one), and the other case of the weak reference being invalid is expected and not worth raising for.

Copy link
Copy Markdown
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I changed the code to not raise an error in case the weak reference is invalid.
The code previous to this change was working when self._node was None, so I don't think that an error shold be raised in that case.

2 changes: 1 addition & 1 deletion rclpy/test/test_time_source.py
Original file line number Diff line number Diff line change
Expand Up @@ -159,7 +159,7 @@ def test_time_source_using_sim_time(self):
node2 = rclpy.create_node('TestTimeSource2', namespace='/rclpy', context=self.context)
time_source.attach_node(node2)
node2.destroy_node()
assert time_source._node == node2
assert time_source._get_node() == node2
assert time_source._clock_sub is None

def test_forwards_jump(self):
Expand Down