Exercise01 — Publisher, Subscriber, Timer

def main():
    rclpy.init()
    node = StatusNode()
    rclpy.spin(node)          # <-- this line is missing
    node.destroy_node()
    rclpy.shutdown()

import threading

class ImuCache(Node):
    def __init__(self):
        super().__init__('imu_cache')
        self.latest_imu = None
        self._lock = threading.Lock()          # <-- add lock
        self.sub = self.create_subscription(Imu, '/imu', self.cb, 10)

    def cb(self, msg):
        with self._lock:                        # <-- acquire before write
            self.latest_imu = msg

    def get_latest(self):
        with self._lock:                        # <-- acquire before read
            return self.latest_imu

map_qos = QoSProfile(
    reliability=ReliabilityPolicy.RELIABLE,
    durability=DurabilityPolicy.TRANSIENT_LOCAL,   # <-- must match publisher
    depth=1,
)

import rclpy
from rclpy.node import Node
from std_msgs.msg import Float64


class SpeedPublisher(Node):
    def __init__(self):
        super().__init__('speed_publisher')
        self.pub = self.create_publisher(Float64, '/robot/speed', 10)
        self.timer = self.create_timer(0.1, self.timer_cb)   # 1/10Hz = 0.1s

    def timer_cb(self):
        msg = Float64()
        msg.data = 1.0
        self.pub.publish(msg)


def main(args=None):
    rclpy.init(args=args)
    node = SpeedPublisher()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

import rclpy
from rclpy.node import Node
from nav_msgs.msg import Odometry
from sensor_msgs.msg import Imu
from std_msgs.msg import Float64


class FusionNode(Node):
    def __init__(self):
        super().__init__('fusion_node')
        self._latest_odom_speed = None
        self._latest_imu_accel  = None

        self.sub_odom = self.create_subscription(
            Odometry, '/odom', self.odom_cb, 10)
        self.sub_imu  = self.create_subscription(
            Imu, '/imu', self.imu_cb, 10)
        self.pub = self.create_publisher(Float64, '/fusion/speed_comparison', 10)
        self.timer = self.create_timer(1.0, self.timer_cb)

    def odom_cb(self, msg: Odometry):
        self._latest_odom_speed = msg.twist.twist.linear.x

    def imu_cb(self, msg: Imu):
        self._latest_imu_accel = msg.linear_acceleration.x

    def timer_cb(self):
        if self._latest_odom_speed is None or self._latest_imu_accel is None:
            self.get_logger().warn(
                'Waiting for sensor data: '
                f'odom={self._latest_odom_speed is not None}, '
                f'imu={self._latest_imu_accel is not None}'
            )
            return

        diff = Float64()
        diff.data = abs(self._latest_odom_speed - self._latest_imu_accel)
        self.pub.publish(diff)


def main(args=None):
    rclpy.init(args=args)
    node = FusionNode()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

import rclpy
from rclpy.lifecycle import LifecycleNode, TransitionCallbackReturn, State
from std_msgs.msg import Float64


class LifecycleSpeedPublisher(LifecycleNode):
    def __init__(self):
        super().__init__('lifecycle_speed_publisher')
        self._pub = None
        self._timer = None

    def on_configure(self, state: State) -> TransitionCallbackReturn:
        self._pub = self.create_lifecycle_publisher(Float64, '/robot/speed', 10)
        self.get_logger().info('Configured: publisher created (inactive).')
        return TransitionCallbackReturn.SUCCESS

    def on_activate(self, state: State) -> TransitionCallbackReturn:
        super().on_activate(state)              # activates the managed publisher
        self._timer = self.create_timer(0.1, self.timer_cb)
        self.get_logger().info('Activated: timer started.')
        return TransitionCallbackReturn.SUCCESS

    def on_deactivate(self, state: State) -> TransitionCallbackReturn:
        if self._timer is not None:
            self._timer.cancel()
            self.destroy_timer(self._timer)
            self._timer = None
        super().on_deactivate(state)            # deactivates the managed publisher
        self.get_logger().info('Deactivated: timer stopped.')
        return TransitionCallbackReturn.SUCCESS

    def on_cleanup(self, state: State) -> TransitionCallbackReturn:
        if self._pub is not None:
            self.destroy_publisher(self._pub)
            self._pub = None
        self.get_logger().info('Cleaned up: publisher destroyed.')
        return TransitionCallbackReturn.SUCCESS

    def timer_cb(self):
        if self._pub is not None and self._pub.is_activated:
            msg = Float64()
            msg.data = 1.0
            self._pub.publish(msg)


def main(args=None):
    rclpy.init(args=args)
    node = LifecycleSpeedPublisher()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

ros2 lifecycle set /lifecycle_speed_publisher configure
ros2 lifecycle set /lifecycle_speed_publisher activate
ros2 lifecycle set /lifecycle_speed_publisher deactivate