def publish_cloud_markers(pub_ns, _arr, c='b', stamp=None, flip_rb=False, frame_id='map'):
"""
Publish point cloud on:
pub_ns: Namespace on which the cloud will be published
arr: numpy array (N x 3) for point cloud data
c: Option 1: 'c', 'b', 'r' etc colors accepted by matplotlibs color
Option 2: float ranging from 0 to 1 via matplotlib's jet colormap
Option 3: numpy array (N x 3) with r,g,b vals ranging from 0 to 1
s: supported only by matplotlib plotting
alpha: supported only by matplotlib plotting
"""
arr, carr = copy_pointcloud_data(_arr, c, flip_rb=flip_rb)
marker = vis_msg.Marker(type=vis_msg.Marker.SPHERE_LIST, ns=pub_ns, action=vis_msg.Marker.ADD)
marker.header.frame_id = frame_id
marker.header.stamp = stamp if stamp is not None else rospy.Time.now()
marker.scale.x = 0.01
marker.scale.y = 0.01
# XYZ
inds, = np.where(~np.isnan(arr).any(axis=1))
marker.points = [geom_msg.Point(arr[j,0], arr[j,1], arr[j,2]) for j in inds]
# RGB (optionally alpha)
N, D = arr.shape[:2]
if D == 3:
marker.colors = [std_msg.ColorRGBA(carr[j,0], carr[j,1], carr[j,2], 1.0)
for j in inds]
elif D == 4:
marker.colors = [std_msg.ColorRGBA(carr[j,0], carr[j,1], carr[j,2], carr[j,3])
for j in inds]
marker.lifetime = rospy.Duration()
_publish_marker(marker)
print 'Publishing marker', N
def publish_line_segments(pub_ns,
_arr1,
_arr2,
c='b',
stamp=None,
frame_id='camera',
flip_rb=False,
size=0.002):
"""
Publish point cloud on:
pub_ns: Namespace on which the cloud will be published
arr: numpy array (N x 3) for point cloud data
c: Option 1: 'c', 'b', 'r' etc colors accepted by matplotlibs color
Option 2: float ranging from 0 to 1 via matplotlib's jet colormap
Option 3: numpy array (N x 3) with r,g,b vals ranging from 0 to 1
s: supported only by matplotlib plotting
alpha: supported only by matplotlib plotting
"""
arr1, carr = copy_pointcloud_data(_arr1, c, flip_rb=flip_rb)
arr2 = (deepcopy(_arr2)).reshape(-1, 3)
marker = vis_msg.Marker(type=vis_msg.Marker.LINE_LIST,
ns=pub_ns,
action=vis_msg.Marker.ADD)
if not arr1.shape == arr2.shape: raise AssertionError
marker.header.frame_id = frame_id
marker.header.stamp = stamp if stamp is not None else rospy.Time.now()
marker.scale.x = size
marker.scale.y = size
marker.color.b = 1.0
marker.color.a = 1.0
marker.pose.position = geom_msg.Point(0, 0, 0)
marker.pose.orientation = geom_msg.Quaternion(0, 0, 0, 1)
# Handle 3D data: [ndarray or list of ndarrays]
arr12 = np.hstack([arr1, arr2])
inds, = np.where(~np.isnan(arr12).any(axis=1))
marker.points = []
for j in inds:
marker.points.extend([
geom_msg.Point(arr1[j, 0], arr1[j, 1], arr1[j, 2]),
geom_msg.Point(arr2[j, 0], arr2[j, 1], arr2[j, 2])
])
# RGB (optionally alpha)
marker.colors = [
std_msg.ColorRGBA(carr[j, 0], carr[j, 1], carr[j, 2], 1.0)
for j in inds
]
marker.lifetime = rospy.Duration()
_publish_marker(marker)
def publish_cloud(pub_ns, _arr, c='b', stamp=None, flip_rb=False, frame_id='map', seq=None):
"""
Publish point cloud on:
pub_ns: Namespace on which the cloud will be published
arr: numpy array (N x 3) for point cloud data
c: Option 1: 'c', 'b', 'r' etc colors accepted by matplotlibs color
Option 2: float ranging from 0 to 1 via matplotlib's jet colormap
Option 3: numpy array (N x 3) with r,g,b vals ranging from 0 to 1
s: supported only by matplotlib plotting
alpha: supported only by matplotlib plotting
"""
arr, carr = copy_pointcloud_data(_arr, c, flip_rb=flip_rb)
pc = xyzrgb_array_to_pointcloud2(arr, carr, stamp=stamp, frame_id=frame_id, seq=seq)
_publish_pc(pub_ns, pc)
def publish_point_type(pub_channel,
_arr,
c='r',
point_type='POINT',
flip_rb=False,
frame_id='camera',
element_id=0,
reset=True):
"""
Publish point cloud on:
pub_channel: Channel on which the cloud will be published
arr: numpy array (N x 3) for point cloud data
c: Option 1: 'c', 'b', 'r' etc colors accepted by matplotlibs color
Option 2: float ranging from 0 to 1 via matplotlib's jet colormap
Option 3: numpy array (N x 3) with r,g,b vals ranging from 0 to 1
s: supported only by matplotlib plotting
alpha: supported only by matplotlib plotting
Supports
POINT=1, LINE_STRIP=2, LINE_LOOP=3, LINES=4,
TRIANGLE_STRIP=5, TRIANGLE_FAN=6, TRIANGLES=7,
QUAD_STRIP=8, QUADS=9, POLYGON=10
"""
global g_viz_pub
# point3d list collection msg
pc_list_msg = vs.point3d_list_collection_t()
pc_list_msg.id = g_viz_pub.channel_uid(pub_channel)
pc_list_msg.name = pub_channel
pc_list_msg.type = getattr(vs.point3d_list_collection_t, point_type)
pc_list_msg.reset = reset
# pc_list_msg.point_lists = []
# Create the point cloud msg
if isinstance(_arr, list) or isinstance(_arr, deque):
element_ids = element_id if isinstance(element_id,
list) else [0] * len(_arr)
# print 'Multiple elements: ', element_ids
assert (len(c) == len(_arr))
for element_id, _arr_item, _carr_item in izip(element_ids, _arr, c):
arr, carr = copy_pointcloud_data(_arr_item,
_carr_item,
flip_rb=flip_rb)
pc_msg = arr_msg(arr,
carr=carr,
frame_uid=g_viz_pub.channel_uid(frame_id),
element_id=element_id)
pc_list_msg.point_lists.extend([pc_msg])
else:
# print 'Single element: ', element_id
arr, carr = copy_pointcloud_data(_arr, c, flip_rb=flip_rb)
pc_msg = arr_msg(arr,
carr=carr,
frame_uid=g_viz_pub.channel_uid(frame_id),
element_id=element_id)
pc_list_msg.point_lists.extend([pc_msg])
# add to point cloud list
# print('published %i lists %s' % (len(_arr), reset))
pc_list_msg.nlists = len(pc_list_msg.point_lists)
g_viz_pub.publish("POINTS_COLLECTION", serialize(pc_list_msg))