def make_target_marker(self): #, scale=.2, color=stdm.ColorRGBA(.5,.5,.5,.5)):
if self.is_current_task_frame:
scale = .3
color = stdm.ColorRGBA(1,0,0,.5)
else:
scale = .2
color = stdm.ColorRGBA(.5,.5,.5,.5)
name = 'target_' + self.tagid
if not self.tag_database.has_id(self.tagid):
return
p_ar = self.tag_database.get(self.tagid)['target_location']
m_ar = tfu.tf_as_matrix(p_ar)
map_T_ar = tfu.transform('map', self.tagid, self.tf_listener, t=0)
p_map = tfu.matrix_as_tf(map_T_ar * m_ar)
pose = p_map
int_marker = interactive_marker(name, (pose[0], (0,0,0,1)), scale)
int_marker.header.frame_id = 'map' #self.tagid
int_marker.description = ''
int_marker.controls.append(make_sphere_control(name, scale/2.))
int_marker.controls[0].markers[0].color = color
int_marker.controls += make_directional_controls(name)
self.server_lock.acquire()
self.marker_server.insert(int_marker, self.marker_cb)
self.server_lock.release()
self.target_marker = int_marker
if self.menu != None:
self._make_menu()
def straighline_planner(self, obs=None):
waypoints = linear_interp_traj([[self.start_x, self.start_y], [self.goal_x, self.goal_y]], 0.1)
waypoints = np.array(waypoints)
lin_x, lin_y, angu_z, ind, obs_lin_x, obs_lin_y = self.holonomic_controller.control(waypoints, self.start_x, self.start_y, self.start_yaw, self.goal_yaw, self.vel_rob[0], self.vel_rob[1], obs)
self.vel_msg.linear.x = lin_x
self.vel_msg.linear.y = lin_y
self.vel_msg.angular.z = 0.0
# self.vel_msg.angular.z = angu_z
# self.velocity_pub.publish(self.vel_msg)
robot_marker = vis_msg.Marker(type=vis_msg.Marker.SPHERE_LIST, action=vis_msg.Marker.ADD)
robot_marker.header.frame_id = 'world'
robot_marker.header.stamp = rospy.Time.now()
robot_marker.scale.x = 0.5
robot_marker.scale.y = 0.5
robot_marker.points = [geom_msg.Point(self.start_x, self.start_y, 0.0)]
robot_marker.colors = [std_msg.ColorRGBA(1.0, 0.0, 0.0, 1.0)]
self.robot_marker_pub.publish(robot_marker)
goal_marker = vis_msg.Marker(type=vis_msg.Marker.SPHERE_LIST, action=vis_msg.Marker.ADD)
goal_marker.header.frame_id = 'world'
goal_marker.header.stamp = rospy.Time.now()
goal_marker.scale.x = 0.5
goal_marker.scale.y = 0.5
goal_marker.points = [geom_msg.Point(self.goal_x, self.goal_y, 0.0)]
goal_marker.colors = [std_msg.ColorRGBA(0.0, 0.0, 1.0, 1.0)]
self.goal_marker_pub.publish(goal_marker)
return waypoints, lin_x, lin_y, ind, obs_lin_x, obs_lin_y
def publish_octomap(pub_ns, arr, carr, size=0.1, stamp=None, frame_id='map', flip_rb=False):
"""
Publish cubes list:
"""
marker = vis_msg.Marker(type=vis_msg.Marker.CUBE_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()
# Point width, and height
marker.scale.x = size
marker.scale.y = size
marker.scale.z = size
N, D = arr.shape
# 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)
rax, bax = 0, 2
# carr = carr.astype(np.float32) * 1.0 / 255
if flip_rb: rax, bax = 2, 0
if D == 3:
marker.colors = [std_msg.ColorRGBA(carr[j,rax], carr[j,1], carr[j,bax], 1.0)
for j in inds]
elif D == 4:
marker.colors = [std_msg.ColorRGBA(carr[j,rax], carr[j,1], carr[j,bax], carr[j,3])
for j in inds]
marker.lifetime = rospy.Duration()
_publish_marker(marker)
def _make_marker(self): #, scale=.2, color=stdm.ColorRGBA(.5,.5,.5,.5)):
if self.is_current_task_frame:
scale = .3
color = stdm.ColorRGBA(1,0,0,.4)
else:
scale = .2
color = stdm.ColorRGBA(.5,.5,.5,.4)
self.marker_name = 'action_' + self.actionid
pose = self.location_in_frame
#print 'made marker with pose', pose
int_marker = interactive_marker(self.marker_name, (pose[0], pose[1]), scale)
#print int_marker.pose.orientation
int_marker.header.frame_id = self.frame #self.tagid
int_marker.description = self._make_description()
int_marker.controls.append(make_sphere_control(self.marker_name + '_1', scale/8.))
int_marker.controls.append(make_sphere_control(self.marker_name + '_2', scale))
int_marker.controls[1].markers[0].color = color
int_marker.controls += make_directional_controls(self.marker_name)
int_marker.controls += make_orientation_controls(self.marker_name)
self.server_lock.acquire()
self.marker_server.insert(int_marker, self.marker_cb)
self.server_lock.release()
self.marker_obj = int_marker
if self.menu_handler != None:
self._make_menu()
else:
self._make_empty_menu()
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 publishTagMarkers(tags, publisher):
"""Republishes the tag_markers topic"""
SHIFT = 0.05
h = std_msgs.Header(stamp=rospy.Time.now(), frame_id='map')
ca = std_msgs.ColorRGBA(r=1, a=1)
cb = std_msgs.ColorRGBA(g=1, a=1)
ct = std_msgs.ColorRGBA(b=1, a=1)
sa = geometry_msgs.Vector3(x=0.1, y=0.5, z=0.5)
sb = geometry_msgs.Vector3(x=0.1, y=0.25, z=0.25)
st = geometry_msgs.Vector3(x=1, y=1, z=1)
ma = visualization_msgs.MarkerArray()
ma.markers.append(
visualization_msgs.Marker(
header=h, id=-1, action=visualization_msgs.Marker.DELETEALL))
for i, t in enumerate(tags):
th = t['th_deg'] * math.pi / 180.
q = tf.transformations.quaternion_from_euler(0, 0, th)
q = geometry_msgs.Quaternion(x=q[0], y=q[1], z=q[2], w=q[3])
ma.markers.append(
visualization_msgs.Marker(
header=h,
id=i * 3,
type=visualization_msgs.Marker.CUBE,
action=visualization_msgs.Marker.ADD,
pose=geometry_msgs.Pose(
position=geometry_msgs.Point(x=t['x'], y=t['y']),
orientation=q),
scale=sa,
color=ca))
ma.markers.append(
visualization_msgs.Marker(
header=h,
id=i * 3 + 1,
type=visualization_msgs.Marker.CUBE,
action=visualization_msgs.Marker.ADD,
pose=geometry_msgs.Pose(
position=geometry_msgs.Point(
x=t['x'] + SHIFT * math.cos(th),
y=t['y'] + SHIFT * math.sin(th)),
orientation=q),
scale=sb,
color=cb))
ma.markers.append(
visualization_msgs.Marker(
header=h,
id=i * 3 + 2,
type=visualization_msgs.Marker.TEXT_VIEW_FACING,
action=visualization_msgs.Marker.ADD,
pose=geometry_msgs.Pose(
position=geometry_msgs.Point(x=t[1], y=t[2], z=0.5),
orientation=q),
scale=st,
color=ct,
text=str(t['id'])))
publisher.publish(ma)
def __init__(self, tagid, pose_in_map_frame, action_marker_manager, server_lock, marker_server, scale=.2):
self.tagid = tagid
self.action_marker_manager = action_marker_manager
self.server_lock = server_lock
self.marker_server = marker_server
self.marker_name = 'ar_marker_' + str(self.tagid)
int_marker = interactive_marker(self.marker_name, pose_in_map_frame, scale)
int_marker.scale = .6
self.marker_obj = int_marker
int_marker.description = 'Tag #%d' % self.tagid
#int_marker.controls += []
#int_marker.controls[0].markers[0].color = stdm.ColorRGBA(0,1,0,.5)
sph = make_sphere_control(self.marker_name, scale)
sph.markers[0].color = stdm.ColorRGBA(0,1,0,.5)
self.menu = mh.MenuHandler()
self.menu.insert('Create New Action', parent=None, callback=self.create_new_action_cb)
menu_control = ims.InteractiveMarkerControl()
menu_control.interaction_mode = ims.InteractiveMarkerControl.MENU
menu_control.description=""
menu_control.name = 'menu_tag_' + str(self.tagid)
menu_control.markers.append(copy.deepcopy(sph.markers[0]))
menu_control.always_visible = True
int_marker.controls += [menu_control]
self.server_lock.acquire()
self.marker_server.insert(int_marker, self.marker_cb)
self.menu.apply(self.marker_server, int_marker.name)
self.server_lock.release()
def draw_marker(self,
ps,
id=None,
type=Marker.CUBE,
ns='default_ns',
rgba=(0, 1, 0, 1),
scale=(.03, .03, .03),
text='',
duration=0):
"""
If markers aren't showing up, it's probably because the id's are being overwritten. Make sure to set
the ids and namespace.
"""
if id is None: id = self.get_unused_id()
marker = Marker(type=type, action=Marker.ADD)
marker.ns = ns
marker.header = ps.header
marker.pose = ps.pose
marker.scale = gm.Vector3(*scale)
marker.color = stdm.ColorRGBA(*rgba)
marker.id = id
marker.text = text
marker.lifetime = rospy.Duration(duration)
self.pub.publish(marker)
self.ids.add(id)
return MarkerHandle(marker, self.pub)
def circle_marker(center,
radius,
scale,
color,
mframe,
z=.03,
duration=10.0,
m_id=0,
resolution=1.):
angles = np.matrix(np.arange(0, 360., resolution)).T
xs = center[0, 0] + np.cos(angles) * radius
ys = center[1, 0] + np.sin(angles) * radius
z = .03
m = vm.Marker()
m.header.frame_id = mframe
m.id = m_id
m.type = create_mdict()['line_strip']
m.action = vm.Marker.ADD
m.points = [
gm.Point(xs[i, 0], ys[i, 0], z) for i in range(angles.shape[0])
]
m.colors = [
sdm.ColorRGBA(color[0, 0], color[1, 0], color[2, 0], color[3, 0])
for i in range(angles.shape[0])
]
m.scale.x = scale
m.lifetime = rospy.Duration(duration)
return m
def list_marker(points, colors, scale, mtype, mframe, duration=10.0, m_id=0):
m = vm.Marker()
m.header.frame_id = mframe
m.id = m_id
m.type = create_mdict()[mtype]
m.action = vm.Marker.ADD
m.points = [
gm.Point(points[0, i], points[1, i], points[2, i])
for i in range(points.shape[1])
]
#pdb.set_trace()
m.colors = [
sdm.ColorRGBA(colors[0, i], colors[1, i], colors[2, i], colors[3, i])
for i in range(colors.shape[1])
]
m.color.r = 1.
m.color.g = 0.
m.color.b = 0.
m.color.a = 1.
m.scale.x = scale[0]
m.scale.y = scale[1]
m.scale.z = scale[2]
m.lifetime = rospy.Duration(duration)
return m
def make_rviz_marker(scale):
marker = ims.Marker()
marker.type = ims.Marker.SPHERE
marker.scale.x = scale * 0.45
marker.scale.y = scale * 0.45
marker.scale.z = scale * 0.45
marker.color = stdm.ColorRGBA(.5,.5,.5,1)
return marker
def make_ar_marker(self, scale=.2):
name = 'ar_' + self.tagid
pose = self.tag_database.get(self.tagid)['ar_location']
int_marker = interactive_marker(name, (pose[0], (0, 0, 0, 1)), scale)
int_marker.description = 'Tag #%d' % self.get_id_numb()
int_marker.controls += [make_sphere_control(name, int_marker.scale)]
int_marker.controls[0].markers[0].color = stdm.ColorRGBA(0, 1, 0, .5)
self.server.insert(int_marker, self.process_feedback)
self.ar_marker = int_marker
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 make_target_marker(self, name, pose, scale=.2):
int_marker = interactive_marker(name, (pose[0], (0, 0, 0, 1)), scale)
int_marker.header.frame_id = 'map' #self.tagid
int_marker.description = ''
int_marker.controls.append(make_sphere_control(name, scale / 2.))
int_marker.controls[0].markers[0].color = stdm.ColorRGBA(1, 0, 0, .5)
int_marker.controls += make_directional_controls(name)
self.server.insert(int_marker, self.process_feedback)
self.target_marker = int_marker
if self.menu != None:
self._make_menu()
def create_marker(w):
global id_count
m = viz_msgs.Marker()
m.header.frame_id = w['frame_id']
m.ns = w['name']
m.id = id_count
m.action = viz_msgs.Marker.ADD
m.pose = create_geo_pose(w)
m.scale = geometry_msgs.Vector3(1.0,0.3,0.3)
m.color = std_msgs.ColorRGBA(0.0,1.0,0.0,1.0)
id_count = id_count + 1
return m
def draw_curve(self,
pose_array,
id=None,
rgba=(0, 1, 0, 1),
width=.01,
ns="default_ns",
duration=0,
type=Marker.LINE_STRIP):
if id is None: id = self.get_unused_id()
marker = Marker(type=type, action=Marker.ADD)
marker.header = pose_array.header
marker.points = [pose.position for pose in pose_array.poses]
marker.lifetime = rospy.Duration(0)
if isinstance(rgba, list):
assert len(rgba) == len(pose_array.poses)
marker.colors = [stdm.ColorRGBA(*c) for c in rgba]
else:
marker.color = stdm.ColorRGBA(*rgba)
marker.scale = gm.Vector3(width, width, width)
marker.id = id
marker.ns = ns
self.pub.publish(marker)
self.ids.add(id)
return MarkerHandle(marker, self.pub)
def make_ar_marker(self, scale=.2):
name = 'ar_' + self.tagid
entry = self.tag_database.get(self.tagid)
pose = entry['ar_location']
int_marker = interactive_marker(name, (pose[0], (0,0,0,1)), scale)
if entry['behavior'] != None:
behavior_name = pt.split(entry['behavior'])[1]
int_marker.description = tag_name(self.tagid, behavior_name)
else:
int_marker.description = 'Tag #%d' % get_id_numb(self.tagid)
int_marker.controls += [make_sphere_control(name, int_marker.scale)]
int_marker.controls[0].markers[0].color = stdm.ColorRGBA(0,1,0,.5)
self.server_lock.acquire()
self.marker_server.insert(int_marker, self.marker_cb)
self.server_lock.release()
self.ar_marker = int_marker
def publish_pose_list(pub_ns, _poses, texts=[], stamp=None, size=0.05, frame_id='camera', seq=1):
"""
Publish Pose List on:
pub_channel: Channel on which the cloud will be published
"""
poses = deepcopy(_poses)
if not len(poses): return
arrs = np.vstack([pose.matrix[:3,:3].T.reshape((1,9)) for pose in poses]) * size
arrX = np.vstack([pose.tvec.reshape((1,3)) for pose in poses])
arrx, arry, arrz = arrs[:,0:3], arrs[:,3:6], arrs[:,6:9]
# Point width, and height
N = len(poses)
markers = vis_msg.Marker(type=vis_msg.Marker.LINE_LIST, ns=pub_ns, action=vis_msg.Marker.ADD)
markers.header.frame_id = frame_id
markers.header.stamp = stamp if stamp is not None else rospy.Time.now()
markers.header.seq = seq
markers.scale.x = size/20 # 0.01
markers.scale.y = size/20 # 0.01
markers.color.a = 1.0
markers.pose.position = geom_msg.Point(0,0,0)
markers.pose.orientation = geom_msg.Quaternion(0,0,0,1)
markers.points = []
markers.lifetime = rospy.Duration()
for j in range(N):
markers.points.extend([geom_msg.Point(arrX[j,0], arrX[j,1], arrX[j,2]),
geom_msg.Point(arrX[j,0] + arrx[j,0],
arrX[j,1] + arrx[j,1],
arrX[j,2] + arrx[j,2])])
markers.colors.extend([std_msg.ColorRGBA(1.0, 0.0, 0.0, 1.0), std_msg.ColorRGBA(1.0, 0.0, 0.0, 1.0)])
markers.points.extend([geom_msg.Point(arrX[j,0], arrX[j,1], arrX[j,2]),
geom_msg.Point(arrX[j,0] + arry[j,0],
arrX[j,1] + arry[j,1],
arrX[j,2] + arry[j,2])])
markers.colors.extend([std_msg.ColorRGBA(0.0, 1.0, 0.0, 1.0), std_msg.ColorRGBA(0.0, 1.0, 0.0, 1.0)])
markers.points.extend([geom_msg.Point(arrX[j,0], arrX[j,1], arrX[j,2]),
geom_msg.Point(arrX[j,0] + arrz[j,0],
arrX[j,1] + arrz[j,1],
arrX[j,2] + arrz[j,2])])
markers.colors.extend([std_msg.ColorRGBA(0.0, 0.0, 1.0, 1.0), std_msg.ColorRGBA(0.0, 0.0, 1.0, 1.0)])
_publish_marker(markers)
def set_color(self, color):
if color == 'black':
self.marker.color = SM.ColorRGBA(*BLACK)
elif color == 'blue':
self.marker.color = SM.ColorRGBA(*BLUE)
elif color == 'green':
self.marker.color = SM.ColorRGBA(*GREEN)
elif color == 'red':
self.marker.color = SM.ColorRGBA(*RED)
elif color == 'white':
self.marker.color = SM.ColorRGBA(*WHITE)
else:
rospy.logwarn("String marker color not recognized!")
self.marker.color = SM.ColorRGBA(*BLACK)
def make_kin_tree_from_link(ps, linkname, ns='default_ns', valuedict=None):
markers = []
U = get_pr2_urdf()
link = U.links[linkname]
if link.visual and link.visual.geometry and isinstance(
link.visual.geometry, urdf.Mesh):
rospy.logdebug("%s is a mesh. drawing it.", linkname)
marker = Marker(type=Marker.MESH_RESOURCE, action=Marker.ADD)
marker.ns = ns
marker.header = ps.header
linkFromGeom = conversions.trans_rot_to_hmat(
link.visual.origin.position,
transformations.quaternion_from_euler(
*link.visual.origin.rotation))
origFromLink = conversions.pose_to_hmat(ps.pose)
origFromGeom = np.dot(origFromLink, linkFromGeom)
marker.pose = conversions.hmat_to_pose(origFromGeom)
marker.scale = gm.Vector3(1, 1, 1)
marker.color = stdm.ColorRGBA(1, 1, 0, .5)
marker.id = 0
marker.lifetime = rospy.Duration()
marker.mesh_resource = str(link.visual.geometry.filename)
marker.mesh_use_embedded_materials = False
markers.append(marker)
else:
rospy.logdebug("%s is not a mesh", linkname)
if U.child_map.has_key(linkname):
for (cjoint, clink) in U.child_map[linkname]:
markers.extend(
make_kin_tree_from_joint(ps,
cjoint,
ns=ns,
valuedict=valuedict))
return markers
def draw_traj_points(self,
pose_array,
rgba=(0, 1, 0, 1),
arrow_scale=.05,
ns="default_ns",
duration=0):
marker_array = MarkerArray()
for pose in pose_array.poses:
marker = Marker(type=Marker.ARROW, action=Marker.ADD)
marker.header = pose_array.header
marker.pose = pose
marker.lifetime = rospy.Duration(0)
marker.color = stdm.ColorRGBA(*rgba)
marker.scale = gm.Vector3(arrow_scale, arrow_scale, arrow_scale)
marker.id = self.get_unused_id()
marker.ns = ns
marker_array.markers.append(marker)
self.ids.add(marker.id)
self.array_pub.publish(marker_array)
return MarkerListHandle([
MarkerHandle(marker, self.pub) for marker in marker_array.markers
])
def create_arrow(w):
m = create_marker(w)
m.type = viz_msgs.Marker.ARROW
m.color = std_msgs.ColorRGBA(0.0,1.0,0.0,1.0)
return m
def main():
# Initialization
rospy.init_node('objrec_spoofer')
# Get spoofer parameters
frame_id = rospy.get_param('~frame_id','/world')
publish_period = rospy.get_param('~publish_period',1.0)
publish_rate = rospy.Rate(1.0/publish_period)
# Get the model info
models = rospy.get_param('~models')
stl_uris = {}
for model in models:
stl_uris[model] = rospy.get_param('~stl_uris/'+model)
# Get objects from rosparam
spoofed_objects = rospy.get_param('~spoofed_objects')
# Create the publishers
obj_pub = rospy.Publisher('/recognized_objects',objrec_msgs.RecognizedObjects)
obj_marker_pub = rospy.Publisher('/recognized_objects_markers',visualization_msgs.MarkerArray)
while not rospy.is_shutdown():
# Initialize objects message
objects_msg = objrec_msgs.RecognizedObjects()
objects_msg.header.stamp = rospy.Time.now()
objects_msg.header.frame_id = frame_id
# Initialize marker array message
marker_array = visualization_msgs.MarkerArray()
marker_id = 0
for obj in spoofed_objects:
# Construct and populate an object message
pss_msg = objrec_msgs.PointSetShape()
pss_msg.label = obj['label']
pss_msg.confidence = obj['confidence']
pss_msg.pose = geometry_msgs.Pose(geometry_msgs.Point(*obj['position']), geometry_msgs.Quaternion(*obj['orientation']))
# Store the message
objects_msg.objects.append(pss_msg)
# Construct and populate a marker message
marker = visualization_msgs.Marker()
marker.header = objects_msg.header
marker.ns = "objrec"
marker.type = visualization_msgs.Marker.MESH_RESOURCE
marker.action = visualization_msgs.Marker.ADD
marker.lifetime = rospy.Duration(publish_period)
marker.scale = geometry_msgs.Vector3(0.001, 0.001, 0.001)
marker.color = std_msgs.ColorRGBA(1.0, 0.2, 0.4, 0.5)
marker.id = marker_id
marker.pose = pss_msg.pose
marker.mesh_resource = stl_uris[obj['label']]
# Store the message
marker_array.markers.append(marker)
marker_id += 1
# Publish the object information
obj_pub.publish(objects_msg)
# Publish the markers
obj_marker_pub.publish(marker_array)
publish_rate.sleep()
def numpy_to_colorRGBA(color):
return std_msgs.ColorRGBA(*color)
