visualize points rviz python
Hello. I am trying to visualize a set o points in rviz. I have a node who subscribes to a topic to get some data and and publishes a Marker msg on a topic.
#!/usr/bin/env python
import rospy
from std_msgs.msg import String
import time
import numpy as np
import math
from time import sleep
from laser_line_extraction.msg import LineSegment
from laser_line_extraction.msg import LineSegmentList
from box_finder_var import *
from visualization_msgs.msg import Marker
from geometry_msgs.msg import Quaternion, Pose, Point, Vector3
from std_msgs.msg import Header, ColorRGBA
rospy.init_node('box_finder', anonymous=True)
marker_publisher = rospy.Publisher('box_visual', Marker, queue_size=100)
rospy.sleep(1)
def callback(data):
# calculate some points...
### Visualize the L-shape elements in rviz ###
show_points_in_rviz(marker_publisher)
def show_points_in_rviz(marker_publisher):
marker = Marker(
ns='box_finder',
type=Marker.POINTS,
action=Marker.ADD,
id=0,
lifetime=rospy.Duration(),
pose=Pose(Point(1.5, 1.5, 0.0), Quaternion(0, 0, 0, 1)),
scale=Vector3(0.5, 0.5, 0.0),
header=Header(frame_id='base_link'),
color=ColorRGBA(0.0, 0.0, 1.0, 0.8),
# lifetime=0,
frame_locked=False)
marker_publisher.publish(marker)
def box_finder():
rospy.Subscriber("/line_segments", LineSegmentList, callback)
rospy.spin()
if __name__ == '__main__':
box_finder()
When I run the node I check with rostopic echo what is published and I see this:
But when I add the Marker element in rviz I don't see any point there. It's clear that I'm missing something here. Can anyone help or give a hint?
I use Ubuntu 16.04, ROS kinetic, gazebo 7
Thank you in advance!
In rviz, under "global options" do you have "Fixed Frame" set to "base_link"?
Also @Spyros can you please format your questions better, it is challenging to understand.
@JackB yes, fixted frame is set to base_link
Why do you never fill out the points field? Is that not what you want to visualize? See here and look how they fill out the points and then set the marker.points field. In your message you echoed the points[] field is empty
First I try to visualize a simple point to see if it works (code above). The position of the points isn't defined by the pose element of Marker? In this case, I should have a marker at 1.5, 1.5 with respect to the base_link frame, right?
Incorrect, see rviz display types points. The pose that you specify represents where the object containing the points will be located (i.e. "transformed to"). If you do not fill out the points[], there will be NO points.
Edit: so sure you have a marker at (1.5, 1.5) but it is empty because you didn't fill out the points and therefore you can't visualize it. Does that make sense?
@Spyros any update on this?
Yes, thanks a lot for the help. I played around a bit with it. I managed to make the points appear. If I get it correctly the points given in points[] are shown with respect to the position given in pose and the given coordinates frame, right? Also, I didn't understand what is the purpose of the frame_locked (I read the documentation but I didn't get the explanation). I tried both false and true but it didn't change anything on the output.