在 rviz 中绘制多条路径

Question

我正在尝试在 rviz 中绘制不同的路径

我正在使用以下代码获得第一种方法（基于此存储库：https://github.com/HaoQChen/show_trajectory/tree/master/src）

import rospy
import math
import numpy as np
from geometry_msgs.msg import PoseStamped
from nav_msgs.msg import Path, Odometry
from std_msgs.msg import Empty


class ProjectElement(object):
    def __init__(self):
        self.path_pub = rospy.Publisher('~path', Path, latch=True, queue_size=10)
        self.circle_sub = rospy.Subscriber('~circle', Empty, self.circle_cb, queue_size=10)
        self.line_sub = rospy.Subscriber('~line', Empty, self.line_cb, queue_size=10)
        self.project_sub = rospy.Subscriber('~project', Empty, self.project_cb, queue_size=10)
        
        self.paths = []

        self.rate = rospy.Rate(50)

    def circle_cb(self, msg):
        
        path = Path()

        centre_x = 1
        centre_y = 1
        R = 0.5
        th = 0.0
        delta_th = 0.1

        while (th<2*math.pi):
            x = centre_x + R * math.sin(th)
            y = centre_y + R * math.cos(th)
            th += delta_th

            this_pose_stamped = PoseStamped()
            this_pose_stamped.pose.position.x = x
            this_pose_stamped.pose.position.y = y

            this_pose_stamped.header.stamp = rospy.get_rostime()
            this_pose_stamped.header.frame_id = "/my_cs"

            path.poses.append(this_pose_stamped)

        path.header.frame_id = "/my_cs"
        path.header.stamp = rospy.get_rostime()
        
        self.paths.append(path)

    def line_cb(self, msg):
        
        path = Path()

        x_start = 0.0
        y_start = 0.0
        length = 2
        angle = 45 * math.pi/180
        th = 0.0
        delta_th = 0.1

        while (th<length):
            x = x_start + th * math.cos(angle)
            y = y_start + th * math.sin(angle)
            th += delta_th

            this_pose_stamped = PoseStamped()
            this_pose_stamped.pose.position.x = x
            this_pose_stamped.pose.position.y = y

            this_pose_stamped.header.stamp = rospy.get_rostime()
            this_pose_stamped.header.frame_id = "/my_cs"

            path.poses.append(this_pose_stamped)

        path.header.frame_id = "/my_cs"
        path.header.stamp = rospy.get_rostime()

        self.paths.append(path)  

    def project_cb(self, msg):

        while(True):
            for element in self.paths:
                # element.header.stamp = rospy.get_rostime()
                self.path_pub.publish(element)


if __name__ == '__main__':
    
    rospy.init_node('path_simulate')

    elements = ProjectElement()

    rospy.spin()

我可以在 rviz 中可视化路径，但我不知道如何以这种方式同时绘制两个图形。

line

circle

我想问一下这种方法是否是解决这个问题的最佳方法，或者哪种方法可能是最好的方法。

Answer 1

我终于根据这个问题用visualization_msgs/MarkerArray解决了： https://answers.ros.org/question/220898/how-to-use-rviz-to-show-multiple-planing-paths/?answer=220993#post-id-220993

我post这里使用的代码以备不时之需

import rospy
import math
import numpy as np
from geometry_msgs.msg import Vector3, Point
from std_msgs.msg import Empty
from visualization_msgs.msg import Marker, MarkerArray

class ProjectElement(object):
    def __init__(self):
        self.marker_pub = rospy.Publisher('~marker', MarkerArray, latch=True, queue_size=10)
        self.circle_sub = rospy.Subscriber('~circle', Empty, self.circle_cb, queue_size=10)
        self.line_sub = rospy.Subscriber('~line', Empty, self.line_cb, queue_size=10)
        self.project_sub = rospy.Subscriber('~project', Empty, self.project_cb, queue_size=10)

        self.marker_array = MarkerArray()

    def circle_cb(self, msg):

        marker = Marker()
        marker.type = Marker.LINE_STRIP

        marker.action = Marker.ADD
        
        marker.scale = Vector3(0.01, 0.01, 0)

        marker.color.g = 1.0
        marker.color.a = 1.0

        centre_x = 1
        centre_y = 1
        R = 0.5
        delta_th = 0.1

        for th in np.arange(0.0, 2*math.pi+delta_th, delta_th):
            x = centre_x + R * math.sin(th)
            y = centre_y + R * math.cos(th)

            point = Point()
            point.x = x
            point.y = y

            marker.points.append(point)

        marker.id = 0
        marker.header.stamp = rospy.get_rostime()
        marker.header.frame_id = "/my_cs"

        self.marker_array.markers.append(marker)

    def line_cb(self, msg):
        
        marker = Marker()
        marker.type = Marker.LINE_STRIP

        marker.action = Marker.ADD
        
        marker.scale = Vector3(0.01, 0.01, 0)

        marker.color.g = 1.0
        marker.color.a = 1.0

        x_start = 0.0
        y_start = 0.0
        length = 2
        angle = 45 * math.pi/180
        delta_th = 0.1

        for th in np.arange(0.0, length, delta_th):
            x = x_start + th * math.cos(angle)
            y = y_start + th * math.sin(angle)

            point = Point()
            point.x = x
            point.y = y

            marker.points.append(point)

        marker.id = 1
        marker.header.stamp = rospy.get_rostime()
        marker.header.frame_id = "/my_cs"

        self.marker_array.markers.append(marker)

    def project_cb(self, msg):

        self.marker_pub.publish(self.marker_array)


if __name__ == '__main__':

    rospy.init_node('markers_simulate')

    elements = ProjectElement()

    rospy.spin()

并取得了成果

marker array display

在 rviz 中绘制多条路径

Plot multiple paths in rviz

path

markers

ros