def _make_mesh_marker(self):
'''Creates a mesh marker'''
mesh = Marker()
mesh.mesh_use_embedded_materials = False
mesh.type = Marker.MESH_RESOURCE
mesh.scale.x = 1.0
mesh.scale.y = 1.0
mesh.scale.z = 1.0
alpha = 0.6
if self.is_dimmed:
alpha = 0.1
if self.is_fake:
alpha = 0.4
mesh.color = ColorRGBA(0.3, 0.3, 0.3, alpha)
return mesh
if self._is_reachable():
# Original: some kinda orange
# r,g,b = 1.0, 0.5, 0.0
# New: rainbow! See method comment for details.
r,g,b = self.get_marker_color()
mesh.color = ColorRGBA(r, g, b, alpha)
else:
mesh.color = ColorRGBA(0.5, 0.5, 0.5, alpha)
return mesh
def poseStampedToLabeledSphereMarker(cls, psdata, label, fmt="{label} %Y-%m-%d %H:%M:%S", zoffset=0.05):
"[poseStamped, meta] -> sphereMarker"
ps, meta = psdata
res = []
h = Header()
h.stamp = rospy.Time.now()
h.frame_id = ps.header.frame_id
sphere = Marker(type=Marker.SPHERE,
action=Marker.ADD,
header=h,
id=cls.marker_id)
sphere.scale.x = 0.07
sphere.scale.y = 0.07
sphere.scale.z = 0.07
sphere.pose = ps.pose
sphere.color = ColorRGBA(1.0,0,0,1.0)
sphere.ns = "db_play"
sphere.lifetime = rospy.Time(3)
cls.marker_id += 1
res.append(sphere)
text = Marker(type=Marker.TEXT_VIEW_FACING,
action=Marker.ADD,
header=h,
id=cls.marker_id)
text.scale.z = 0.1
text.pose = deepcopy(ps.pose)
text.pose.position.z += zoffset
text.color = ColorRGBA(1.0,1.0,1.0,1.0)
text.text = meta["inserted_at"].strftime(fmt).format(label=label)
text.ns = "db_play_text"
text.lifetime = rospy.Time(300)
cls.marker_id += 1
res.append(text)
return res
def pubViz(self, ast, bst):
rate = rospy.Rate(10)
for i in range(self.winSize):
msgs = MarkerArray()
#use1について
msg = Marker()
#markerのプロパティ
msg.header.frame_id = 'camera_link'
msg.header.stamp = rospy.Time.now()
msg.ns = 'j1'
msg.action = 0
msg.id = 1
msg.type = 8
msg.scale.x = 0.1
msg.scale.y = 0.1
msg.color = self.carray[2]
#ジョイントポイントを入れる処理
for j1 in range(self.jointSize):
point = Point()
point.x = self.pdata[0][ast+i][j1][0]
point.y = self.pdata[0][ast+i][j1][1]
point.z = self.pdata[0][ast+i][j1][2]
msg.points.append(point)
msg.pose.orientation.w = 1.0
msgs.markers.append(msg)
msg = Marker()
msg.header.frame_id = 'camera_link'
msg.header.stamp = rospy.Time.now()
msg.ns = 'j2'
msg.action = 0
msg.id = 2
msg.type = 8
msg.scale.x = 0.1
msg.scale.y = 0.1
msg.color = self.carray[1]
for j2 in range(self.jointSize):
point = Point()
point.x = self.pdata[1][bst+i][j2][0]
point.y = self.pdata[1][bst+i][j2][1]
point.z = self.pdata[1][bst+i][j2][2]
msg.points.append(point)
msg.pose.orientation.w = 1.0
msgs.markers.append(msg)
self.mpub.publish(msgs)
rate.sleep()
def _make_mesh_marker(self):
'''Creates a mesh marker'''
mesh = Marker()
mesh.mesh_use_embedded_materials = False
mesh.type = Marker.MESH_RESOURCE
mesh.scale.x = 1.0
mesh.scale.y = 1.0
mesh.scale.z = 1.0
if self._is_reachable():
mesh.color = ColorRGBA(1.0, 0.5, 0.0, 0.6)
else:
mesh.color = ColorRGBA(0.5, 0.5, 0.5, 0.6)
return mesh
def _make_mesh_marker(self):
mesh = Marker()
mesh.mesh_use_embedded_materials = False
mesh.type = Marker.MESH_RESOURCE
mesh.scale.x = 1.0
mesh.scale.y = 1.0
mesh.scale.z = 1.0
mesh.color = ColorRGBA(0.0, 0.5, 1.0, 0.6)
ik_solution = self.ik.get_ik_for_ee(self.ee_pose)
if (ik_solution == None):
mesh.color = ColorRGBA(1.0, 0.0, 0.0, 0.6)
else:
mesh.color = ColorRGBA(0.0, 1.0, 0.0, 0.6)
return mesh
def pubRviz(self, pos, joints):
msgs = MarkerArray()
for p in range(len(pos)):
msg = Marker()
msg.header.frame_id = 'camera_link'
msg.header.stamp = rospy.Time.now()
msg.ns = 'marker'
msg.action = 0
msg.id = p
msg.type = 4
msg.scale.x = 0.1
msg.scale.y = 0.1
msg.color = self.carray[2]
for i in range(len(pos[p])):
point = Point()
point.x = pos[p][i][0]
point.y = pos[p][i][1]
point.z = pos[p][i][2]
msg.points.append(point)
msg.pose.orientation.w = 1.0
msgs.markers.append(msg)
for j in range(len(joints)):
msg = Marker()
msg.header.frame_id = 'camera_link'
msg.header.stamp = rospy.Time.now()
msg.ns = 'joints'
msg.action = 0
msg.id = j
msg.type = 8
msg.scale.x = 0.1
msg.scale.y = 0.1
msg.color = self.carray[j]
#print "joints len:"+str(len(joints[j]))
for i in range(len(joints[j])):
point = Point()
point.x = joints[j][i][0]
point.y = joints[j][i][1]
point.z = joints[j][i][2]
msg.points.append(point)
msg.pose.orientation.w = 1.0
msgs.markers.append(msg)
self.mpub.publish(msgs)
def _make_mesh_marker(self):
mesh = Marker()
mesh.mesh_use_embedded_materials = False
mesh.type = Marker.MESH_RESOURCE
mesh.scale.x = 1.0
mesh.scale.y = 1.0
mesh.scale.z = 1.0
target_pose = GripperMarkers._offset_pose(self.ee_pose)
ik_sol = self._ik_service.get_ik_for_ee(target_pose)
if ik_sol == None:
mesh.color = ColorRGBA(1.0, 0.0, 0.0, 0.6)
else:
mesh.color = ColorRGBA(0.0, 1.0, 0.0, 0.6)
return mesh
def _pub_sight_areas_cylinder(self, orders, target=True):
"""
Publish sight areas as cylinders
"""
color = ColorRGBA()
if target:
ns = 'sight.target.cyl'
color = COL_TARGET
else:
ns = 'sight.current.cyl'
color = COL_CURRENT
x_coords, y_coords = extract_coords(orders, target=target)
viz = Marker()
viz.header.stamp = rospy.Time()
viz.header.frame_id = '/map'
viz.action = viz.MODIFY
viz.ns = ns
viz.type = viz.CYLINDER
viz.color = color
viz.scale.x = 2*orders.sight_radius
viz.scale.y = 2*orders.sight_radius
viz.scale.z = CYL_HEIGHT
viz.lifetime = MARKERS_LIFETIME
for i_drone in range(orders.num_drones):
viz.id = i_drone
viz.pose.position.x = x_coords[i_drone]
viz.pose.position.y = y_coords[i_drone]
viz.pose.position.z = CYL_HEIGHT/2.
self.orders_viz_pub.publish(viz)
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 to_marker_array(self):
markers = []
for x in range(0, len(self.field)):
for y in range(0, len(self.field[0])):
marker = Marker()
marker.header.stamp = rospy.get_rostime()
marker.header.frame_id = '/odom_combined'
marker.ns = 'suturo_planning/search_grid'
marker.id = x + (y * len(self.field))
marker.action = Marker.ADD
marker.type = Marker.CUBE
marker.pose.position.x = self.coordinates[x, y][0]
marker.pose.position.y = self.coordinates[x, y][1]
marker.pose.position.z = self.coordinates[x, y][2]
marker.pose.orientation.w = 1
marker.scale.x = 2.0 / len(self.coordinates)
marker.scale.y = marker.scale.x
marker.scale.z = 0.01
marker.color = SearchGrid._get_color_for_value(self.field[x][y])
marker.lifetime = rospy.Time(0)
markers.append(marker)
return MarkerArray(markers)
def marker_from_point_radius(point,
radius,
index=0,
linewidth=0.1,
color=ColorRGBA(1, 0, 0, 1),
z=0.,
lifetime=10.0):
marker = Marker()
marker.header = make_header("/map")
marker.ns = "Speed_NS"
marker.id = index
marker.type = Marker.CYLINDER
marker.action = 0 # action=0 add/modify object
# marker.color.r = 1.0
# marker.color.g = 0.0
# marker.color.b = 0.0
# marker.color.a = 0.4
marker.color = color
marker.lifetime = rospy.Duration.from_sec(lifetime)
marker.pose = Pose()
marker.pose.position.z = z
marker.pose.position.x = point[0]
marker.pose.position.y = point[1]
marker.scale = Vector3(radius * 2.0, radius * 2.0, 0.001)
return marker
def draw_wall(self, points, c=(1.0, 0.0, 0.0, 1.0)):
'''
draws marker line in rviz based on list of points
:param points: list of points
:param c: RGBA color tuple; defaults to red
'''
marker = Marker()
marker.id = 0
marker.header.frame_id = 'base_link'
marker.type = Marker.LINE_STRIP
marker.lifetime = rospy.Duration(60)
marker.color = ColorRGBA(*c)
marker.scale.x = 0.1
marker.scale.y = 0.1
marker.scale.z = 0.1
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
marker.points = points
self.marker_pub.publish(marker)
def as_marker_msg(self, ns=u'', id=1):
"""
:param ns:
:param id:
:rtype: Marker
"""
if len(self.get_link_names()) > 1:
raise TypeError(
u'only urdfs objects with a single link can be turned into marker'
)
link = self.get_urdf_link(self.get_link_names()[0])
m = Marker()
m.ns = u'{}/{}'.format(ns, self.get_name())
m.id = id
if link.visual:
geometry = link.visual.geometry
else:
geometry = link.visuals[0].geometry
if isinstance(geometry, up.Box):
m.type = Marker.CUBE
m.scale = Vector3(*geometry.size)
elif isinstance(geometry, up.Sphere):
m.type = Marker.SPHERE
m.scale = Vector3(geometry.radius * 2, geometry.radius * 2,
geometry.radius * 2)
elif isinstance(geometry, up.Cylinder):
m.type = Marker.CYLINDER
m.scale = Vector3(geometry.radius * 2, geometry.radius * 2,
geometry.length)
else:
raise Exception(
u'world body type {} can\'t be converted to marker'.format(
geometry.__class__.__name__))
m.color = ColorRGBA(0, 1, 0, 0.5)
return m
def create_point_list(cls,
ref_pose,
points,
colors,
size=0.01,
namespace='point_list'):
"""
Creates a point list referenced to a pose
Args:
ref_pose (PoseStamped): This is used for the header and the ref_points
points (list): List of Point objects with the positions relative to ref_pose
colors (list): The colors for the points
size (float): size of the points in meters
"""
marker = Marker()
marker.type = Marker.POINTS
marker.header = ref_pose.header
marker.pose = ref_pose.pose
marker.ns = namespace
marker.action = Marker.ADD
marker.scale.x = size
marker.scale.y = size
if isinstance(colors[0], list):
# colors defined for every element
marker.colors = [cls.rgba_from_list(color) for color in colors]
else:
marker.color = cls.rgba_from_list(colors)
marker.points = points
return marker
def create_point_markers(cls,
poses,
size=0.01,
color=None,
namespace='point_marker'):
"""
create point markers from a list of poses
Params:
poses ([PoseStamped]): list of poses
size (float): size of the markers in meter
Returns:
[Marker]
"""
color = cls.rgba_from_list(color)
markers = list()
for pose in poses:
marker = Marker()
marker.ns = namespace
marker.type = Marker.SPHERE
marker.header = pose.header
marker.pose = pose.pose
marker.scale.x = size
marker.scale.y = size
marker.scale.z = size
marker.color = color
marker.action = Marker.ADD
markers.append(marker)
return markers
def createSphereMarker(color, scale, offset=(0,0,0), orientation=(0,0,0,1), frame_id = "/map", marker_id = 1):
marker = Marker()
marker.header.frame_id = frame_id
marker.type = marker.SPHERE
marker.scale.x = scale[0]
marker.scale.y = scale[1]
marker.scale.z = scale[2]
marker.id = marker_id
p = ColorRGBA()
p.r = color[0]
p.g = color[1]
p.b = color[2]
p.a = color[3]
marker.color = p
marker.pose.orientation.x = orientation[0]
marker.pose.orientation.y = orientation[1]
marker.pose.orientation.z = orientation[2]
marker.pose.orientation.w = orientation[3]
marker.pose.position.x = offset[0]
marker.pose.position.y = offset[1]
marker.pose.position.z = offset[2]
return marker
def get_particle_marker(self, timestamp, particle, marker_id):
"""Returns an rviz marker that visualizes a single particle"""
msg = Marker()
msg.header.stamp = timestamp
msg.header.frame_id = 'map'
msg.ns = 'particles'
msg.id = marker_id
msg.type = 0 # arrow
msg.action = 0 # add/modify
msg.lifetime = rospy.Duration(1)
yaw_in_map = particle.theta
vx = cos(yaw_in_map)
vy = sin(yaw_in_map)
msg.color = ColorRGBA(0, 1.0, 0, 1.0)
msg.points.append(Point(particle.x, particle.y, 0.2))
msg.points.append(
Point(particle.x + 0.3 * vx, particle.y + 0.3 * vy, 0.2))
msg.scale.x = 0.05
msg.scale.y = 0.15
msg.scale.z = 0.1
return msg
def makeViz(self, reachSets):
# if self.skipfreq == self.skipcount:
# self.skipcount = 0
# self.skipcount += 1
pointSets = [[tuple(p.p) for p in rs.set] for rs in reachSets.sets]
#triangleSets = [tripy.earclip(ps) for ps in pointSets]
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = reachSets.header.frame_id
origin = Pose(Point(0, 0, 0), Quaternion(0, 0, 0, 1))
lineMarkers = MarkerArray()
lineMarkerArray = []
for ii in range(len(pointSets)):
m = Marker()
m.header = header
m.id = self.outline_marker_id + ii
m.action = 0
m.pose = origin
m.type = 4 #LINE_STRIP
m.color = self.colors[ii % len(self.colors)]
m.points = [Point(p[0], p[1], 0) for p in pointSets[ii]]
m.scale = Vector3(.05, 0, 0)
lineMarkerArray.append(m)
lineMarkers.markers = lineMarkerArray
self.outlinePub.publish(lineMarkers)
rospy.logdebug("Published Visualization")
def render_axes(self,track,marker_array): for i in range( len( track.pose ) ): for axis in range(3): marker = Marker() marker.header.stamp = track.header.stamp marker.header.frame_id = track.header.frame_id marker.ns = "track_visualizer-%d"%(track.id) marker.id = self.num_markers[track.id] marker.action = Marker.ADD marker.scale = Vector3(0.003,0.003,0.003) marker.color = self.generate_color_axis(track.id, i,axis) marker.type = Marker.LINE_STRIP marker.pose = track.pose[i] marker.points.append( Point(0,0,0) ) if axis==0: marker.points.append( Point(0.05,0,0) ) elif axis==1: marker.points.append( Point(0,0.05,0) ) elif axis==2: marker.points.append( Point(0,0,0.05) ) marker_array.markers.append(marker) self.num_markers[track.id] += 1
def create_cylinder(cx,cy,cz,ax,ay,az,radius, frame_id='/', obj_id=0, length=1, color=(0,1,0,0.3)):
#The cylinder's axis if unrotated is originally pointing along the z axis of the referential
#so that will be the up direction (0, 0 1).
#
#The cross product of the desired cylinder axis (normalized) and the up vector will give us
#the axis of rotation, perpendicular to the plane defined by the cylinder axis and the up vector.
#
#The dot product of the cylinder axis and the up vector will provide the angle of rotation.
axis = (ax, ay, az)
rotation_axis = np.cross((0, 0, 1), axis)
rotation_angle = simple_geom.angle_between_vectors(axis, (0, 0, 1))
cyl = Marker()
cyl.id = obj_id
cyl.header.frame_id = frame_id
cyl.type = Marker.CYLINDER
cyl.action = Marker.ADD
cyl.pose.position = Point(x=cx,y=cy,z=cz)
cyl.pose.orientation = Quaternion(*tf.transformations.quaternion_about_axis(rotation_angle, axis))
cyl.scale.x = 2*radius
cyl.scale.y = 2*radius
cyl.scale.z = length
cyl.color = ColorRGBA(*color)
return cyl
def publishSignalStrength():
with mutex:
for mac in beaconLists:
l = beaconLists[mac]
#publish markers
points = Marker()
points.header.frame_id = "/map";
points.header.stamp = rospy.Time.now()
points.ns = "points_and_lines";
points.action = Marker.ADD;
points.pose.orientation.w = 1.0;
points.id = 0;
points.type = Marker.POINTS;
# POINTS markers use x and y scale for width/height respectively
points.scale.x = 0.2;
points.scale.y = 0.2;
# Points color
points.color = getColorForMac(mac)
# Create the vertices for the points and lines
for b in l:
p = Point();
p.x = b.x
p.y = b.y
p.z = b.strength
points.points.append(p)
# write to topic
rospy.Publisher("signal"+str(mac), Marker, queue_size=100) .publish(points)
def make_sphere(x, y, radius, color, mark_id, off_x, off_y):
""" Function to generate a spherical marker
ARGUMENTS:
x, y: coordinates of point in grid space
radius: float
color: ColorRGBA
mark_id: unique id
off_x, off_y: Origin of map coordinates in world frame """
global resolution
marker = Marker()
marker.header.stamp = rospy.Time.now()
marker.header.frame_id = 'map'
marker.ns = 'frontier'
marker.id = mark_id
marker.type = 2
marker.action = 0
marker.pose = geometry_msgs.msg.Pose()
marker.pose.position.x = resolution * x + off_x
marker.pose.position.y = resolution * y + off_y
marker.scale = geometry_msgs.msg.Vector3(radius, radius, radius)
marker.color = color
marker.lifetime = rospy.rostime.Duration(0)
marker.frame_locked = True
return marker
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 scan_callback(msg):
global pose, pub, seq, last_marker
if pose is not None:
angle = msg.angle * (np.pi / 200.0) # Convert grad to rad
angle += np.pi # Transform to baselink frame
ori = pose.orientation
_, _, robot_yaw = tf.transformations.euler_from_quaternion(
[ori.x, ori.y, ori.z, ori.w])
quat = tf.transformations.quaternion_from_euler(
0, 0, angle + robot_yaw)
# Delete last marker
m = last_marker
m.action = m.DELETE
m.pose.position = pose.position
m.pose.orientation = Quaternion(quat[0], quat[1], quat[2], quat[3])
m.scale = Vector3(1, 0.1, 1)
m.color = ColorRGBA(2, 0, 0, 1)
pub.publish(m)
last_marker = m
# Publish new marker
m = Marker()
m.header.frame_id = "map"
m.id = seq
m.type = m.ARROW
m.ns = ""
m.pose.position = pose.position
m.pose.orientation = Quaternion(quat[0], quat[1], quat[2], quat[3])
m.scale = Vector3(1, 0.1, 1)
m.color = ColorRGBA(2, 0, 0, 1)
pub.publish(m)
last_marker = m
def _pub_order(self, orders, target=True):
color = ColorRGBA()
if target:
color = COL_TARGET
ns = "orders.target"
else:
color = COL_CURRENT
ns = "orders.current"
viz = Marker()
viz.header.stamp = rospy.Time()
viz.header.seq = self.seq
viz.header.frame_id = '/map'
viz.action = viz.MODIFY
viz.ns = ns
viz.id = 0
viz.type = viz.POINTS
viz.color = color
viz.scale = self.scale
viz.lifetime = MARKERS_LIFETIME
x_coords, y_coords = extract_coords(orders, target=target)
for i_drone in range(orders.num_drones):
loc = Point()
loc.x = x_coords[i_drone]
loc.y = y_coords[i_drone]
loc.z = 0
viz.points.append(loc)
rospy.loginfo("pub {} points".format(len(viz.points)))
self.orders_viz_pub.publish(viz)
def _draw_sphere_list(self,
uid,
namespace,
scale,
points,
color=None,
colors=None):
# type: (int, str, Vector3, list, ColorRGBA, list) -> None
marker = Marker()
# Header
marker.header.frame_id = "base_link"
marker.header.stamp = rospy.Time()
# Body
marker.ns = namespace
marker.id = uid
marker.type = Marker.SPHERE_LIST
marker.action = Marker.ADD
marker.scale = scale
marker.pose.orientation = Quaternion(0, 0, 0, 1)
marker.points = points
if color is not None:
marker.color = color
if colors is not None:
marker.colors = colors
self._visualization_publisher.publish(marker)
def show_mean_left_in_rviz(self, opacity):
marker = Marker()
marker.header.frame_id = "map"
marker.header.stamp = rospy.Time.now()
marker.ns = "mean_left"
# marker.id = self.count
marker.id = 3
marker.type = Marker.LINE_STRIP
marker.action = Marker.ADD
for i in range(100):
point = Point() # moved in to for loop
point.x = self.all_means[2][i][0]
point.y = self.all_means[2][i][1]
point.z = 0
marker.points.append(point)
marker.pose.orientation = Quaternion(0, 0, 0, 1)
marker.scale = Vector3(self.all_std[2][i][0] * 2,
self.all_std[2][i][1] * 2, 0.1)
marker.color = ColorRGBA(0.0, 1.0, 0.0, opacity)
self.count += 1
# marker.lifetime = rospy.Duration(0.25)
self.marker_publisher_mean.publish(marker)
def publish(anns, data):
table_list = TableList()
marker_list = MarkerArray()
marker_id = 1
for a, d in zip(anns, data):
# Tables
object = pickle.loads(d.data)
table_list.tables.append(object)
# Markers
marker = Marker()
marker.id = marker_id
marker.header = a.pose.header
marker.type = a.shape
marker.ns = "table_obstacles"
marker.action = Marker.ADD
marker.lifetime = rospy.Duration.from_sec(0)
marker.pose = copy.deepcopy(a.pose.pose.pose)
marker.scale = a.size
marker.color = a.color
marker_list.markers.append(marker)
marker_id = marker_id + 1
marker_pub = rospy.Publisher('table_marker', MarkerArray, latch=True, queue_size=1)
table_pub = rospy.Publisher('table_pose_list', TableList, latch=True, queue_size=1)
table_pub.publish(table_list)
marker_pub.publish(marker_list)
return
def MarkerTimerCB(self, event):
markerArray = MarkerArray()
ad_msg = AmmoDetect()
ammobox_detected = 0
for ab in self.ammobox_list:
if ab.vote >= 2: ad_msg.ammo_detect.append(ab.id)
marker = Marker()
marker.header.frame_id = "map"
marker.type = marker.CUBE
marker.action = marker.ADD
marker.scale = Vector3(0.2, 0.2, 0.2)
marker.color = ColorRGBA(
1, 0, 0, 1) if ab.vote == 1 else ColorRGBA(1, 1, 1, 0.5)
marker.pose.orientation.w = 1.0
marker.pose.position.x = ab.x
marker.pose.position.y = ab.y
marker.pose.position.z = ab.z
markerArray.markers.append(marker)
if ab.vote == 1: ammobox_detected += 1
# renumber the markers
id = 0
for m in markerArray.markers:
m.id = id
id += 1
self.pub_markers.publish(markerArray)
self.pub_ammo_detect.publish(ad_msg)
print ad_msg
def createTriangleListMarker(marker_id,
points,
rgba,
frame_id="/camera_rgb_optical_frame"):
marker = Marker()
marker.header.frame_id = frame_id
marker.type = marker.TRIANGLE_LIST
marker.scale = Vector3(1, 1, 1)
marker.id = marker_id
n = len(points)
if rgba is not None:
marker.color = ColorRGBA(*rgba)
o = Point(0, 0, 0)
for i in xrange(n):
p = Point(*points[i])
marker.points.append(p)
p = Point(*points[(i + 1) % 4])
marker.points.append(p)
marker.points.append(o)
marker.pose = poselist2pose([0, 0, 0, 0, 0, 0, 1])
return marker
def pub_obj(self, obj_type, pos, dim):
m = Marker()
m.id = self.obj_types.index(obj_type)
m.ns = "simple_tabletop"
m.type = Marker.CUBE
m.pose = Pose(Point(*pos), Quaternion(0, 0, 0, 1))
m.header = Header(0, rospy.Time(0), "base_link")
m.scale = Vector3(*dim)
m.color = self.colors[obj_type]
mtext = deepcopy(m)
mtext.type = Marker.TEXT_VIEW_FACING
mtext.id += 100
mtext.text = obj_type
M = MarkerArray([m, mtext])
self.pub.publish(M)
#moveit collision objects
co = CollisionObject()
co.header = deepcopy(m.header)
co.id = obj_type
obj_shape = SolidPrimitive()
obj_shape.type = SolidPrimitive.BOX
obj_shape.dimensions = list(dim)
co.primitives.append(obj_shape)
obj_pose = Pose(Point(*pos), Quaternion(0, 0, 0, 1))
co.primitive_poses = [obj_pose]
self.pub_co.publish(co)
def delete_all_markers(box_, color, index):
marker = Marker()
marker.id = index
marker.ns = ns
marker.header.frame_id = frame_id
marker.type = marker.LINE_STRIP
marker.action = 3 # marker.DELETEALL: deletes all objects
# marker.frame_locked=False
# marker scale
marker.scale = Vector3(0.04, 0.04, 0.04) # x,yz
# marker color
marker.color = ColorRGBA(color[0], color[1], color[2],
color[3]) # r,g,b,a
# marker orientaiton
marker.pose.orientation = Quaternion(0., 0., 0., 1.) # x,y,z,w
# marker position
marker.pose.position = Point(0., 0., 0.) # x,y,z
# marker.lifetime = rospy.Duration(0.1)
p0, p1, p2, p3, p4, p5, p6, p7 = box3d_2conner(box_, 0)
# marker line points
marker.points = []
for p in [
p0, p1, p2, p3, p0, p4, p5, p6, p7, p4, p5, p1, p2, p6, p7, p3
]:
marker.points.append(Point(
p[0],
p[1],
p[2],
))
return marker
def publish_marker_vector(start,
end,
diameter_shaft=0.01,
diameter_head=0.02,
id_=0):
"""
assumes points to be in frame map
:type start: Point
:type end: Point
:type diameter_shaft: float
:type diameter_head: float
:type id_: int
"""
m = Marker()
m.action = m.ADD
m.ns = u'debug'
m.id = id_
m.type = m.ARROW
m.points.append(start)
m.points.append(end)
m.color = ColorRGBA(1, 0, 0, 1)
m.scale.x = diameter_shaft
m.scale.y = diameter_head
m.scale.z = 0
pub = rospy.Publisher('/visualization_marker', Marker, queue_size=1)
while pub.get_num_connections() < 1:
# wait for a connection to publisher
# you can do whatever you like here or simply do nothing
pass
rospy.sleep(0.3)
pub.publish(m)
def visualize(self):
lineVis = Marker() # Marker to visualize used lidar pts
lineVis.header.frame_id = "odom" # Publishes it to the laser link, idk if it should be changed
lineVis.header.stamp = rospy.Time.now()
endVis = deepcopy(lineVis)
startVis = deepcopy(lineVis)
lineVis.points.append(
Point(self.wp_1.point.x, self.wp_1.point.y, self.wp_1.point.z))
startVis.points.append(
Point(self.wp_1.point.x, self.wp_1.point.y, self.wp_1.point.z))
lineVis.points.append(
Point(self.wp_2.point.x, self.wp_2.point.y, self.wp_2.point.z))
endVis.points.append(
Point(self.wp_2.point.x, self.wp_2.point.y, self.wp_2.point.z))
lineVis.color = ColorRGBA(1, 1, 1, 1) # Color is blue
startVis.color = ColorRGBA(1, 0, 0, 1) # Color is blue
endVis.color = ColorRGBA(0, 1, 0, 1) # Color is blue
lineVis.type = 4 # makes it a list of points
startVis.type = 8
endVis.type = 8
endVis.scale.x = 0.3
endVis.scale.y = 0.3
startVis.scale.x = 0.3
startVis.scale.y = 0.3
lineVis.scale.x = 0.05 # scale is about 2 cm
lineVis.scale.y = 0.05
self.line_pub.publish(lineVis)
self.end_pub.publish(endVis)
self.start_pub.publish(startVis)
def create_interactive_mesh(name,
resource,
color=(1, 0, 0, 1),
frame_id='base_link',
transform=None,
scale=1):
if transform is None:
transform = np.eye(4)
# Start with the interactive control
int_marker = create_interactive_6dof(name, color, frame_id, transform)
# Mesh control
control = InteractiveMarkerControl()
control.always_visible = True
control.interaction_mode = InteractiveMarkerControl.NONE
# Mesh marker
marker = Marker()
marker.type = Marker.MESH_RESOURCE
marker.scale = Vector3(*[scale for _ in range(3)])
marker.color = ColorRGBA(*color)
marker.action = Marker.ADD
marker.mesh_resource = resource
# Add mesh control
control.markers.append(marker)
int_marker.controls.append(control)
return int_marker
def marker_from_circle(circle,
index=0,
linewidth=0.1,
color=ColorRGBA(1, 0, 0, 1),
z=0.,
lifetime=10.0):
marker = Marker()
marker.header = make_header("/map")
marker.ns = "Markers_NS"
marker.id = index
marker.type = Marker.CYLINDER
marker.action = 0 # action=0 add/modify object
# marker.color.r = 1.0
# marker.color.g = 0.0
# marker.color.b = 0.0
# marker.color.a = 0.4
marker.color = color
marker.lifetime = rospy.Duration.from_sec(lifetime)
marker.pose = Pose()
marker.pose.position.z = z
marker.pose.position.x = circle.center[0]
marker.pose.position.y = circle.center[1]
marker.scale = Vector3(circle.radius * 2.0, circle.radius * 2.0, 0)
return marker
def addBoxes(self, in_objects, out_markers):
for obj in in_objects.targets:
box = Marker()
# Message headers
box.header = in_objects.header
box.type = Marker.CUBE
box.action = Marker.MODIFY if obj.uid in self._tracker_set else Marker.ADD
box.ns = self._params.marker_namespace + "/boxs"
box.id = obj.uid
# Marker properties
box.color = self._params.box_color
box.scale.x = obj.bbox.dimension.length_x
box.scale.y = obj.bbox.dimension.length_y
box.scale.z = obj.bbox.dimension.length_z
box.pose.position = obj.bbox.pose.pose.position
box.pose.orientation = obj.bbox.pose.pose.orientation
# Skip large boxes to prevent messy visualization
if box.scale.x > self._params.box_max_size or box.scale.y > self._params.box_max_size or box.scale.z > self._params.box_max_size:
rospy.logdebug("Object skipped with size %.2f x %.2f x %.2f!",
box.scale.x, box.scale.y, box.scale.z)
continue
box_str = str(box)
if 'nan' in box_str or 'inf' in box_str:
print("----------------------------------")
print(box_str)
out_markers.markers.append(box)
def draw_speed_control_spline(self):
HSV_min = 0
HSV_max = 240 / 360.0
marker = Marker()
marker.header.stamp = rospy.get_rostime()
marker.header.frame_id = "map"
marker.id = self.MARKER_SPEED_SPLINE
marker.type = Marker.LINE_STRIP
marker.action = Marker.ADD
marker.pose.orientation.x = 0.0
marker.pose.orientation.y = 0.0
marker.pose.orientation.z = 0.0
marker.pose.orientation.w = 1.0
color = self.max_lookahead_curv / \
(self.max_curv - self.min_curv) * (HSV_max - HSV_min)
# Invert so the largest curvature maps to smallest HSV
color = HSV_max - HSV_min - color
r, g, b = colorsys.hsv_to_rgb(color, 1, 1)
marker.color = ColorRGBA(r, g, b, 1)
marker.scale.x = 0.5
index = self.traj_closest_index
# Loop from the closest trajectory point to the speed lookahead point.
while (index != self.speed_lookahead_index):
x, y = self.traj_coords[index]
marker.points.append(Point(x, y, 0))
index = (index + 1) % len(self.traj_coords)
self.pub_visualiser.publish(marker)
def viz_cars(self, obj_list, header, color_rgba, publisher):
'''
Publish objects to the provied rviz publisher.
Parameters
----------
obj_list : [Objects]
The list of Objects that should be visualized in rviz.
header : Header
The Header object containing the current ROS time and frame id.
color_rgba : ColorRGBA
The color in which the objects should be visualized.
publisher : rospy.Publisher
The ROS Publisher in which to publish the messages.
'''
marker_msg = MarkerArray()
count = 0
for obj in obj_list:
mk = Marker()
mk.header = header
mk.type = Marker.CUBE
mk.scale.x = 3.7
mk.scale.y = 1.6
mk.scale.z = 0.5
mk.color = color_rgba
mk.pose = obj.pose
mk.id = count
mk.lifetime = rospy.Duration(1)
marker_msg.markers.append(mk)
count += 1
publisher.publish(marker_msg)
def add_marker(self, mesh_frame, marker_name, mesh_file, use_materials=False, color=None):
marker_msg = Marker()
marker_msg.frame_locked = True
marker_msg.id = 0
marker_msg.action = Marker.ADD
marker_msg.mesh_use_embedded_materials = use_materials
if marker_msg.mesh_use_embedded_materials:
marker_msg.color.r = 0
marker_msg.color.g = 0
marker_msg.color.b = 0
marker_msg.color.a = 0
elif color:
marker_msg.color = color
else:
marker_msg.color.r = 0.6
marker_msg.color.g = 0.6
marker_msg.color.b = 0.6
marker_msg.color.a = 1.0
marker_msg.header.stamp = rospy.get_rostime()
#marker_msg.lifetime =
#marker_msg.pose =
marker_msg.type = Marker.MESH_RESOURCE
marker_msg.header.frame_id = mesh_frame
marker_msg.ns = marker_name
marker_msg.mesh_resource = 'file://%s' % (os.path.abspath(mesh_file))
scale = 1.0
marker_msg.scale.x, marker_msg.scale.y, marker_msg.scale.z = scale, scale, scale
marker_msg.pose.position.x, marker_msg.pose.position.y, marker_msg.pose.position.z = 0, 0, 0
marker_msg.pose.orientation.x, marker_msg.pose.orientation.y, marker_msg.pose.orientation.z, marker_msg.pose.orientation.w = 0, 0, 0, 1
self.pub_marker_sync(marker_msg)
if not self.marker_thread:
self.marker_thread = thread.start_new_thread(MarkerPublisher.publish_markers, (self,))
def handle_image_msg(msg):
assert msg._type == 'sensor_msgs/Image'
with g_fusion_lock:
g_fusion.filter(EmptyObservation(msg.header.stamp.to_sec()))
if g_fusion.last_state_mean is not None:
pose = g_fusion.lidar_observation_function(
g_fusion.last_state_mean)
yaw_q = ros_tf.transformations.quaternion_from_euler(0, 0, pose[3])
br = ros_tf.TransformBroadcaster()
br.sendTransform(tuple(pose[:3]), tuple(yaw_q), rospy.Time.now(),
'obj_fuse_centroid', 'velodyne')
if last_known_box_size is not None:
# bounding box
marker = Marker()
marker.header.frame_id = "obj_fuse_centroid"
marker.header.stamp = rospy.Time.now()
marker.type = Marker.CUBE
marker.action = Marker.ADD
marker.scale.x = last_known_box_size[2]
marker.scale.y = last_known_box_size[1]
marker.scale.z = last_known_box_size[0]
marker.color = ColorRGBA(r=0., g=1., b=0., a=0.5)
marker.lifetime = rospy.Duration()
pub = rospy.Publisher("obj_fuse_bbox", Marker, queue_size=10)
pub.publish(marker)
def visualize_poop(x,y,z,color,frame,ns):
msg = Marker()
msg.header = Header(stamp=Time.now(), frame_id=frame)
#msg.scale = Vector3(x=0.02, y=0.02, z=0.02) # for sphere
msg.scale = Vector3(x=0.005, y=0.04, z=0.0) # for arrow
#msg.pose.position = Point(x=x, y=y, z=z)
#msg.pose.position = Point(x=x, y=y, z=z+0.15) # arrow
#msg.pose.orientation = Quaternion(x=0, y=0, z=0, w=1) # arrow
#msg.pose.orientation = Quaternion(x=0.707, y=0, z=0, w=0.707)
msg.points = [Point(x=x, y=y,z=z+0.15), Point(x=x,y=y,z=z)]
msg.ns = ns
msg.id = 0
msg.action = 0 # add
#msg.type = 2 # sphere
msg.type = 0 # arrow
msg.color = ColorRGBA(r=0, g=0, b=0, a=1)
if color == 0:
msg.color.g = 1;
elif color == 1:
msg.color.b = 1;
elif color == 2:
msg.color.r = 1;
msg.color.g = 1;
elif color == 3:
msg.color.g = 1;
msg.color.b = 1;
#loginfo("Publishing %s marker at %0.3f %0.3f %0.3f",ns,x,y,z)
viz_pub.publish(msg)
def __update_mesh(self, mesh):
"""
Refreshes mesh in rviz.
:param mesh: Mesh object representing the space.
"""
m = Marker()
m.header.frame_id = str(1)
m.type = Marker.LINE_LIST
m.action = Marker.ADD
m.id = 1
m.color = RED
m.scale.x = 0.001
edges_visited = set()
for n1 in mesh.nodes:
for n2 in n1.edges:
if n1 < n2 and (n1, n2) not in edges_visited:
m.points.append(n1.position())
m.points.append(n2.position())
edges_visited.add((n1, n2))
elif n2 < n1 and (n2, n1) not in edges_visited:
m.points.append(n2.position())
m.points.append(n1.position())
edges_visited.add((n2, n1))
self.__mesh_publisher.publish(m)
def line(self, frame_id, p, r, t=[0.0, 0.0], key=None):
"""
line: t r + p
This will be drawn for t[0] .. t[1]
"""
r = np.array(r)
p = np.array(p)
m = Marker()
m.header.frame_id = frame_id
m.ns = key or 'lines'
m.id = 0 if key else len(self.lines)
m.type = Marker.LINE_STRIP
m.action = Marker.MODIFY
m.pose = Pose(Point(0,0,0), Quaternion(0,0,0,1))
m.scale = Vector3(0.005, 0.005, 0.005)
m.color = ColorRGBA(0,0.8,0.8,1)
m.points = [Point(*(p+r*t)) for t in np.linspace(t[0], t[1], 10)]
m.colors = [m.color] * 10
key = key or element_key(m)
with self.mod_lock:
self.lines[key] = m
return key
def draw_marker(
self,
ps,
id=None,
type=Marker.CUBE,
ns="default_ns",
rgba=(0, 1, 0, 1),
scale=(0.03, 0.03, 0.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 create_person_label_marker(self, person, color):
h = Header()
h.frame_id = person.header.frame_id #tie marker visualization to laser it comes from
h.stamp = rospy.Time.now() # Note you need to call rospy.init_node() before this will work
#create marker:person_marker, modify a red cylinder, last indefinitely
mark = Marker()
mark.header = h
mark.ns = "person_label_marker"
## simple hack to map persons name to integer value for unique marker id
char_list = list(person.name)
char_int_list = [str(ord(x)) for x in char_list]
char_int_str = "".join(char_int_list)
char_int = int(char_int_str) & 255
print
print "str to int"
print char_int_list
print char_int
print "Char int binary) ", bin(char_int)
# mark.id = int(char_int / 10000)
mark.id = int(float(person.name)) #char_int
mark.type = Marker.TEXT_VIEW_FACING
mark.action = 0
mark.scale = Vector3(self.scale_factor * 0.5, self.scale_factor * 0.5, 1)
mark.color = color
mark.lifetime = rospy.Duration(0.5,0)
print "person name: ", person.name
mark.text = person.name
pose = Pose(Point(person.pose.position.x + 0.75, person.pose.position.y + 0.5, self.person_height + 0.75), Quaternion(0.0,0.0,1.0,cos(person.pose.position.z/2)))
mark.pose = pose
return mark
def create_cylinder(cx,cy,cz,ax,ay,az,radius, frame_id='/', obj_id=0, length=1, color=(0,1,0,0.3)):
#The cylinder's axis if unrotated is originally pointing along the z axis of the referential
#so that will be the up direction (0, 0 1).
#
#The cross product of the desired cylinder axis (normalized) and the up vector will give us
#the axis of rotation, perpendicular to the plane defined by the cylinder axis and the up vector.
#
#The dot product of the cylinder axis and the up vector will provide the angle of rotation.
axis = (ax, ay, az)
rotation_axis = np.cross((0, 0, 1), axis)
rotation_angle = simple_geom.angle_between_vectors(axis, (0, 0, 1))
cyl = Marker()
cyl.id = obj_id
cyl.header.frame_id = frame_id
cyl.type = Marker.CYLINDER
cyl.action = Marker.ADD
cyl.pose.position = Point(x=cx,y=cy,z=cz)
cyl.pose.orientation = Quaternion(*tf.transformations.quaternion_about_axis(rotation_angle, axis))
cyl.scale.x = 2*radius
cyl.scale.y = 2*radius
cyl.scale.z = length
cyl.color = ColorRGBA(*color)
return cyl
def publish(anns, data):
wall_list = WallList()
marker_list = MarkerArray()
marker_id = 1
for a, d in zip(anns, data):
# Walls
object = deserialize_msg(d.data, Wall)
wall_list.obstacles.append(object)
# Markers
marker = Marker()
marker.id = marker_id
marker.header = a.pose.header
marker.type = a.shape
marker.ns = "wall_obstacles"
marker.action = Marker.ADD
marker.lifetime = rospy.Duration.from_sec(0)
marker.pose = copy.deepcopy(a.pose.pose.pose)
marker.scale = a.size
marker.color = a.color
marker_list.markers.append(marker)
marker_id = marker_id + 1
marker_pub = rospy.Publisher('wall_marker', MarkerArray, latch=True, queue_size=1)
wall_pub = rospy.Publisher('wall_pose_list', WallList, latch=True, queue_size=1)
wall_pub.publish(wall_list)
marker_pub.publish(marker_list)
return
def input_handler(space):
global marker_color
global marker_pub
global matches
for str in matches:
if space.aux_payload.find(str) == -1:
# print "FAIL: str='%s' aux_payload='%s'" % (str, space.aux_payload)
return
# print space
# print
marker = Marker()
marker.header.frame_id = "/map"
marker.header.stamp = rospy.Time()
marker.ns = ""
marker.id = 0
marker.type = Marker.POINTS
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 = 0
marker.scale.x = .1
marker.scale.y = .1
marker.scale.z = .1
marker.color = marker_color
marker.points = []
# this line cuts off anything before the string "convex_set: " in the aux_payload
convex_set = space.aux_payload[ space.aux_payload.find("convex_set: ") :]
# this line cuts off that string at the next newline character
convex_set = convex_set.split('\n')[0]
# this line splits the string into pieces delineated by spaces
convex_set = convex_set.split(' ')
# print convex_set
for candidate in convex_set:
if len(candidate) == 0 or candidate[0] != '(':
continue
coords = candidate[1:-1].split(',')
# print coords
marker.points.append(Point(float(coords[0]), float(coords[1]), float(coords[2])))
# print marker.points[-1]
# print
# print marker.points
# print
marker_pub.publish(marker)
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 publish(anns, data):
ar_mk_list = AlvarMarkers()
marker_list = MarkerArray()
marker_id = 1
for a, d in zip(anns, data):
# AR markers
object = deserialize_msg(d.data, AlvarMarker)
ar_mk_list.markers.append(object)
# Visual markers
marker = Marker()
marker.id = marker_id
marker.header = a.pose.header
marker.type = a.shape
marker.ns = "ar_mk_obstacles"
marker.action = Marker.ADD
marker.lifetime = rospy.Duration.from_sec(0)
marker.pose = copy.deepcopy(a.pose.pose.pose)
marker.scale = a.size
marker.color = a.color
marker_list.markers.append(marker)
marker_id = marker_id + 1
marker_pub = rospy.Publisher('ar_mk_marker', MarkerArray, latch=True, queue_size=1)
ar_mk_pub = rospy.Publisher('ar_mk_pose_list', AlvarMarkers,latch=True, queue_size=1)
ar_mk_pub.publish(ar_mk_list)
marker_pub.publish(marker_list)
return
def createSphereMarker(color, scale, offset=(0,0,0), orientation=(0,0,0,1)):
marker = Marker()
marker.header.frame_id = "/map"
marker.type = marker.SPHERE
marker.scale.x = scale[0]
marker.scale.y = scale[1]
marker.scale.z = scale[2]
marker.id = 1
p = ColorRGBA()
p.r = color[0]
p.g = color[1]
p.b = color[2]
p.a = color[3]
marker.color = p
marker.pose.orientation.x = orientation[0]
marker.pose.orientation.y = orientation[1]
marker.pose.orientation.z = orientation[2]
marker.pose.orientation.w = orientation[3]
marker.pose.position.x = offset[0]
marker.pose.position.y = offset[1]
marker.pose.position.z = offset[2]
return marker
def run(self):
while self.is_looping():
navigation_tf_msg = TFMessage()
try:
motion_to_robot = almath.Pose2D(self.motion.getRobotPosition(True))
localization = self.navigation.getRobotPositionInMap()
exploration_path = self.navigation.getExplorationPath()
except Exception as e:
navigation_tf_msg.transforms.append(self.get_navigation_tf(almath.Pose2D()))
self.publishers["map_tf"].publish(navigation_tf_msg)
self.rate.sleep()
continue
if len(localization) > 0 and len(localization[0]) == 3:
navigation_to_robot = almath.Pose2D(localization[0][0], localization[0][1], localization[0][2])
navigation_to_motion = navigation_to_robot * almath.pinv(motion_to_robot)
navigation_tf_msg.transforms.append(self.get_navigation_tf(navigation_to_motion))
self.publishers["map_tf"].publish(navigation_tf_msg)
if len(localization) > 0:
if self.publishers["uncertainty"].get_num_connections() > 0:
uncertainty = Marker()
uncertainty.header.frame_id = "/base_footprint"
uncertainty.header.stamp = rospy.Time.now()
uncertainty.type = Marker.CYLINDER
uncertainty.scale = Vector3(localization[1][0], localization[1][1], 0.2)
uncertainty.pose.position = Point(0, 0, 0)
uncertainty.pose.orientation = self.get_ros_quaternion(
almath.Quaternion_fromAngleAndAxisRotation(0, 0, 0, 1))
uncertainty.color = ColorRGBA(1, 0.5, 0.5, 0.5)
self.publishers["uncertainty"].publish(uncertainty)
if self.publishers["map"].get_num_connections() > 0:
aggregated_map = None
try:
aggregated_map = self.navigation.getMetricalMap()
except Exception as e:
print("error " + str(e))
if aggregated_map is not None:
map_marker = OccupancyGrid()
map_marker.header.stamp = rospy.Time.now()
map_marker.header.frame_id = "/map"
map_marker.info.resolution = aggregated_map[0]
map_marker.info.width = aggregated_map[1]
map_marker.info.height = aggregated_map[2]
map_marker.info.origin.orientation = self.get_ros_quaternion(
almath.Quaternion_fromAngleAndAxisRotation(-1.57, 0, 0, 1))
map_marker.info.origin.position.x = aggregated_map[3][0]
map_marker.info.origin.position.y = aggregated_map[3][1]
map_marker.data = aggregated_map[4]
self.publishers["map"].publish(map_marker)
if self.publishers["exploration_path"].get_num_connections() > 0:
path = Path()
path.header.stamp = rospy.Time.now()
path.header.frame_id = "/map"
for node in exploration_path:
current_node = PoseStamped()
current_node.pose.position.x = node[0]
current_node.pose.position.y = node[1]
path.poses.append(current_node)
self.publishers["exploration_path"].publish(path)
self.rate.sleep()
def _make_mesh_marker(self):
mesh = Marker()
mesh.mesh_use_embedded_materials = False
mesh.type = Marker.MESH_RESOURCE
mesh.scale.x = 1.0
mesh.scale.y = 1.0
mesh.scale.z = 1.0
target_pose = GripperMarkers._offset_pose(self.marker_pose, GripperMarkers._offset)
print target_pose
ik_solution = self.ik.get_ik_for_ee(target_pose)
if (ik_solution == None):
mesh.color = ColorRGBA(1.0, 0.0, 0.0, 0.6)
else:
mesh.color = ColorRGBA(0.0, 1.0, 0.0, 0.6)
return mesh
def rviz_obj(self, obj_id, obj_ns, obj_type, obj_size, obj_color=0, obj_life=0):
obj = Marker()
obj.header.frame_id, obj.header.stamp = "camera_link", rospy.Time.now()
obj.ns, obj.action, obj.type = str(obj_ns), 0, obj_type
obj.scale.x, obj.scale.y, obj.scale.z = obj_size[0], obj_size[1], obj_size[2]
obj.color = self.carray[obj_color]
obj.lifetime = rospy.Duration.from_sec(obj_life)
obj.pose.orientation.w = 1.0
return obj
def create_base_marker(pose, id, color):
marker = Marker()
marker.header.frame_id = "base_link"
marker.header.stamp = rospy.Time.now()
marker.ns = "ar_servo"
marker.id = id
marker.pose = PoseConverter.to_pose_msg(pose)
marker.color = ColorRGBA(*(color + (1.0,)))
marker.scale.x = 0.7; marker.scale.y = 0.7; marker.scale.z = 0.2
return marker
def createMarker(self): marker = Marker() marker.header = Header(stamp=rospy.Time.now(), frame_id="odom") marker.id = 1 marker.type = Marker.SPHERE marker.action = Marker.ADD marker.pose.orientation = Quaternion(x=0, y=0, z=0, w=1) marker.scale = Vector3(x=.1, y=.1, z=.1) marker.color = ColorRGBA(a=1, r=0, g=1, b=0) return marker
def transformStampedArrayToLabeledArrayMarker(cls, tsdata_lst, fmt="%Y-%m-%d %H:%M:%S", zoffset=0.05, label_downsample=1, discrete=False):
"[[transformStamped, meta],...] -> LineStrip / String"
h = Header()
h.stamp = rospy.Time(0) #rospy.Time.now()
h.frame_id = "eng2" #t_first.child_frame_id
res = []
t_first = tsdata_lst[0][0]
prev_t = t_first.transform.translation
for ts, _ in tsdata_lst:
m = Marker(type=Marker.ARROW,
action=Marker.ADD,
header=h,
id=cls.marker_id)
cls.marker_id += 1
t = ts.transform.translation
dist = distanceOfVector3(prev_t, t) * 0.65
rgb = colorsys.hsv_to_rgb(dist, 1.0, 1.0)
m.points = [Point(x=prev_t.x,y=prev_t.y,z=prev_t.z),
Point(x=(prev_t.x + t.x) / 2.,y=(prev_t.y + t.y) /2.,z=(prev_t.z + t.z) /2.)]
m.ns = "labeled_line"
m.lifetime = rospy.Time(3000)
m.scale.x, m.scale.y, m.scale.z = 0.02, 0.06, 0.1
m.color = ColorRGBA(rgb[0],rgb[1],rgb[2],1.0)
if dist < 0.65:
res.append(m)
prev_t = t
for t, t_meta in tsdata_lst[::label_downsample]:
text = Marker(type=Marker.TEXT_VIEW_FACING,
action=Marker.ADD,
header=h,
id=cls.marker_id)
cls.marker_id += 1
text.scale.z = 0.1
text.pose = T.poseFromTransform(t.transform)
text.pose.position.z += zoffset
text.color = ColorRGBA(0.0,0.0,1.0,1.0)
text.text = t_meta["inserted_at"].strftime(fmt)
text.ns = "labeled_line_text"
text.lifetime = rospy.Time(3000)
res.append(text)
return res
def populate_marker(self):
marker = Marker()
marker.header.frame_id = "world"
marker.header.stamp = rospy.Time.now()
marker.id = 0
marker.ns = self.name
marker.type = self.shape
marker.action = Marker.ADD
marker.color = self.color
marker.lifetime = rospy.Duration();
return marker
