def make_marker(self, barcode_msg):
ean = barcode_msg.barcode
m = Marker()
m.header = barcode_msg.barcode_pose.header
m.pose = barcode_msg.barcode_pose.pose
m.action = Marker.ADD
m.type = Marker.CUBE
m.scale = Vector3(.06, .06, .06)
m.color = ColorRGBA(1, 0, 0, 1.0)
text = Marker()
text.header = barcode_msg.barcode_pose.header
text.action = Marker.ADD
text.type = Marker.TEXT_VIEW_FACING
text.text = ean
text.scale = Vector3(0, 0, .06)
text.color = ColorRGBA(1, 0, 0, 1)
text.pose.position.x = barcode_msg.barcode_pose.pose.position.x
text.pose.position.y = barcode_msg.barcode_pose.pose.position.y
text.pose.position.z = barcode_msg.barcode_pose.pose.position.z + 0.05
m.text = ean
m.ns = ean
text.ns = ean
m.id = 2
self.marker_pub.publish(m)
self.marker_pub.publish(text)
def update_goal(self, setpoint_msg):
array_msg = MarkerArray()
array_msg.markers = []
marker_msg = Marker()
marker_msg.header = setpoint_msg.header
marker_msg.ns = "guidance/current"
marker_msg.id = 0
marker_msg.type = 2
marker_msg.action = 0
marker_msg.pose = setpoint_msg.pose
marker_msg.scale = Vector3(2 * self.distance_error_acceptance,
2 * self.distance_error_acceptance,
2 * self.distance_error_acceptance)
marker_msg.color = ColorRGBA(0.77432878, 0.31884126, 0.54658502,
1.0) #HOT PINK
array_msg.markers.append(marker_msg)
marker_msg = Marker()
marker_msg.header = setpoint_msg.header
marker_msg.ns = "guidance/current"
marker_msg.id = 1
marker_msg.type = 0
marker_msg.action = 0
marker_msg.pose = setpoint_msg.pose
marker_msg.scale = Vector3(20, 2, 2)
marker_msg.color = ColorRGBA(0.77432878, 0.31884126, 0.54658502,
1.0) #HOT PINK
array_msg.markers.append(marker_msg)
self.marker_pub.publish(array_msg)
def makeViz(self, reachSets):
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)
triPoints = [xy for tri in triangleSets for xy in tri]
triMarker = Marker()
triMarker.header = header
triMarker.id = self.tri_marker_id
triMarker.type = 11 #TRIANGLE_LIST
triMarker.action = 0
triMarker.pose = origin
triMarker.color = ColorRGBA(1, 1, 1, 1)
triMarker.scale = Vector3(1, 1, 1)
triMarker.points = [
Point(p[0], p[1], 0) for tri in triPoints for p in tri
]
#expand color array to cover all verts for all tris in each set with same color
triFrequency = [len(ps) for ps in pointSets]
triColors = [
self.colors[ii % len(self.colors)] for ii in range(len(pointSets))
]
triMarker.colors = [
c for cidx in range(len(triColors))
for c in repeat(triColors[cidx], triFrequency[cidx])
]
self.trisPub.publish(triMarker)
def viz_waypoints():
markers = MarkerArray()
path_marker = Marker()
path_marker.header = waypoints[0].header
path_marker.ns = 'path'
path_marker.id = len(markers.markers)
path_marker.type = Marker.LINE_STRIP
path_marker.action = path_marker.ADD
path_marker.points = [p.pose.position for p in waypoints]
path_marker.color.b = 1.
path_marker.color.a = 1.
path_marker.scale.x = .05
markers.markers.append(path_marker)
wp_marker = Marker()
wp_marker.header = waypoints[0].header
wp_marker.ns = 'waypoints'
wp_marker.id = len(markers.markers)
wp_marker.type = Marker.SPHERE_LIST
wp_marker.action = Marker.ADD
wp_marker.points = [p.pose.position for p in waypoints]
wp_marker.color.r = 1.
wp_marker.color.a = 1.
wp_marker.scale.x = .25
wp_marker.scale.y = .25
wp_marker.scale.z = .25
markers.markers.append(wp_marker)
dir_marker_del = Marker()
dir_marker_del.ns = 'direction'
dir_marker_del.action = Marker.DELETEALL
markers.markers.append(dir_marker_del)
for wp in waypoints:
dir_marker = Marker()
dir_marker.ns = 'direction'
dir_marker.header = wp.header
dir_marker.action = Marker.ARROW
dir_marker.action = Marker.ADD
dir_marker.ns = 'direction'
dir_marker.id = len(markers.markers)
dir_marker.pose = wp.pose
dir_marker.color.r = 1.
dir_marker.color.g = 1.
dir_marker.color.b = 0.
dir_marker.color.a = 0.5
dir_marker.scale.x = .5
dir_marker.scale.y = .2
dir_marker.scale.z = .05
markers.markers.append(dir_marker)
viz_pub.publish(markers)
def class_visualization(data):
center_point = Marker()
center_point.header = data.header
center_point.ns = "segment_center"
center_point.action = center_point.MODIFY
center_point.pose.orientation.w = 1.0
center_point.id = 0
center_point.type = center_point.SPHERE_LIST
center_point.scale.x = center_point.scale.y = 0.1
center_point.scale.z = 0.1
center_point.color.a = 0.5
center_point.color.r = 1.0
center_point.color.g = 1.0
center_point.color.b = 1.0
class_text_array = MarkerArray()
class_text = Marker()
class_text.header = data.header
class_text.action = class_text.DELETEALL
class_text_array.markers.append(class_text)
text_marker_pub.publish(class_text_array)
for index, item in enumerate(data.segments):
class_text = Marker()
class_text.header = data.header
class_text.ns = "class_text"
class_text.action = class_text.ADD
class_text.pose.orientation.w = 1.0
class_text.id = index
class_text.type = class_text.TEXT_VIEW_FACING
class_text.scale.z = 0.05
class_text.color.a = 1.0
class_text.color.r = 1.0
class_text.color.g = 1.0
class_text.color.b = 1.0
class_text.text = str(round(item.class_id, 2))
class_text.pose.position.x = (item.center.x)
class_text.pose.position.y = (item.center.y)
class_text.pose.position.z = (item.center.z)
class_text_array.markers.append(class_text)
if (item.class_id > 0.7):
center_point.points.append(item.center)
marker_pub.publish(center_point)
text_marker_pub.publish(class_text_array)
def sensed_objects_callback(data, ns):
marker_array = MarkerArray()
for id, sensed in enumerate(data.objects):
type_marker = Marker()
type_marker.header = sensed.header
type_marker.ns = ns
type_marker.lifetime = rospy.Duration(1.0)
type_marker.id = 10 * id
type_marker.type = Marker.TEXT_VIEW_FACING
type_marker.action = Marker.ADD
type_marker.pose = copy.deepcopy(sensed.pose.pose)
type_marker.pose.position.z += 0.15 # A little bit up
type_marker.scale.z = 0.1
type_marker.color.r = 1.0
type_marker.color.g = 1.0
type_marker.color.b = 1.0
type_marker.color.a = 1.0
type_marker.text = get_type(sensed.type) # TODO: Add id?
marker_array.markers.append(type_marker)
pose_marker = Marker()
pose_marker.header = sensed.header
pose_marker.ns = ns
pose_marker.lifetime = rospy.Duration(1.0)
pose_marker.id = 10 * id + 1
pose_marker.type = Marker.SPHERE
pose_marker.action = Marker.ADD
pose_marker.pose = sensed.pose.pose
pose_marker.scale.x = sensed.pose.covariance[0]
pose_marker.scale.y = sensed.pose.covariance[7]
pose_marker.scale.z = sensed.pose.covariance[14]
pose_marker.color.r = 1.0
pose_marker.color.g = 1.0
pose_marker.color.b = 1.0
pose_marker.color.a = 1.0
marker_array.markers.append(pose_marker)
scale_marker = Marker()
scale_marker.header = sensed.header
scale_marker.ns = ns
scale_marker.lifetime = rospy.Duration(1.0)
scale_marker.id = 10 * id + 2
scale_marker.type = Marker.CUBE
scale_marker.action = Marker.ADD
scale_marker.pose = sensed.pose.pose
scale_marker.scale = sensed.scale
scale_marker.color = get_color(sensed.color, 0.5)
marker_array.markers.append(scale_marker)
marker_array_pub.publish(marker_array)
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 on_enter(self, userdata):
ma = MarkerArray()
self._path = MoveBaseActionPath()
for i in range(len(userdata.path.poses)):
marker = Marker(type=Marker.ARROW)
marker.header = userdata.path.header
marker.pose = userdata.path.poses[i].pose
marker.scale.x = 0.2
marker.scale.y = 0.02
marker.scale.z = 0.02
marker.color.b = 1.0
marker.color.r = 0.9 - 0.7 * i / len(userdata.path.poses)
marker.color.g = 0.9 - 0.7 * i / len(userdata.path.poses)
marker.color.a = 0.8 - 0.5 * i / len(userdata.path.poses)
marker.id = i
ma.markers.append(marker)
self._failed = False
self._path.goal.target_path.poses = userdata.path.poses
self._path.goal.target_path.header.frame_id = "map"
self._pub.publish(self._pathTopic, self._path)
self._pub.publish(self._marker_topic, ma)
self._reached = True
def makeMarker(self, id_num, point, rgb=(1.0,1.0,1.0)):
## Make a std_msgs/Marker object
#
# @param id_num The id number corresponding to the marker. Note that these must be unique.
# @param point An (x,y,z) tuple where the marker should be located in space
# @param rgb A tuple corresponding to the RGB values on a scale from 0.0 to 1.0
#
# @return A std_msgs/Marker object
marker = Marker()
marker.header = Header()
marker.header.stamp = rospy.Time.now()
marker.header.frame_id = self.frame_id
marker.id = id_num
marker.type = marker.SPHERE
marker.action = marker.ADD
marker.scale.x = 0.02
marker.scale.y = 0.02
marker.scale.z = 0.02
marker.color.r = rgb[0]
marker.color.g = rgb[1]
marker.color.b = rgb[2]
marker.color.a = 1.0
marker.pose.orientation.w = 1.0
marker.pose.position.x = point[0]
marker.pose.position.y = point[1]
marker.pose.position.z = point[2]
return marker
def publish(self, grasps, obj=None):
msg = MarkerArray()
self.marker_id = 0 # reset marker counter
if obj and len(obj.primitives) > 0 and len(obj.primitive_poses) > 0:
m = Marker()
m.header = obj.header
m.ns = "object"
m.id = self.marker_id
self.marker_id += 1
m.type = m.CUBE
m.action = m.ADD
m.pose = obj.primitive_poses[0]
m.scale.x = obj.primitives[0].dimensions[0]
m.scale.y = obj.primitives[0].dimensions[1]
m.scale.z = obj.primitives[0].dimensions[2]
m.color.r = 0
m.color.g = 0
m.color.b = 1
m.color.a = 0.8
msg.markers.append(m)
for grasp in grasps:
msg.markers.append(self.get_gripper_marker(grasp.grasp_pose, grasp.grasp_quality))
self.pub.publish(msg)
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 create_line_marker(p1, p2, frame_id):
h = Header()
h.frame_id = 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 = "unit_vector"
mark.id = 0
mark.type = Marker.LINE_STRIP
mark.action = 0
mark.scale.x = 0.2
mark.color = color = ColorRGBA(0.2, 0.5, 0.7, 1.0)
mark.text = "unit_vector"
points = []
pt1 = Point(p1[0], p1[1], p1[2])
pt2 = Point(p2[0], p2[1], p2[2])
# print "Pt 1 ", pt1
# print "Pt 2 ", pt2
points.append(pt1)
points.append(pt2)
mark.points = points
return mark
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 gpscallback(msg):
marker = Marker()
marker.header = msg.header
marker.header.frame_id = "map"
marker.ns = 'gps'
marker.id = 0
marker.type = Marker.SPHERE_LIST
marker.action = Marker.MODIFY
marker.scale.x = 0.3
marker.scale.y = 0.3
marker.scale.z = 0.3
marker.color.r = 0
marker.color.g = 0
marker.color.b = 1
marker.color.a = 1
marker.lifetime = rospy.Duration(2000)
marker.frame_locked = False
point = Point()
point.x = msg.pose.pose.position.x
point.y = msg.pose.pose.position.y
point.z = msg.pose.pose.position.z
gpspoints.append(point)
marker.points.extend(gpspoints)
vis_pub.publish(marker)
def linelist(self):
"""Build line list marker from 8 points in 3d space."""
line_list = Marker()
line_list.header = self._header
line_list.type = Marker.LINE_LIST
line_list.action = Marker.ADD
line_list.scale.x = 0.005
line_list.color = self.YELLOW
line_list.pose = deepcopy(self.POSE)
line_list.pose.position.x = self._p1.x
line_list.pose.position.y = self._p1.y
line_list.pose.position.z = self._p1.z
line_list.points.extend((self._p1, self._p2))
line_list.points.extend((self._p2, self._p3))
line_list.points.extend((self._p3, self._p4))
line_list.points.extend((self._p4, self._p1))
line_list.points.extend((self._p5, self._p6))
line_list.points.extend((self._p6, self._p7))
line_list.points.extend((self._p7, self._p8))
line_list.points.extend((self._p8, self._p5))
line_list.points.extend((self._p1, self._p5))
line_list.points.extend((self._p2, self._p6))
line_list.points.extend((self._p3, self._p7))
line_list.points.extend((self._p4, self._p8))
return line_list
def pose_cb(self, pose):
pose = self.listener.transformPose(self.reference_frame,pose)
q = pose.pose.orientation
qvec = [q.x,q.y,q.z,q.w]
euler = euler_from_quaternion(qvec)
self.goal_x = pose.pose.position.x
self.goal_y = pose.pose.position.y
self.goal_theta = euler[2]
(ex,ey,etheta) = self.compute_error()
if ex < -self.dist_threshold:
self.goal_theta = norm_angle(etheta + pi)
print "New goal: %.2f %.2f %.2f" % (self.goal_x, self.goal_y, self.goal_theta)
marker = Marker()
marker.header = pose.header
marker.ns = "goal_marker"
marker.id = 10
marker.type = Marker.ARROW
marker.action = Marker.ADD
marker.pose = pose.pose
marker.scale.x = 0.5
marker.scale.y = 0.5
marker.scale.z = 1.0;
marker.color.a = 1.0;
marker.color.r = 1.0;
marker.color.g = 1.0;
marker.color.b = 0.0;
marker.lifetime.secs=-1.0
self.marker_pub.publish(marker)
def callback(msg):
marker = Marker()
marker.header = msg.header
marker.header.frame_id = "map"
marker.ns = 'obstacles'
marker.id = 0
marker.type = Marker.SPHERE_LIST
marker.action = Marker.MODIFY
marker.scale.x = 0.1
marker.scale.y = 0.1
marker.scale.z = 0.1
marker.color.r = 0
marker.color.g = 1
marker.color.b = 0
marker.color.a = 1
marker.lifetime = rospy.Duration(2)
marker.frame_locked = False
for obstacle in msg.obstacles:
point = Point()
point.x = obstacle.odom.pose.pose.position.x
point.y = obstacle.odom.pose.pose.position.y
point.z = 0
marker.points.append(point)
vis_pub.publish(marker)
def make_circle_marker(point,
scale,
color,
frame_id,
namespace,
sid,
duration=0):
marker = Marker()
marker.header = make_header(frame_id)
marker.ns = namespace
marker.id = sid
marker.type = 2 # sphere
marker.lifetime = rospy.Duration.from_sec(duration)
marker.action = 0
marker.pose.position.x = point[0]
marker.pose.position.y = point[1]
marker.pose.orientation.w = 1.0
marker.scale.x = scale
marker.scale.y = scale
marker.scale.z = scale
marker.color.r = float(color[0])
marker.color.g = float(color[1])
marker.color.b = float(color[2])
marker.color.a = 1.0
return marker
def create_marker(t, type, lifetime):
global counter
marker = Marker()
marker.header = t.header
marker.type = type
marker.action = marker.ADD
id = t.id
if id == 0:
id = counter
counter += 1
marker.id = int(id)
marker.lifetime = lifetime
marker.ns = 'obstacle_tracking.%d' % type
marker.color.a = 1.
marker.color.r = marker.color.g = marker.color.b = 0.5
marker.pose = copy.deepcopy(t.pose.pose)
marker.scale.x = marker.scale.y = marker.scale.z = 1. # default scale
return marker
def addBoxes(self, in_objects, out_markers):
for obj in in_objects.detections:
box = Marker()
box.lifetime = rospy.Duration(self._params.marker_lifetime)
# Message headers
box.header = in_objects.header
box.type = Marker.CUBE
box.action = Marker.ADD
box.ns = self._params.marker_namespace + "/boxs"
box.id = self._marker_id
self._marker_id += 1
# 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
out_markers.markers.append(box)
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 callback(self, state):
self._id = 0
m = self.create_arrow(state.base_pose)
m.color.r = 1.0
self._out_pub.publish(m)
m = self.create_arrow(state.bridge_pose)
m.color.g = 1.0
m.scale.x = Constants.BRIDGE_TO_STRIKE_MIN
self._out_pub.publish(m)
m = Marker()
m.header = state.bridge_pose.header
m.ns = "billiards_shot_plan"
m.id = self._id
m.action = Marker.ADD
m.type = Marker.CUBE
m.pose = state.bridge_pose.pose
m.pose.position.z += 0.055
m.scale.x = 0.005
m.scale.y = 0.05
m.scale.z = 0.11
m.color.a = 1.0
m.color.b = 1.0
self._out_pub.publish(m)
self._id += 1
def addLabels(self, in_objects, out_markers):
for obj in in_objects.detections:
label_marker = Marker()
label_marker.lifetime = rospy.Duration(
self._params.marker_lifetime)
# Message headers
label_marker.header = in_objects.header
label_marker.action = Marker.ADD
label_marker.type = Marker.TEXT_VIEW_FACING
label_marker.ns = self._params.marker_namespace + "/labels"
label_marker.id = self._marker_id
self._marker_id += 1
# Marker properties
label_marker.scale.x = self._params.label_scale
label_marker.scale.y = self._params.label_scale
label_marker.scale.z = self._params.label_scale
label_marker.color = self._params.label_color
if len(obj.classes) > 0: # Use classification name if presented
label_marker.text = "[" + str(obj.classes[0].classid) + "]"
x = obj.bbox.pose.pose.position.x
y = obj.bbox.pose.pose.position.y
label_marker.text += "%.2f m" % math.sqrt(x * x + y * y)
if obj.comments:
label_marker.text += "\n" + obj.comments
label_marker.pose.position.x = x
label_marker.pose.position.y = y
label_marker.pose.position.z = self._params.label_height
out_markers.markers.append(label_marker)
def pose_cb(self, pose):
pose = self.listener.transformPose(self.reference_frame, pose)
q = pose.pose.orientation
qvec = [q.x, q.y, q.z, q.w]
euler = euler_from_quaternion(qvec)
self.goal_x = pose.pose.position.x
self.goal_y = pose.pose.position.y
self.goal_theta = euler[2]
(ex, ey, etheta) = self.compute_error()
if ex < -self.dist_threshold:
self.goal_theta = norm_angle(etheta + pi)
print "New goal: %.2f %.2f %.2f" % (self.goal_x, self.goal_y,
self.goal_theta)
marker = Marker()
marker.header = pose.header
marker.ns = "goal_marker"
marker.id = 10
marker.type = Marker.ARROW
marker.action = Marker.ADD
marker.pose = pose.pose
marker.scale.x = 0.5
marker.scale.y = 0.5
marker.scale.z = 1.0
marker.color.a = 1.0
marker.color.r = 1.0
marker.color.g = 1.0
marker.color.b = 0.0
marker.lifetime.secs = -1.0
self.marker_pub.publish(marker)
def makeMarker(self, header, id_num, point):
## Make a std_msgs/Marker object
#
# @param header A ROS Header object for the marker
# @param id_num The id number corresponding to the marker. Note that these must be unique.
# @param point An (x,y,z) tuple where the marker should be located in space
#
# @return A std_msgs/Marker object
marker = Marker()
marker.header = header
marker.id = id_num
marker.type = marker.SPHERE
marker.action = marker.ADD
marker.scale.x = 0.03
marker.scale.y = 0.03
marker.scale.z = 0.03
marker.color.r = 1.0
marker.color.g = 0.2
marker.color.b = 0.3
marker.color.a = 1.0
marker.pose.orientation.w = 1.0
marker.pose.position.x = point[0]
marker.pose.position.y = point[1]
marker.pose.position.z = point[2]
return marker
def pose_cb(self, pose):
m_pose = PointStamped()
m_pose.header = pose.header
m_pose.point = pose.pose.position
m_pose = self.listener.transformPoint(self.reference_frame,m_pose)
self.goal_x = m_pose.point.x
self.goal_y = m_pose.point.y
print "New goal: %.2f %.2f" % (self.goal_x, self.goal_y)
marker = Marker()
marker.header = pose.header
marker.ns = "goal_marker"
marker.id = 10
marker.type = Marker.CYLINDER
marker.action = Marker.ADD
marker.pose = pose.pose
marker.scale.x = 0.1
marker.scale.y = 0.1
marker.scale.z = 0.5;
marker.color.a = 1.0;
marker.color.r = 1.0;
marker.color.g = 1.0;
marker.color.b = 0.0;
marker.lifetime.secs=-1.0
self.marker_pub.publish(marker)
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 make_marker_stub(self, stamped_pose, size=None, color=None):
if color is None:
color = (0.5, 0.5, 0.5, 1.0)
if size is None:
size = (1, 1, 1)
marker = Marker()
marker.header = stamped_pose.header
# marker.ns = "collision_scene"
marker.id = self.next_id
marker.lifetime = Time(0) # forever
marker.action = Marker.ADD
marker.pose = stamped_pose.pose
marker.color.r = color[0]
marker.color.g = color[1]
marker.color.b = color[2]
if len(color) == 4:
alpha = color[3]
else:
alpha = 1.0
marker.color.a = alpha
marker.scale.x = size[0]
marker.scale.y = size[1]
marker.scale.z = size[2]
self.next_id += 1
return 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 pose_cb(self, pose):
m_pose = PointStamped()
m_pose.header = pose.header
m_pose.point = pose.pose.position
m_pose = self.listener.transformPoint(self.reference_frame, m_pose)
self.goal_x = m_pose.point.x
self.goal_y = m_pose.point.y
print "New goal: %.2f %.2f" % (self.goal_x, self.goal_y)
marker = Marker()
marker.header = pose.header
marker.ns = "goal_marker"
marker.id = 10
marker.type = Marker.CYLINDER
marker.action = Marker.ADD
marker.pose = pose.pose
marker.scale.x = 0.1
marker.scale.y = 0.1
marker.scale.z = 0.5
marker.color.a = 1.0
marker.color.r = 1.0
marker.color.g = 1.0
marker.color.b = 0.0
marker.lifetime.secs = -1.0
self.marker_pub.publish(marker)
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_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 goal_parts_cb(self, msg: PoseWithCertaintyArray):
arr = []
i = 0
for post in msg.poses:
post_marker = Marker()
pose = Pose()
pose.position = post.pose.pose.position
post_marker.pose = pose
post_marker.pose.position.z = self.post_height / 2
post_marker.pose.orientation = Quaternion()
post_marker.pose.orientation.x = 0
post_marker.pose.orientation.y = 0
post_marker.pose.orientation.z = 0
post_marker.pose.orientation.w = 1
post_marker.type = Marker.CYLINDER
post_marker.action = Marker.MODIFY
post_marker.id = i
post_marker.ns = "rel_goal_parts"
color = ColorRGBA()
color.r = 1.0
color.g = 1.0
color.b = 1.0
color.a = 1.0
post_marker.color = color
post_marker.lifetime = rospy.Duration(nsecs=self.goal_lifetime)
scale = Vector3(self.post_diameter, self.post_diameter,
self.post_height)
post_marker.scale = scale
post_marker.header = msg.header
arr.append(post_marker)
i += 1
self.goal_parts_marker.markers = arr
self.marker_array_publisher.publish(self.goal_parts_marker)
def publish_trajectory(self, duration=0.0):
should_publish = len(self.points) > 1
if self.visualize and self.traj_pub.get_num_connections() > 0:
print "Publishing trajectory"
marker = Marker()
marker.header = self.make_header("/map")
marker.ns = self.viz_namespace + "/trajectory"
marker.id = 2
marker.type = marker.LINE_STRIP # line strip
marker.lifetime = rospy.Duration.from_sec(duration)
if should_publish:
marker.action = marker.ADD
marker.scale.x = 0.3
marker.color.r = 1.0
marker.color.g = 1.0
marker.color.b = 1.0
marker.color.a = 1.0
for p in self.points:
pt = Point32()
pt.x = p[0]
pt.y = p[1]
pt.z = 0.0
marker.points.append(pt)
else:
# delete
marker.action = marker.DELETE
self.traj_pub.publish(marker)
print('publishing traj')
elif self.traj_pub.get_num_connections() == 0:
print "Not publishing trajectory, no subscribers"
def callback(self, data):
marker = Marker()
marker.header = data.header #_id = data.header.frame_id
marker.header.stamp = rospy.Time.now()
angle = 0
marker.id = 0
marker.ns = "scan_ds"
marker.lifetime.secs = 100
marker.scale.x = self.scale
marker.scale.y = self.scale
marker.scale.z = self.scale
for i in range(0,len(data.ranges), self.n):
if np.abs(data.ranges[i]) < 20.0:
p = Point()
p.x = data.ranges[i] * np.cos(angle)
p.y = data.ranges[i] * np.sin(angle)
p.z = 0
marker.points.append(p)
angle += self.n * data.angle_increment
marker.type = marker.POINTS
marker.color.a = 1.0
marker.color.r = 1.0
marker.color.b = 0.0
marker.color.g = 0.0
self.marker_pub.publish(marker)
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 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 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 create_circle_marker(self,
pose_x,
pose_y,
ellipse_theta,
ellipse_a,
ellipse_b,
marker_id=0,
color=ColorRGBA(0, 1, 0, 1)):
h = Header()
# h.frame_id = self.scan_frame_id #tie marker visualization to laser it comes from
h.frame_id = "base_laser_link"
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 = "ellipse_marker"
mark.id = marker_id
mark.type = 3
mark.action = 0
mark.scale = Vector3(ellipse_a * 2, ellipse_b * 2,
1) #scale, in meters
mark.color = color
pose = Pose(Point(pose_x, pose_y, 0.5),
Quaternion(0.0, 0.0, 1.0, cos(ellipse_theta / 2)))
mark.pose = pose
return mark
def publish_end_point(self, duration=0.0):
should_publish = len(self.points) > 1
if self.visualize and self.end_pub.get_num_connections() > 0:
print "Publishing end point"
marker = Marker()
marker.header = self.make_header("/map")
marker.ns = self.viz_namespace + "/trajectory"
marker.id = 1
marker.type = 2 # sphere
marker.lifetime = rospy.Duration.from_sec(duration)
if should_publish:
marker.action = 0
marker.pose.position.x = self.points[-1][0]
marker.pose.position.y = self.points[-1][1]
marker.pose.orientation.w = 1.0
marker.scale.x = 1.0
marker.scale.y = 1.0
marker.scale.z = 1.0
marker.color.r = 1.0
marker.color.g = 0.0
marker.color.b = 0.0
marker.color.a = 1.0
else:
# delete marker
marker.action = 2
self.end_pub.publish(marker)
def pub_at_position(self, odom_msg):
"""
Handles necessary information for displaying things at
ACCEPTED (NOVATEL) POSITION SOLUTION:
-vehicle mesh
!!!this should only be invoked when the accepted (novatel) position
is updated
"""
### G35 Mesh #############################################################
marker = Marker()
pub = self.curpos_publisher
marker.header = odom_msg.header
marker.id = 0 # enumerate subsequent markers here
marker.action = Marker.MODIFY # can be ADD, REMOVE, or MODIFY
marker.pose = odom_msg.pose.pose
marker.pose.position.z = 1.55
marker.lifetime = rospy.Duration() # will last forever unless modified
marker.ns = "vehicle_model"
marker.type = Marker.MESH_RESOURCE
marker.scale.x = 0.0254 # artifact of sketchup export
marker.scale.y = 0.0254 # artifact of sketchup export
marker.scale.z = 0.0254 # artifact of sketchup export
marker.color.r = .05
marker.color.g = .05
marker.color.b = .05
marker.color.a = .2
marker.mesh_resource = self.veh_mesh_resource
marker.mesh_use_embedded_materials = False
pub.publish(marker)
def publish_trajectory(self, duration=0.0, should_publish=True):
rospy.loginfo('Publishing subsampled trajectory.')
marker = Marker()
marker.header = utils.make_header("/map")
marker.ns = 'visualization/'
marker.id = 2
marker.type = 4 # line strip
marker.lifetime = rospy.Duration.from_sec(duration)
if should_publish:
marker.action = 0
marker.scale.x = 0.1
marker.scale.y = 0.1
marker.scale.z = 0.05
marker.color.r = 0.0
marker.color.g = 0.0
marker.color.b = 1.0
marker.color.a = 0.75
for p in self.traj_pts:
pt = Point32()
pt.x = p[0]
pt.y = p[1]
pt.z = -0.1
marker.points.append(pt)
else:
marker.action = 2
self.traj_pub.publish(marker)
rospy.loginfo('Publishing a subsampled trajectory with %d points.' %
len(marker.points))
def _build_marker_array(self, header, objects):
"""
Build MarkerArray from given header and list of ObjectItem.
Args:
header (std_msgs.msg.Header):
Header of current processing frame
objects (list):
List of ObjectItem in which each item represents a merged result
Returns:
MarkerArray: Includes marker for cleaning preivous markers,
marker for bouding box and marker for text.
"""
marker_array = MarkerArray()
clean_all_marker = Marker()
clean_all_marker.header = header
clean_all_marker.action = Marker.DELETEALL
marker_array.markers.append(clean_all_marker)
markers = []
for obj in objects:
markers.extend([obj.linelist()])
markers.extend([obj.min_text()])
markers.extend([obj.max_text()])
markers.extend([obj.name_id_text()])
for idx, marker in enumerate(markers):
marker.id = idx
marker_array.markers.extend(markers)
return marker_array
def local_display(self, frame):
object_list = self.objects.obstacles
for i in range(len(object_list)):
marker = Marker()
marker.header = Header()
marker.header.frame_id = frame
marker.ns = "Object_NS"
marker.id = i
marker.type = Marker.CYLINDER
marker.action = 0
marker.color.r = 0.0
marker.color.g = 0.0
marker.color.b = 1.0
marker.color.a = 0.5
marker.lifetime = rospy.Duration.from_sec(0.1)
marker.pose.position.x = object_list[i].pos.point.x
marker.pose.position.y = object_list[i].pos.point.y
marker.pose.position.z = 0
radius = object_list[i].radius
marker.scale.x = radius
marker.scale.y = radius
marker.scale.z = 0.01
self.display_pub.publish(marker)
def publish(self, grasps, obj = None):
msg = MarkerArray()
self.marker_id = 0 # reset marker counter
if obj and len(obj.primitives) > 0 and len(obj.primitive_poses) > 0:
m = Marker()
m.header = obj.header
m.ns = "object"
m.id = self.marker_id
self.marker_id += 1
m.type = m.CUBE
m.action = m.ADD
m.pose = obj.primitive_poses[0]
m.scale.x = obj.primitives[0].dimensions[0]
m.scale.y = obj.primitives[0].dimensions[1]
m.scale.z = obj.primitives[0].dimensions[2]
m.color.r = 0
m.color.g = 0
m.color.b = 1
m.color.a = 0.8
msg.markers.append(m)
for grasp in grasps:
msg.markers.append(self.get_gripper_marker(grasp.grasp_pose, grasp.grasp_quality))
self.pub.publish(msg)
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 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 make_arrow_marker(self, header, pose, rgba, arrow_scale, ns):
marker = Marker(type=Marker.ARROW,action=Marker.ADD)
marker.header = header
marker.pose = pose
marker.lifetime = rospy.Duration(0)
marker.color = stdm.ColorRGBA(*rgba)
marker.scale = gm.Vector3(6*arrow_scale, arrow_scale, arrow_scale)
marker.id = self.get_unused_id()
self.ids.add(marker.id)
marker.ns = ns
return marker
def send_marker(self, named_pt):
m=Marker()
m.header = copy.deepcopy(named_pt.header)
m.type=Marker.CYLINDER
m.pose.position = named_pt.point
m.pose.orientation.x=0.707
m.pose.orientation.y=0.0
m.pose.orientation.z=0.0
m.pose.orientation.w=0.707
m.scale.x=0.2
m.scale.y=0.2
m.scale.z=0.2
m.color.r=0.8
m.color.g=0.8
m.color.b=0.8
m.color.a=1.0
m.id = self.count
#m.text=named_pt.name
self.marker_pub.publish(m)
self.count += 1
t=Marker()
t.header = copy.deepcopy(named_pt.header)
m.type = Marker.TEXT_VIEW_FACING
m.pose.position = named_pt.point
m.pose.position.z += 0.1
m.pose.orientation.x=0.707
m.pose.orientation.y=0.0
m.pose.orientation.z=0.0
m.pose.orientation.w=0.707
m.scale.x=0.2
m.scale.y=0.2
m.scale.z=0.2
m.color.r=0.8
m.color.g=0.8
m.color.b=0.8
m.color.a=1.0
m.text = named_pt.name
m.id = self.count
self.marker_pub.publish(m)
self.count += 1
def pub_marker():
marker = Marker(ns="car_model", id=0, type=Marker.MESH_RESOURCE, action=Marker.ADD, frame_locked=True, mesh_resource =
"package://gazebo_drive_simulator/models/polaris.stl"
, mesh_use_embedded_materials=False)
marker.scale = Vector3(1, 1, 1)
marker.header = std_msgs.msg.Header(frame_id="car", stamp=rospy.get_rostime())
marker.pose = Pose(position=Point(x=0, y=0, z=0), orientation=Quaternion(x=0, y=0, z=-numpy.sqrt(2.0)/2.0, w=numpy.sqrt(2.0)/2.0))
marker.color = std_msgs.msg.ColorRGBA(r=0.2, g=0.2, b=1.0, a=0.7)
r = rospy.Rate(1)
while not rospy.is_shutdown():
marker_pub.publish(MarkerArray([marker]+get_line_markers(rad)))
r.sleep()
def _fit_ellipse(self, data, publisher):
# print "fitting"
ellipse_xy = []
# points = [] #array to hold all points - FOR DEBUGGING
angle = data.angle_min
incr = data.angle_increment
max_range = data.range_max
ranges = data.ranges
# polar >> cartesian
for r in ranges:
if r < max_range:
ellipse_xy.append([cos(angle) * r, sin(angle) * r]) # make xy
angle += incr
# eliminate outlying points
# x_avg = sum([xy[0] for xy in ellipse_xy])/len(ellipse_xy)
# y_avg = sum([xy[1] for xy in ellipse_xy])/len(ellipse_xy)
# for xy in ellipse_xy:
# if (abs(xy[0]-x_avg)<0.5 or abs(xy[1]-y_avg)<0.5):
# ellipse_xy.remove(xy)
# fit ellipse to points - constrain size
if len(ellipse_xy) > 1:
e2 = Ellipse2d()
e2.fit(ellipse_xy)
# apply alpha to smooth changes over time, if old data exists
if self.last_a != None and self.last_b != None and self.last_center != None:
e2.center = [
self.last_center[i] * self.center_alpha + e2.center[i] * (1 - self.center_alpha) for i in [0, 1]
]
e2.a = self.last_a * self.axis_alpha + e2.a * (1 - self.axis_alpha)
e2.b = self.last_b * self.axis_alpha + e2.b * (1 - self.axis_alpha)
self.last_center = e2.center
self.last_b = e2.b
self.last_a = e2.a
# Publish ellipse data as Marker message
h = Header()
h.frame_id = "laser" # 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
# publish marker:person_marker, modify a red cylinder, last indefinitely
mark = Marker()
mark.header = h
mark.ns = "person_marker"
mark.id = 0
mark.type = 3
mark.action = 0
mark.pose = geometry_msgs.msg.Pose(
geometry_msgs.msg.Point(e2.center[0], e2.center[1], 0.5),
geometry_msgs.msg.Quaternion(0.0, 0.0, 1.0, cos(e2.theta / 2)),
)
mark.scale = geometry_msgs.msg.Vector3(e2.a * 2, e2.b * 2, 1) # scale, in meters
mark.color = self.color # marker is red if clean, green if infected
publisher.publish(mark)
else:
print "data not received"
def marker_at_point(point_stamped, ns='', mid=0, mtype=Marker.SPHERE, sx=0.05, sy=0.05, sz=0.05, r=0.0, g=0.0,
b=1.0, a=0.8):
'''
Returns a single marker at a point.
Orientation is always (0, 0, 0, 1). See the visualization_msgs.msg.Marker documentation for more details on
any of these fields.
**Args:**
**point_stamped (geometry_msgs.msg.PointStamped):** Point for marker
*ns (string):* namespace
*mid (int):* ID
*mtype (int):* Shape type
*sx (double):* Scale in x
*sy (double):* Scale in y
*sz (double):* scale in z
*r (double):* Red (scale 0 to 1)
*g (double):* Green (scale 0 to 1)
*b (double):* Blue (scale 0 to 1)
*a (double):* Alpha (scale 0 to 1)
**Returns:**
visualization_msgs.msg.Marker at point_stamped
'''
marker = Marker()
marker.header = copy.deepcopy(point_stamped.header)
marker.ns = ns
marker.id = mid
marker.type = mtype
marker.action = marker.ADD
marker.pose.position = copy.deepcopy(point_stamped.point)
marker.pose.orientation.w = 1.0
marker.scale.x = sx
marker.scale.y = sy
marker.scale.z = sz
marker.color.r = r
marker.color.g = g
marker.color.b = b
marker.color.a = a
return marker
def create_arrow(self, pose_stamped):
m = Marker()
m.header = pose_stamped.header
m.ns = "billiards_shot_plan"
m.id = self._id
m.action = Marker.ADD
m.type = Marker.ARROW
m.pose = pose_stamped.pose
m.scale.x = 0.5
m.scale.y = 0.2
m.scale.z = 0.2
m.color.a = 1.0
self._id += 1
return m
def axis_marker(pose_stamped, id,
ns):
"""
Create three Marker msgs from a PoseStamped set of arrows (x=red, y=green, z=blue).
Parameters:
id: the id number for the x-arrow (y is id+1, z is id+2)
ns: the namespace for the marker
"""
marker = Marker()
marker.header = pose_stamped.header
marker.ns = ns
marker.type = Marker.ARROW
marker.action = Marker.ADD
marker.scale.x = 0.01
marker.scale.y = 0.02
marker.color.a = 1.0
marker.lifetime = rospy.Duration(1/5.0)
orientation = pose_stamped.pose.orientation
quat = [orientation.x, orientation.y, orientation.z, orientation.w]
mat = tf.transformations.quaternion_matrix(quat)
start = (pose_stamped.pose.position.x,
pose_stamped.pose.position.y,
pose_stamped.pose.position.z)
x_end = list(mat[:,0][0:3]*.05 + numpy.array(start))
y_end = list(mat[:,1][0:3]*.05 + numpy.array(start))
z_end = list(mat[:,2][0:3]*.05 + numpy.array(start))
marker.id = id
marker.points = [pose_stamped.pose.position, Point(*x_end)]
marker.color.r = 1.0
marker.color.g = 0.0
marker.color.b = 0.0
marker2 = copy.deepcopy(marker)
marker2.id = id+1
marker2.points = [pose_stamped.pose.position, Point(*y_end)]
marker2.color.r = 0.0
marker2.color.g = 1.0
marker2.color.b = 0.0
marker3 = copy.deepcopy(marker2)
marker3.id = id+2
marker3.points = [pose_stamped.pose.position, Point(*z_end)]
marker3.color.r = 0.0
marker3.color.g = 0.0
marker3.color.b = 1.0
return (marker, marker2, marker3)
def newCompositeDetectedPersonsAvailable(compositeDetectedPersons):
global currentMarkerId, oldestActiveMarkerId
markerArray = MarkerArray()
markerZ = rospy.get_param("~marker_z", 2.0)
markerScale = rospy.get_param("~marker_scale", 1.0)
# Delete old markers
for markerId in xrange(oldestActiveMarkerId, currentMarkerId):
marker = Marker()
marker.header = compositeDetectedPersons.header
marker.id = markerId
marker.action = Marker.DELETE
markerArray.markers.append(marker)
oldestActiveMarkerId = currentMarkerId
# Create new markers
for compositeDetectedPerson in compositeDetectedPersons.elements:
if len(compositeDetectedPerson.original_detections) > 1:
for originalDetectedPerson in compositeDetectedPerson.original_detections:
marker = Marker()
marker.header = compositeDetectedPersons.header
marker.id = currentMarkerId
marker.type = Marker.ARROW
marker.color.r = marker.color.b = marker.color.a = 1.0
marker.color.g = 0.3
marker.points.append( Point(compositeDetectedPerson.pose.pose.position.x, compositeDetectedPerson.pose.pose.position.y, markerZ ) )
marker.points.append( Point(originalDetectedPerson.pose.pose.position.x, originalDetectedPerson.pose.pose.position.y, markerZ ) )
marker.scale.x = 0.01 * markerScale
marker.scale.y = 0.05 * markerScale
marker.scale.z = 0.05 * markerScale
markerArray.markers.append(marker)
currentMarkerId += 1
markerArrayPublisher.publish(markerArray)
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 visualize_base_ray(): msg = Marker() msg.header = Header(stamp=Time.now(), frame_id="base_footprint") msg.scale = Vector3(x=0.005, y=0.0, z=0.0) # only x is used msg.pose.position = Point(x=0, y=0, z=0) # arrow msg.pose.orientation = Quaternion(x=0, y=0, z=0, w=1) msg.points = [Point(x=0, y=0,z=0.01), Point(x=WORKING_DIST_FROM_POOP,y=0,z=0.01)] msg.ns = "base_ray" msg.id = 0 msg.action = 0 # add msg.type = 4 # line strip msg.color = ColorRGBA(r=0, g=0, b=0, a=1) msg.color.g = 0.5; msg.color.b = 1; viz_pub.publish(msg)
def lineMarker(self, p, d): marker = Marker() header = Header() header.stamp = rospy.Time.now() header.frame_id = self.base_frame marker.header = header marker.ns = "rays" marker.id = self.recent_next marker.type = Marker.LINE_LIST marker.action = Marker.ADD marker.scale = Vector3(*[0.02 for i in range(3)]) marker.points = toPoints(np.column_stack([p, (p+2*d/np.linalg.norm(d))])) color = self.recent_color marker.color = color marker.colors = [color for i in range(len(marker.points))] return marker
def publish_markers(self, topic):
'''
Publish RViz visualization markers for the current collection of annotations.
@param topic: Where we must publish annotations markers.
'''
if len(self._annotations) == 0:
messages = "No annotations retrieved. Nothing to publish!"
rospy.logwarn(messages)
return
# Advertise a topic for retrieved annotations' visualization markers
markers_pub = rospy.Publisher(topic, MarkerArray, latch=True, queue_size=5)
# Process retrieved data to build markers lists
markers_list = MarkerArray()
marker_id = 1
for a in self._annotations:
marker = Marker()
marker.id = marker_id
marker.header = a.pose.header
marker.type = a.shape
marker.ns = a.type
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
markers_list.markers.append(marker)
label = copy.deepcopy(marker)
label.id = label.id + 1000000 # marker id must be unique
label.type = Marker.TEXT_VIEW_FACING
label.text = a.name + ' [' + a.type + ']'
label.pose.position.z = label.pose.position.z + label.scale.z/2.0 + 0.12 # just above the visual
label.scale.x = label.scale.y = label.scale.z = 0.3
label.color.a = 1.0
markers_list.markers.append(label)
marker_id = marker_id + 1
markers_pub.publish(markers_list)
