import math
import rclpy
from rclpy.node import Node
from visualization_msgs.msg import Marker
from geometry_msgs.msg import Point
class MarkerPublisher(Node):
def init(self):
super().init(‘marker_publisher’)
self.marker_pub = self.create_publisher(Marker, ‘visualization_marker’, 10)
self.timer = self.create_timer(0.1, self.timer_callback)
self.f = 0.0
def timer_callback(self):
# Create markers
points = Marker()
line_strip = Marker()
line_list = Marker()
# Set the header of the markers
points.header.frame_id = "map"
line_strip.header.frame_id = "map"
line_list.header.frame_id = "map"
# Set the ID of the markers
points.id = 0
line_strip.id = 1
line_list.id = 2
# Set the type of the markers
points.type = Marker.POINTS
line_strip.type = Marker.LINE_STRIP
line_list.type = Marker.LINE_LIST
# Set the scale of the markers
points.scale.x = 0.2
points.scale.y = 0.2
line_strip.scale.x = 0.1
line_list.scale.x = 0.1
# Set the color of the markers
points.color.g = 1.0
points.color.a = 1.0
line_strip.color.b = 1.0
line_strip.color.a = 1.0
line_list.color.r = 1.0
line_list.color.a = 1.0
# Set the number of points/lines for each marker
num_points = 10
for i in range(num_points):
x = i * 0.1
y = math.sin(x + self.f)
z = math.cos(x + self.f)
# Add point to points marker
p = Point()
p.x = x
p.y = y
p.z = z
points.points.append(p)
# Add point to line_strip marker
line_strip.points.append(p)
# Add two points to line_list marker, representing a line segment
line_list.points.append(p)
p.z += 1.0
line_list.points.append(p)
self.marker_pub.publish(points)
self.marker_pub.publish(line_strip)
self.marker_pub.publish(line_list)
# Update f for next iteration
self.f += 0.04
if self.f > 2 * math.pi:
self.f -= 2 * math.pi
def main(args=None):
rclpy.init(args=args)
marker_publisher = MarkerPublisher()
rclpy.spin(marker_publisher)
marker_publisher.destroy_node()
rclpy.shutdown()
if name == ‘main’:
main()