Visualization marker not appearing in RViz
I am trying to draw a camera frustum but to begin with, I am drawing a few lines on my RViz visualization that is also showing a 3D point cloud ("map" topic). I am able to see my 3D point cloud in RViz, which I publish to a new topic called "map2" of type PointCloud2. However, I am not able to view the visualization marker that I wrote Python code for. How do I visualize a line that moved with the base_link frame?
Please view my code and help!
import pdb
import rospy
import roslib
import math
import tf
import numpy as np
# import sensor_msgs.point_cloud2 as pc2
from sensor_msgs.msg import PointCloud2, PointField
from ros_numpy.point_cloud2 import pointcloud2_to_array, array_to_pointcloud2, split_rgb_field, merge_rgb_fields
from tf.msg import tfMessage
# for RGBD camera frustum visualization
from visualization_msgs.msg import Marker, MarkerArray
from geometry_msgs.msg import Quaternion, Pose, Point, Vector3, PointStamped
from std_msgs.msg import Header, ColorRGBA
import cv2
import pyrealsense2 as realsense
lastpcl_data = ""
started = False
pclpublisher = rospy.Publisher(
'/map2', PointCloud2, latch=True, queue_size=1000)
marker_publisher = rospy.Publisher(
'visualization_marker', Marker, queue_size=2)
# markers_publisher = rospy.Publisher('visualization_marker_array', MarkerArray)
def frustum_visualize(tf_data):
global marker, started
marker = Marker()
marker.header.stamp = rospy.Time.now()
marker.header.frame_id = "map2"
marker.ns = "view_frustum"
marker.type = Marker.LINE_LIST
marker.action = Marker.ADD
marker.pose.position.x = 0
marker.pose.position.y = 0
marker.pose.position.z = 0
marker.pose.orientation.x = 0
marker.pose.orientation.y = 0
marker.pose.orientation.z = 0
marker.pose.orientation.w = 1.0
marker.scale.x = 0.01
marker.color.a = 1.0
marker.color.r = 1.0
camera = PointStamped()
camera_map = PointStamped()
camera.header.stamp = rospy.Time.now()
camera.header.frame_id = "map2"
camera.point.x = 0.0
camera.point.y = 0.0
camera.point.z = 0.0
# tf.TransformListener.transformPoint("/map2", camera, camera_map)
br = PointStamped()
br_map = PointStamped()
br.header.stamp = rospy.Time.now()
br.header.frame_id = "map2"
br.point.x = -4
br.point.y = -2
br.point.z = 9
# tf.TransformListener.transformPoint("/map2", br, br_map)
marker.points.append(camera.point)
marker.points.append(br.point)
# marker.points.push_back(camera_map.point)
# marker.points.push_back(br_map.point)
# print(type(tf_data)) #<class 'tf.msg._tfMessage.tfMessage'>
# print(dir(tf_data.transforms[-1].transform.translation.x))
# tf_data.transforms[-1].transform.translation.x
# tf_data.transforms[-1].transform.rotation
# print('CHECKPOINT ****2', "\n")
# if (not started):
# started = True
def pcl_callback(pcl2data):
print("New message received", "\n")
global started, lastpcl_data
pcl_data = pcl2data # <class 'sensor_msgs.msg._PointCloud2.PointCloud2'>
pcl2_array = pointcloud2_to_array(pcl_data)
pcl2_array = split_rgb_field(pcl2_array)
print('pcl2 array shape BEFORE color separation: ', pcl2_array.shape)
# maskint1 = (pcl2_array['r'] <= 190)
# maskint2 = (pcl2_array['g'] <= 190)
# maskint3 = (pcl2_array['b'] >=140)
maskint1 = (pcl2_array['r'] <= 210)
maskint2 = (pcl2_array['g'] <= 210)
maskint3 = (pcl2_array['b'] >= 130)
pcl2_array = pcl2_array[np.logical_not(
np.logical_and(maskint1, maskint2, maskint3))]
maskint4 = (pcl2_array['z'] >= 0.35)
pcl2_array = pcl2_array[np.logical_not(maskint4)]
pcl2_array = merge_rgb_fields(pcl2_array)
print('pcl2 array shape AFTER color separation: ', pcl2_array.shape)
# <class 'sensor_msgs.msg._PointCloud2.PointCloud2'>
lastpcl_data = array_to_pointcloud2(
pcl2_array, stamp=pcl2data.header.stamp, frame_id=pcl2data.header.frame_id ...
So the package I suggested in #q410027 didn't work?
I tried using the jsk_rviz_plugins/camera_info plugin. I am getting an error when I try to use the CameraInfo Display in RViz when I am trying to choose the topic. I tried using the /camera/color/camera_info and /camera/depth/camera_info topics; there is an error related to the missing tf. Please help I put the error message below. How do I go about fixing this? Thank you.
You'll need a complete TF tree for the plugin to work. RViz places objects in its 3D view based on TF data. If the transform between
mapandcamera_color_optical_frameis not available, it (ie: the plugin) will not be able to render the frustum at the correct location.You'll have to make sure to provide the required transforms.
Note that this is not a "problem" with, nor limited to the plugin I recommended: any method with which you'd want RViz to render additional geometry for you, and which uses the
frame_idto provide spatial information would require a working TF setup.I am extremely grateful for your help. So I have determined that I will need the exact same tf between the "map" link and "base_link" link. How do I assign this same tf to be the tf between map and camera_color_optical_frame? How do I create a complete TF tree for the plugin to work? Is the following question going to help me?
https://answers.ros.org/question/2984...
Edit: I tried using
rosrun tf static_transform_publisher 0 0 0 0 0 0 map camera_color_optical_frame 10this drew the camera frustum but I realized that (obviously) because this is a static transform, the camera frustum will not move with the base_link. My approach now should be to write a node that subscribes to the tf between "map" and "base_link" and then publish the tf between "map" and "camera_color_optical_frame" frames right?
If you have a link which represents your vehicle/robot/camera base, why not publish a transform between that link and
camera_color_optical_frame?I would suggest to determine what the TF tree was/is for the hw configuration you were/are using and just using that.
And note: you'll have to have a tree with all the transforms between whatever you've configured as the Fixed Frame in RViz and your
camera_color_optical_frame. Otherwise it will still not work.The link which represents my vehicle/robot/camera base is "base_link." I am confused on how I can publish a transform between that link and the
camera_color_optical_framebecause I only have the tf between the "map" link (i.e. this is the Fixed Frame link) and the "base_link" link. My TF tree is unconnected as shown in this rqt_tf_tree graph:https://drive.google.com/file/d/1LhhN...
Also, how do I publish the transform between the map link and
camera_color_optical_frame? Do I create another node in my ROS graph? Thank you.you only have a single TF transform? It's possible, but in practice doesn't really work very well (as I believe you've just discovered).
if you know how your robot was/is structured, you could use either a URDF or the static transform publisher to publish an appropriate transform between
base_linkandcamera_color_optical_frame. That should make things work.Remember: TF is a tree (ie: a sequence of parent-child relationships with forks allowed (ie: parents reused by multiple children, as long as there are no cycles)), not a star topology where everything is connected to a single parent.
Thanks a lot for your help. I want to publish a tf between
base_linkandcamera_color_optical_framewhich is the same tf betweenmapandbase_link. Suppose I do this, I will end up connecting the 2 unconnected TF trees as shown in this image right? https://drive.google.com/file/d/1LhhN...I want to try and publish a static transform between
base_linkandcamera_color_optical_frame. Since the this transform will be static, will my camera frustum marker move according to howbase_linkmoves? To publish the static transform, I have to follow this right?