# Data Visualization

This tutorial aims to provide a basis for displaying ROS2 data. It can be applied for any topic or message.

# Prerequisites

Setup, tutorial 1, and tutorial 2 must be completed to understand the following.
Have an understanding on what topics are, how to publish and subscribe to them (tutorial 1), and how messages are related to topics.

# Markers

ROS markers are a data type that allow renderers (foxglove studio, rviz, etc) to display a wide variety of elements ranging from fundamentals, such as lines, to meshes that form avocados (https://foxglove.dev/blog/how-to-visualize-ros-mesh-markers).

# File Structure

When it comes to file organization in ROS, it is typically best to keep packages and nodes as modular as possible. In the case of data visualization, this means that you should keep node fundamentals separate from its helpers. An example would be: you are trying to show a line between the vehicle and its target waypoint. Intuitively, it is simpler to display the marker in the waypoint following node. However, for the sake of modularity, this should be avoided. Instead, publish the target waypoint from the waypoint following node, create a separate "painter" package, and then subscribe and display. This method avoids node cluttering and allows you to not display the visuals by default (which is typically desired behavior, as most visualization will be used for debugging and testing). Additionally, publishing more data is more beneficial in the long term when more nodes are added. On the other hand, if your node has a very simple purpose, say publishing waypoints from a file, and you know that you want the visual to be constantly enabled, then publishing the markers directly through the waypoint publisher is correct.

TLDR: keep code modular and clean. If drawing stuff makes your node ugly, move it to a separate "painting" package.

# Create a node

Or don't... read above on "file structure" and decide.

Refer to tutorial 1 on how to set up a package and a node. It should be named "painter", "visualizer", or something else informative.

# Line displaying

To publish lines, we will use a helper function:

    def draw_line(self, c1, c2, id=0, color=(0.0, 1.0, 0.0)):
        # p1, p2 to Points
        p1 = Point()
        p1.x = c1[0]
        p1.y = c1[1]
        p1.z = 0.1

        p2 = Point()
        p2.x = c2[0]
        p2.y = c2[1]
        p2.z = 0.1

        marker = Marker()
        marker.header.frame_id = "map"
        marker.id = id
        marker.type = marker.LINE_STRIP
        marker.action = marker.ADD
        marker.points = [p1, p2]
        marker.pose.orientation.w = 1.0
        scale = 0.1  # adjust if needed
        marker.scale.x = scale
        marker.scale.y = scale
        marker.scale.z = scale
        marker.color.a = 1.0
        marker.color.r = color[0]
        marker.color.g = color[1]
        marker.color.b = color[2]

        self._line_publisher.publish(marker)

The function creates two Points (another ROS type), assigns it to a marker, sets its parameters, and publishes. To make a line with many points, appened more Points to marker.points.

If the function is run multiple times, setting a separate id is essential otherwise it will try to overwrite the previous line.

# Painter Node

Next we will subscribe to odom and call our draw_line function to display the car's direction as a line.

class PainterNode(Node):
    def __init__(self):
        super().__init__("painter_node")
        self._line_publisher = self.create_publisher(Marker, "/line_test", 10)

        # subscribe to /odom
        self.subscription = self.create_subscription(
            Odometry, "/odom", self.odom_callback, 10
        )

        self._i = 0

    def odom_callback(self, msg):
        self._i += 1
        self.get_logger().info("Publishing line %d" % self._i)

        pose = msg.pose.pose
        car_pos = (pose.position.x, pose.position.y)

        # car direction
        quaternion = (
            pose.orientation.x,
            pose.orientation.y,
            pose.orientation.z,
            pose.orientation.w,
        )
        euler = tf_transformations.euler_from_quaternion(quaternion)
        car_direction = euler[2]  # radians

        draw_point = (
            car_pos[0]
            + math.cos(car_direction),
            car_pos[1]
            + math.sin(car_direction),
        )

        # draws from car to car movement direction
        self.draw_line(
            car_pos,
            draw_point,
            id=0,
            color=(1.0, 1.0, 0.0),
        )

Make sure to add a main function and make necessary imports.

If compiled and run, the node should output Publishing line {number}. Enabling the /line_test topic on foxglove studio should display a line in front of the car. Good job!

# Drawing the car's instantaneous movement

Add:

car_speed = msg.twist.twist.linear.x
car_steering = msg.twist.twist.angular.z

below car_pos to obtain the car's speed and steering angle (in radians).

then modify draw_point to:

draw_point = (
    car_pos[0]
    + math.cos(car_direction + car_steering / math.pi) * car_speed * 0.5,
    car_pos[1]
    + math.sin(car_direction + car_steering / math.pi) * car_speed * 0.5,
)

This will adust the line's length based on the car speed and steering angle. In autonomous vehicles, continous steering and acceleration is required for a smooth driving experience. Our painter node allows us to analyze whether our steering and velocity are abrupt or not.

# Visualizing an array of markers

Simply publish a MarkerArray (https://docs.ros2.org/galactic/api/visualization_msgs/msg/MarkerArray.html). This is useful to publish different marker types (see types: https://docs.ros2.org/galactic/api/visualization_msgs/msg/Marker.html). However, if you're seeking to display waypoints, a single Marker using "Sphere list" (type 7) will be more efficient.

# Final code

import rclpy
from rclpy.node import Node

from visualization_msgs.msg import Marker
from geometry_msgs.msg import Point
from nav_msgs.msg import Odometry

import tf_transformations  # should be installed otherwise apt install ros-iron-tf-transformations

import math


class PainterNode(Node):
    def __init__(self):
        super().__init__("painter_node")
        self._line_publisher = self.create_publisher(Marker, "/line_test", 10)

        # subscribe to /odom
        self.subscription = self.create_subscription(
            Odometry, "/odom", self.odom_callback, 10
        )

        self._i = 0

    def odom_callback(self, msg):
        self._i += 1
        self.get_logger().info("Publishing line %d" % self._i)

        pose = msg.pose.pose
        car_pos = (pose.position.x, pose.position.y)
        car_speed = msg.twist.twist.linear.x
        car_steering = msg.twist.twist.angular.z

        # car direction
        quaternion = (
            pose.orientation.x,
            pose.orientation.y,
            pose.orientation.z,
            pose.orientation.w,
        )
        euler = tf_transformations.euler_from_quaternion(quaternion)
        car_direction = euler[2]  # radians

        draw_point = (
            car_pos[0]
            + math.cos(car_direction + car_steering / math.pi) * car_speed * 0.5,
            car_pos[1]
            + math.sin(car_direction + car_steering / math.pi) * car_speed * 0.5,
        )

        # draws from car to car movement direction
        self.draw_line(
            car_pos,
            draw_point,
            id=0,
            color=(1.0, 1.0, 0.0),
        )

    def draw_line(self, c1, c2, id=0, color=(0.0, 1.0, 0.0)):
        # p1, p2 to Points
        p1 = Point()
        p1.x = c1[0]
        p1.y = c1[1]
        p1.z = 0.1
        p2 = Point()
        p2.x = c2[0]
        p2.y = c2[1]
        p2.z = 0.1

        marker = Marker()
        marker.header.frame_id = "map"
        marker.id = id
        marker.type = marker.LINE_STRIP
        marker.action = marker.ADD
        marker.points = [p1, p2]
        marker.pose.orientation.w = 1.0
        scale = 0.1  # adjust if needed
        marker.scale.x = scale
        marker.scale.y = scale
        marker.scale.z = scale
        marker.color.a = 1.0
        marker.color.r = color[0]
        marker.color.g = color[1]
        marker.color.b = color[2]

        self._line_publisher.publish(marker)


def main(args=None):
    rclpy.init(args=args)

    painter_node = PainterNode()

    rclpy.spin(painter_node)

    painter_node.destroy_node()

    rclpy.shutdown()


if __name__ == "__main__":
    main()

# Challenges

Display the car's active path using a kinematic bicycle model
Give the car's visual an upgrade using a mesh marker (avocado?)
Create and visualize a set of obstacles that are visible to the car