counter_with_reset.py 2.65 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
#    Copyright 2022 Marian Begemann
#
#    Licensed under the Apache License, Version 2.0 (the "License");
#    you may not use this file except in compliance with the License.
#    You may obtain a copy of the License at
#
#        http://www.apache.org/licenses/LICENSE-2.0
#
#    Unless required by applicable law or agreed to in writing, software
#    distributed under the License is distributed on an "AS IS" BASIS,
#    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
#    See the License for the specific language governing permissions and
#    limitations under the License.
import rclpy
from rclpy.node import Node
from std_msgs.msg import Int64, Bool, String


class CounterWithReset(Node):
    """
    A simple example to try the TeSSLa Bridge.
    When started the counter starts counting up from zero.
    It can be reset via an event on the /reset_counter topic, with TeSSLa triggering this event.
    """

    def __init__(self):
        """Initialize the counter node."""
        super().__init__('counter_with_reset')

        self.declare_parameters(
            namespace='',
            parameters=[
                ('counter_timer_period', None),
            ])

        self.counter: int = 0
        self.counter_publisher = self.create_publisher(Int64, '/counter', 10)
        self.reset_subscriber = self.create_subscription(Bool, '/reset_counter', self.reset_callback, 10)

        counter_timer_period = float(self.get_parameter("counter_timer_period").get_parameter_value().double_value)
        self.timer = self.create_timer(counter_timer_period, self.timer_callback)

    def timer_callback(self):
        """Publish the counter value and increased it by one."""
        msg = Int64()
        msg.data = self.counter
        self.counter_publisher.publish(msg)
        self.get_logger().debug('Publishing:"{}"'.format(msg))
        self.counter += 1

    def reset_callback(self, msg):
        """If event of type bool is true reset counter to zero, else do nothing."""
        if msg.data:
            self.counter = 0
55
56
57
            self.get_logger().warn('Counter Node: Reset Counter to zero, after receiving on reset topic:"{}"'.
                                   format(msg.data))
        self.get_logger().debug('Counter Node: Received on reset topic:"{}"'.format(msg.data))
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76


def main(args=None):
    """Create a node for the counter."""
    rclpy.init(args=args)
    node = CounterWithReset()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()


def destroy_node(self):
    """Allows to perform last actions before destruction."""
    # Add extra instructions here
    super().destroy_node()


if __name__ == '__main__':
    main()