Source code for simulation.utils.ros_base.visualization

#!/usr/bin/env python3
"""Utility functions for visualizing ROS messages in rviz."""

import math
from typing import List

import rospy
from visualization_msgs.msg import Marker

from simulation.utils.geometry.point import Point


[docs]def get_marker( frame_id: str, type: int = 4, rgba: List[float] = [0, 0, 0, 1], id: int = 0, ns: str = None, duration: float = 1, scale: float = 0.02, ): """Create a rviz marker message. The position or points can be added. Arguments: frame_id: Name of the points' coordinate frame (e.g. world, vehicle, simulation) type: Rviz marker type rgba: Color of the marker id: Marker id ns: Rviz namespace duration: Marker will be shown for this long Returns: Marker msg that rviz can display """ marker = Marker() marker.header.frame_id = frame_id marker.header.stamp = rospy.get_rostime() if ns: marker.ns = ns marker.lifetime = rospy.Duration(secs=duration) marker.color.r = rgba[0] marker.color.g = rgba[1] marker.color.b = rgba[2] marker.color.a = rgba[3] marker.scale.x = scale marker.scale.y = scale marker.id = id marker.type = type marker.pose.orientation.w = math.sqrt(1 - 0.000001 ** 2) marker.pose.orientation.z = 0.000001 return marker
[docs]def get_marker_for_points( points: List[Point], *, frame_id: str, type: int = 4, rgba: List[float] = [0, 0, 0, 1], id: int = 0, ns: str = None, duration: float = 1, scale: float = 0.02 ) -> Marker: """Rviz marker message from a list of points. Arguments: points: Points to visualize frame_id: Name of the points' coordinate frame (e.g. world, vehicle, simulation) type: Rviz marker type rgba: Color of the marker id: Marker id ns: Rviz namespace duration: Marker will be shown for this long Returns: Marker msg that rviz can display """ marker = get_marker(frame_id, type, rgba, id, ns, duration, scale) marker.points = [p.to_geometry_msg() for p in points] return marker