# This code generates a custom Mypath2 message with the desired trajectory to be visualized in RVIZ through the "Marker" display

import rclpy
from rclpy.node import Node
from visualization_msgs.msg import Marker
from geometry_msgs.msg import Point
from custom_messages.msg import Mypath2

class TrajectoryVisualizer(Node):

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

    def trajectory_callback(self, msg):
        marker = Marker()
        marker.header.frame_id = 'map'
        marker.header.stamp = self.get_clock().now().to_msg()
        marker.ns = "trajectory"
        marker.id = 0
        marker.type = Marker.LINE_STRIP
        marker.action = Marker.ADD
        marker.scale.x = 100.0
        marker.scale.y = 100.0
        marker.scale.z = 100.0
        marker.color.r = 0.0
        marker.color.g = 1.0
        marker.color.b = 0.0
        marker.color.a = 0.5
        
        poses = []

        for pose in msg.poses:
            point = Point()
            point.x = pose.x
            point.y = pose.y
            point.z = pose.z
            poses.append(point)

        marker.points = poses

        self.publisher.publish(marker)

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()
