# Plot multiple paths in rviz I'm trying to plot different paths at rviz

I'm using the following code to get a first approach (based on this repository: https://github.com/HaoQChen/show_traj...)

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

path.poses.append(this_pose_stamped)

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

path.poses.append(this_pose_stamped)

self.paths.append(path)

def project_cb(self, msg):

while(True):
for element in self.paths:
self.path_pub.publish(element)
if __name__ == '__main__':

rospy.init_node('path_simulate')

elements = ProjectElement()

rospy.spin()


I can visualize the paths at rviz, but I don't know how to plot both figures at the same time in this way. line rviz image circle rviz image

I would like to ask if this approach is the best way to address this issue or which could be the best way.

edit retag close merge delete

Instead of a single path_pub on the 'path' topic, you could have multiple path publishers on different topics (e.g. 'path1' & 'path2'), and an rviz Path display for each. But MarkerArray is more flexible especially if you want to scale up to many more paths than two.

Sort by » oldest newest most voted

This question was solved here: https://stackoverflow.com/questions/6...

(moderator edit: duplicating the contents of the cross-post on SO here, to avoid this being a link-only answer)

I finally solved with visualization_msgs/MarkerArray based on this question: https://answers.ros.org/question/2208...

I post the code used here in case anyone needs it

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.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

self.marker_array.markers.append(marker)

def line_cb(self, msg):

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

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

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


And the result achieved more