Path 在rviz中绘制多条路径

Path 在rviz中绘制多条路径,path,ros,markers,Path,Ros,Markers,我正试图在rviz绘制不同的路径 我使用以下代码获得第一种方法(基于此存储库:) 导入rospy 输入数学 将numpy作为np导入 从geometry_msgs.msg导入PoseStamped 从nav_msgs.msg导入路径,里程计 从std_msgs.msg导入空 类ProjectElement(对象): 定义初始化(自): self.path\u pub=rospy.Publisher('~path',path,lack=True,queue\u size=10) self.circ

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

我使用以下代码获得第一种方法(基于此存储库:)

导入rospy
输入数学
将numpy作为np导入
从geometry_msgs.msg导入PoseStamped
从nav_msgs.msg导入路径,里程计
从std_msgs.msg导入空
类ProjectElement(对象):
定义初始化(自):
self.path\u pub=rospy.Publisher('~path',path,lack=True,queue\u size=10)
self.circle\u sub=rospy.Subscriber(“~circle”,空,self.circle\u cb,队列大小=10)
self.line\u sub=rospy.Subscriber(“~line”,空,self.line\u cb,队列大小=10)
self.project\u sub=rospy.Subscriber(“~project”,空,self.project\u cb,队列大小=10)
self.path=[]
自费率=备用费率(50)
def循环_cb(自身,消息):
路径=路径()
中心x=1
中心y=1
R=0.5
th=0.0
delta_th=0.1

而(th我最终用visualization_msgs/Markerary解决了这个问题:

我将使用的代码张贴在这里,以防有人需要

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()
以及所取得的成果


你在rviz中使用的绘制路径的插件是什么?你考虑过使用两个不同的发布者吗?我没有使用任何插件,我在rviz上显示nav_msg/path。另一方面,我没有使用两个发布者,因为数字的数量是动态的(可以超过2个,也可以不超过2个),我只绘制了2个,作为内置“路径”的示例在rviz中显示只打印一条路径。我认为您正在寻找一种打印多条路径的方法,但即使这样,问题是rviz如何知道您的一个主题上新发布的路径是对现有路径的更新还是要打印的新路径--
nav_msgs/path
没有名称或id字段来区分它们。因此我认为您的日志ic不完整。您可能会从使用点云显示中得到您想要的。(并相应地联合发布所有路径)实际上,我无法以这种方式显示多个路径,因此我在下面的答案中选择了该选项。Marker ros msg具有id字段,以便我可以修改旧图形并添加新图形
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()