<reponame>feagi/feagi-core<filename>third_party/ros/serial/HC_SR04_Foxy.py
import rclpy
import serial
import time
import std_msgs

from example_interfaces.msg import Int64
from rclpy.node import Node
from rclpy.qos import QoSProfile
from rclpy.qos import qos_profile_sensor_data

ser = serial.Serial(
    port="/dev/ttyACM0",
    baudrate=9600,
    parity=serial.PARITY_NONE,
    stopbits=serial.STOPBITS_ONE,
    bytesize=serial.EIGHTBITS
)


class MinimalPublisher(Node):

    def __init__(self):
        super().__init__('minimal_publisher')
        self.publisher_ = self.create_publisher(Int64, "scan", 10)
        timer_period = 0  # seconds
        self.timer = self.create_timer(timer_period, self.timer_callback)
        self.i = 0

    def timer_callback(self):
        sensor_val = ser.readline()
        try:
            msg = Int64()
            msg.data= int(sensor_val)
            # self.get_logger().info("PUBLISHER: {}".format(msg.data))
            self.publisher_.publish(msg)
        except ValueError as error:
            print(error)
            pass


def main(args=None):
    rclpy.init(args=args)

    minimal_publisher = MinimalPublisher()

    rclpy.spin(minimal_publisher)

    # Destroy the node explicitly
    # (optional - otherwise it will be done automatically
    # when the garbage collector destroys the node object)
    minimal_publisher.destroy_node()
    rclpy.shutdown()
    ser.close()


if __name__ == '__main__':
    main()
