[Show Point]

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()

评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值