如何解决在rviz中绘制多个路径
我正在尝试在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',self.line_cb,queue_size=10)
self.project_sub = rospy.Subscriber('~project',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处看到路径，但是我不知道如何以这种方式同时绘制两个图形。
我想问一下这种方法是解决此问题的最佳方法，还是哪种可能是最佳方法。
解决方法
我终于根据以下问题使用visualization_msgs / MarkerArray解决了： https://answers.ros.org/question/220898/how-to-use-rviz-to-show-multiple-planing-paths/?answer=220993#post-id-220993
如果有人需要，我会在这里张贴代码
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',self.line_cb,queue_size=10)
self.project_sub = rospy.Subscriber('~project',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)
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()
取得了成果
版权声明：本文内容由互联网用户自发贡献，该文观点与技术仅代表作者本人。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如发现本站有涉嫌侵权/违法违规的内容， 请发送邮件至 dio@foxmail.com 举报，一经查实，本站将立刻删除。
