import kitcar_utils.ros_base.visualization as visualization
import rclpy
from gazebo_simulation_msgs.msg import CarState as CarStateMsg
from kitcar_utils.geometry.point import Point
from kitcar_utils.ros_base.node_base import NodeBase
from visualization_msgs.msg import Marker
[docs]
class CarStateVisualizationNode(NodeBase):
"""ROS node which allows to visualize the car state in rviz.
Attributes:
frame_publisher: Publishes the cars frame as a rviz marker.
view_cone_publisher: Publishes the cars view cone as a rviz marker.
state_subscriber: Subscribes to car_state.
"""
def __init__(self):
"""Initialize the node."""
super().__init__(name="car_state_visualization_node")
self.run()
[docs]
def start(self):
"""Start visualization."""
self.frame_publisher = self.create_publisher(
Marker, self.param.topics.rviz.frame, 1
)
self.view_cone_publisher = self.create_publisher(
Marker, self.param.topics.rviz.cone, 1
)
self.state_subscriber = self.create_subscription(
CarStateMsg, self.param.topics.car_state, self.state_cb, 10
)
super().start()
[docs]
def stop(self):
"""Stop visualization."""
self.destroy_subscription(self.state_subscriber)
self.destroy_publisher(self.frame_publisher)
self.destroy_publisher(self.view_cone_publisher)
super().stop()
[docs]
def state_cb(self, msg: CarStateMsg):
"""Call when car state is published.
Arguments:
msg (CarStateMsg): Msg published by car state node
"""
frame_marker = visualization.get_marker_for_points(
(Point(p) for p in msg.frame.points),
frame_id=self.param.vehicle_simulation_link.frame.simulation,
rgba=[0, 0, 1, 0.7],
)
self.frame_publisher.publish(frame_marker)
if len(msg.view_cone.points):
cone_marker = visualization.get_marker_for_points(
(Point(p) for p in msg.view_cone.points),
frame_id=self.param.vehicle_simulation_link.frame.simulation,
id=1,
)
self.view_cone_publisher.publish(cone_marker)
[docs]
def main(args=None):
"""Console-script entry point for the car state visualization node."""
rclpy.init(args=args)
try:
CarStateVisualizationNode()
except KeyboardInterrupt:
pass
finally:
if rclpy.ok():
rclpy.shutdown()