visualize_locations.py

#!/usr/bin/env python

import rospy

from visualization_msgs.msg import Marker, MarkerArray
from geometry_msgs.msg import Pose, PoseArray, PoseStamped, Point, Quaternion
from tf.transformations import quaternion_from_euler
from std_msgs.msg import ColorRGBA


def create_pose_markers(x, y, theta, pose_num, marker_id):
    m = Marker()
    m.type = m.ARROW
    m.header.frame_id = 'map'
    m.pose.position.x = x
    m.pose.position.y = y
    m.pose.position.z = 0.2
    m.pose.orientation = Quaternion(*quaternion_from_euler(0.0, 0.0, theta))
    m.scale.x = 0.2
    m.scale.y = 0.2
    m.scale.z = 0.2
    m.id = marker_id + 100
    m.ns = 'arrow'
    m.color = ColorRGBA(0.0, 1.0, 0.0, 1.0)

    mt = Marker()
    mt.type = m.TEXT_VIEW_FACING
    mt.header.frame_id = 'map'
    # mt.text = 'location ' + str(pose_num)
    mt.text = str(round(x, 2)) + ", " + str(round(y, 2)) + \
        ", " + str(round(theta, 2)) + " (" + str(idx) + ")"
    mt.pose.position.x = x
    mt.pose.position.y = y
    mt.pose.position.z = 0.4
    mt.pose.orientation = Quaternion(*quaternion_from_euler(0.0, 0.0, theta))
    mt.scale.z = 0.2
    mt.id = marker_id + 100000000
    mt.ns = 'text'
    mt.color = ColorRGBA(1.0, 0.0, 0.0, 1.0)

    return m, mt


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

    pub = rospy.Publisher('clean_up_locs', MarkerArray, queue_size=1,
                          latch=True)

    locations = [
        [-14.28147846804975, -10.578538843751152, 0.5243748692572374],
        [-14.235518921638132, -9.91396637415809, 2.8292988953888676],
        [-13.290040735408944, -9.5188394461611, -0.40573597068190925],
        [-13.899200447156906, -10.71338352177931, -0.3773776473700742],
        [-14.940, -12.438, -0.498],
        [-12.162501862647746, -12.270776676383756, 1.0840147688470032],
        [-11.862999188621854, -11.67658210015222, 2.6341962982544875],
        [-11.411367191040616, -10.960762634418693, 2.6422395874390583],
        [-11.418195057053168, -11.05644396253836, 0.5347505865276659],
        [-9.351023682929041, -12.411964430568098, 1.1276941255828832],
        [-9.542293107761846, -12.963363175806165, 2.957523880820298],
        [-12.870558432763005, -14.23203622059411, 2.037407290914208],
        [-12.787, -13.321, 2.701],
        [-12.455472841258384, -14.660707458572972, 0.5145754346753647],
        [-12.205190679053842, -14.979646824936175, -1.8781305422876786],
        [-11.863743731207517, -15.297268773437931, -1.1284051408861588],
        [-15.485, -13.646, -2.644],
        [-16.477, -13.683, -2.872],
        [-16.089462821185485, -14.13050347931775, -2.126509675387615],
        [-15.516267626367238, -14.584791247764631, -1.8843526929657397],
        [-16.236922726274997, -12.916869147283716, 2.7073442846496167],
        [-16.23692060362116, -12.916868942798503, 2.7073442846496167],
        [-11.739, -14.956, 1.124],
        [-11.130, -15.569, -1.275]
    ]

    ma = MarkerArray()
    for idx, location in enumerate(locations):
        m, mt = create_pose_markers(
            location[0], location[1], location[2], idx, idx)
        ma.markers.append(m)
        ma.markers.append(mt)

    pub.publish(ma)

    rospy.spin()