# This code generates a Path message with the desired trajectory to be properly handled and visualized in RVIZ through the "Path" display


import rclpy
from rclpy.node import Node
from nav_msgs.msg import Path
from geometry_msgs.msg import PoseStamped
from custom_messages.msg import Mypath2

class TrajectoryVisualizer(Node):

    def __init__(self):
        super().__init__('trajectory_visualizer_path')
        self.subscription = self.create_subscription(
            Mypath2,
            '/route_coord',
            self.trajectory_callback,
            10)
        self.subscription  # prevent unused variable warning
        self.publisher = self.create_publisher(Path, '/visualization_path', 10)

    def trajectory_callback(self, msg):
        path = Path()
        path.header.frame_id = 'map'
        path.header.stamp = self.get_clock().now().to_msg()
                
        coords = []

        for item in msg.poses:
            point = PoseStamped()
            point.pose.position.x = item.x
            point.pose.position.y = item.y
            point.pose.position.z = item.z
            point.header.frame_id = 'map'
            point.header.stamp = self.get_clock().now().to_msg()
            coords.append(point)

        path.poses = coords
        self.publisher.publish(path)

def main(args=None):
    rclpy.init(args=args)
    trajectory_visualizer = TrajectoryVisualizer()
    rclpy.spin(trajectory_visualizer)
    trajectory_visualizer.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()
