def publish_sensor_frame(self, channel, pose=None):
"""
Publish sensor frame in which the point clouds
are drawn with reference to. sensor_frame_msg.id is hashed
by its channel (may be collisions since its right shifted by 32)
"""
# Sensor frames msg
msg = vs.obj_collection_t()
msg.id = self.channel_uid(channel)
msg.name = 'BOTFRAME_' + channel
msg.type = vs.obj_collection_t.AXIS3D
msg.reset = True
# Send sensor pose
pose_msg = vs.obj_t()
roll, pitch, yaw, x, y, z = pose.to_rpyxyz(axes='sxyz')
pose_msg.id = 0
pose_msg.x, pose_msg.y, pose_msg.z, \
pose_msg.roll, pose_msg.pitch, pose_msg.yaw = x, y, z, roll, pitch, yaw
# Save pose
self.set_sensor_pose(channel, pose)
# msg.objs = [pose_msg]
msg.objs.extend([pose_msg])
msg.nobjs = len(msg.objs)
self.publish("OBJ_COLLECTION", serialize(msg))
def publish_covar_list(pub_channel,
poses,
covars=[],
frame_id='camera',
reset=True):
covar_list_msg = vs.cov_collection_t()
covar_list_msg.name = pub_channel + '-covars'
covar_list_msg.id = g_viz_pub.channel_uid(covar_list_msg.name)
covar_list_msg.type = vs.cov_collection_t.ELLIPSOID
covar_list_msg.reset = reset
assert (len(poses) == len(covars))
nposes = len(covars)
covar_list_msg.covs = [vs.cov_t() for j in range(nposes)]
covar_list_msg.ncovs = nposes
for j, pose in enumerate(poses):
# assert(covars[j].ndim == 2 and covars[j].shape[1] == 6)
covar_list_msg.covs[j].id = getattr(pose, 'id', j)
covar_list_msg.covs[j].collection = g_viz_pub.channel_uid(pub_channel)
covar_list_msg.covs[j].element_id = getattr(pose, 'id', j)
covar_list_msg.covs[j].entries = covars[j]
covar_list_msg.covs[j].n = len(covars[j])
# print 'covar_list', nposes, covar_list_msg.covs[j].collection, covar_list_msg.covs[j].id, \
# covar_list_msg.covs[j].element_id, covars[j]
g_viz_pub.publish("COV_COLLECTION", serialize(covar_list_msg))
def publish_pose_t(channel, pose, frame_id='camera'):
global g_viz_pub
frame_pose = g_viz_pub.get_sensor_pose(frame_id)
out_pose = frame_pose.oplus(pose)
p = pose_t(list(out_pose.quat.to_wxyz()), out_pose.tvec.tolist())
g_viz_pub.publish(channel, serialize(p))
def publish_text_list(pub_channel,
poses,
texts=[],
frame_id='camera',
reset=True):
text_list_msg = vs.text_collection_t()
text_list_msg.name = pub_channel + '-text'
text_list_msg.id = g_viz_pub.channel_uid(text_list_msg.name)
text_list_msg.type = 1 # doesn't matter
text_list_msg.reset = reset
assert (len(poses) == len(texts))
nposes = len(texts)
text_list_msg.texts = [vs.text_t() for j in range(nposes)]
text_list_msg.n = nposes
for j, pose in enumerate(poses):
text_list_msg.texts[j].id = getattr(pose, 'id', j)
text_list_msg.texts[j].collection_id = g_viz_pub.channel_uid(
pub_channel)
text_list_msg.texts[j].object_id = getattr(pose, 'id', j)
text_list_msg.texts[j].text = texts[j]
g_viz_pub.publish("TEXT_COLLECTION", serialize(text_list_msg))
def publish_image_t(pub_channel, im, jpeg=False, flip_rb=True):
global g_viz_pub
out = image_t()
# Populate appropriate fields
h, w = im.shape[:2]
c = 3
out.width, out.height = w, h
out.row_stride = w * c
out.utime = 1
# Propagate encoded/raw data,
color_code = cv2.COLOR_GRAY2RGB if flip_rb else cv2.COLOR_GRAY2BGR
image = cv2.cvtColor(im, color_code) if im.ndim == 2 else im
# Propagate appropriate encoding
out.pixelformat = image_t.PIXEL_FORMAT_MJPEG if jpeg \
else image_t.PIXEL_FORMAT_RGB
out.data = cv2.imencode('.jpg', image)[1] if jpeg else image.tostring()
out.size = len(out.data)
out.nmetadata = 0
# Pub
g_viz_pub.publish(pub_channel, serialize(out))
def reset_visualization(self):
print('{} :: Reseting Visualizations'.format(self.__class__.__name__))
msg = vs.reset_collections_t()
self.publish("RESET_COLLECTIONS", serialize(msg))
def publish_pose_list(pub_channel,
poses,
texts=[],
covars=[],
frame_id='camera',
reset=True,
object_type='AXIS3D'):
"""
Publish Pose List on:
pub_channel: Channel on which the cloud will be published
element_id: None (defaults to np.arange(len(poses)), otherwise provide list of ids
"""
global g_viz_pub
# poses = deepcopy(_poses)
frame_pose = g_viz_pub.get_sensor_pose(frame_id)
# pose list collection msg
pose_list_msg = vs.obj_collection_t()
pose_list_msg.id = g_viz_pub.channel_uid(pub_channel)
pose_list_msg.name = pub_channel
pose_list_msg.type = getattr(vs.obj_collection_t, object_type)
pose_list_msg.reset = reset
nposes = len(poses)
# fill out points
pose_list_msg.objs.extend([vs.obj_t() for j in range(0, nposes)])
pose_list_msg.nobjs = nposes
inds = np.arange(0, nposes)
arr = np.zeros((len(poses), 3))
for j, pose in enumerate(poses):
arr[j, 0] = pose.tvec[0]
arr[j, 1] = pose.tvec[1]
arr[j, 2] = pose.tvec[2]
# Pose compounding
p = frame_pose.oplus(pose)
roll, pitch, yaw, x, y, z = p.to_rpyxyz(axes='sxyz')
# Optionally get the id of the pose,
# for plotting clouds with corresponding pose
# Note: defaults to index of pose
pose_list_msg.objs[j].id = getattr(pose, 'id', j)
pose_list_msg.objs[j].x = x
pose_list_msg.objs[j].y = y
pose_list_msg.objs[j].z = z
pose_list_msg.objs[j].roll = roll
pose_list_msg.objs[j].pitch = pitch
pose_list_msg.objs[j].yaw = yaw
g_viz_pub.publish("OBJ_COLLECTION", serialize(pose_list_msg))
# Publish corresponding text
if len(texts):
assert (len(texts) == len(poses))
publish_text_list(pub_channel,
poses,
texts,
frame_id=frame_id,
reset=reset)
# Publish corresponding covariances
if len(covars):
publish_covar_list(pub_channel,
poses,
covars,
frame_id=frame_id,
reset=reset)
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))
