import rclpy
from nav2_msgs.msg import Path
from geometry_msgs.msg import PoseStamped
from trajectory import Path

start = (48.7438, 9.0861) 
end = (48.7479, 9.1029)
filename = "campus.osm"

myPath = Path(start, end, filename)
myPath.smooth(alpha=0.3)
myPath.increase_density()


def main():
    rclpy.init()
    node = rclpy.create_node('trajectory_publisher')

    path_pub = node.create_publisher(Path, '/trajectory', 10)
    rate = node.create_rate(1)  # Adjust the rate according to your needs

    while rclpy.ok():
        # Create a Path message and populate it with trajectory points
        path_msg = Path()
        for i in len(myPath):  # Replace with your actual number of points
            pose = PoseStamped()
            pose.pose.position.x = myPath[i][0]  # Replace with your actual x-coordinate
            pose.pose.position.y = myPath[i][1]  # Replace with your actual y-coordinate
            pose.pose.position.z = 0  # Replace with your actual z-coordinate
            path_msg.poses.append(pose)

        path_pub.publish(path_msg)
        rate.sleep()

    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

