Ask Your Question
0

Plot multiple paths in rviz

asked 2020-09-07 07:16:56 -0500

rluque gravatar image

updated 2022-03-04 06:37:43 -0500

lucasw gravatar image

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

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

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 line rviz image

circle rviz 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 flag offensive close merge delete

Comments

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.

lucasw gravatar image lucasw  ( 2022-03-04 06:42:51 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2020-09-09 06:32:26 -0500

rluque gravatar image

updated 2020-09-09 11:06:07 -0500

gvdhoorn gravatar image

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

And the result achieved

image description

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2020-09-07 07:16:56 -0500

Seen: 337 times

Last updated: Sep 09 '20