#!/usr/bin/python
import rospy
from visualization_msgs.msg import Marker


if __name__ == '__main__':
    rospy.init_node('display_terrain')

    # Build the msg
    msg = Marker()
    msg.header.frame_id = '/gt_world'
    msg.header.stamp = rospy.Time()
    msg.ns = '/gt'
    msg.id = 0
    msg.type = Marker.MESH_RESOURCE
    msg.action = Marker.ADD
    msg.pose.position.x = 0
    msg.pose.position.y = 0
    msg.pose.position.z = 0
    msg.pose.orientation.x = 0
    msg.pose.orientation.y = 0
    msg.pose.orientation.z = 0
    msg.pose.orientation.w = 1
    msg.scale.x = 0.8
    msg.scale.y = 0.8
    msg.scale.z = 0.8
    msg.color.a = 0.95
    msg.color.r = 63.0 / 255
    msg.color.g = 70.0 / 255
    msg.color.b = 110.0 / 255
    msg.mesh_resource = 'package://srcp2_models_qual/models/lunar_terrain/meshes/terrain5.dae'

    # Marker pub
    pub = rospy.Publisher('terrain', Marker, queue_size=1)

    rate = rospy.Rate(24)
    while not rospy.is_shutdown():
        pub.publish(msg)
        rate.sleep()
