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 create_marker(self, message):
# type: (Location) -> (Marker, Marker)
r = random.random()
g = random.random()
b = random.random()
scale = Vector3(0.5, 0.5, 0.5)
frame_id = "map"
ns = "location_markers"
marker_str = Marker()
marker_str.header.stamp = rospy.Time.now()
marker_str.header.frame_id = frame_id
marker_str.ns = ns
marker_str.id = self.marker_id
marker_str.type = Marker.TEXT_VIEW_FACING
marker_str.text = message.name
marker_str.action = Marker.ADD
marker_str.pose.position = Point(message.pose.position.x, message.pose.position.y, 0.5)
marker_str.scale = scale
marker_str.color = ColorRGBA(r, g, b, 1)
self.marker_id += 1
marker_sphere = Marker()
marker_sphere.header.stamp = rospy.Time.now()
marker_sphere.header.frame_id = frame_id
marker_sphere.ns = ns
marker_sphere.id = self.marker_id
marker_sphere.type = Marker.SPHERE
marker_sphere.action = Marker.ADD
marker_sphere.pose.position = Point(message.pose.position.x, message.pose.position.y, 0)
marker_sphere.scale = scale
marker_sphere.color = ColorRGBA(r, g, b, 0.5)
self.marker_id += 1
return [marker_sphere, marker_str]
def world_body_to_marker_msg(world_body, id=1, ns=u''):
"""
:type world_body: WorldBody
:rtype: Marker
"""
m = Marker()
m.ns = u'{}/{}'.format(ns, world_body.name)
m.id = id
if world_body.type == WorldBody.URDF_BODY:
raise Exception(
u'can\'t convert urdf body world object to marker array')
elif world_body.type == WorldBody.PRIMITIVE_BODY:
if world_body.shape.type == SolidPrimitive.BOX:
m.type = Marker.CUBE
m.scale = Vector3(*world_body.shape.dimensions)
elif world_body.shape.type == SolidPrimitive.SPHERE:
m.type = Marker.SPHERE
m.scale = Vector3(world_body.shape.dimensions[0],
world_body.shape.dimensions[0],
world_body.shape.dimensions[0])
elif world_body.shape.type == SolidPrimitive.CYLINDER:
m.type = Marker.CYLINDER
m.scale = Vector3(
world_body.shape.dimensions[SolidPrimitive.CYLINDER_RADIUS],
world_body.shape.dimensions[SolidPrimitive.CYLINDER_RADIUS],
world_body.shape.dimensions[SolidPrimitive.CYLINDER_HEIGHT])
else:
raise Exception(
u'world body type {} can\'t be converted to marker'.format(
world_body.shape.type))
elif world_body.type == WorldBody.MESH_BODY:
m.type = Marker.MESH_RESOURCE
m.mesh_resource = world_body.mesh
m.color = ColorRGBA(0, 1, 0, 0.8)
return m
def make_individual_markers(self, msg):
lpost = Marker()
lpost.type = Marker.CYLINDER
lpost.scale = Vector3(POST_DIAMETER, POST_DIAMETER, GOAL_HEIGHT)
lpost.color.r = 1.0
lpost.color.g = 1.0
lpost.color.b = 1.0
lpost.color.a = 1.0
lpost.pose.position = Point(0, GOAL_WIDTH / 2, GOAL_HEIGHT / 2)
rpost = Marker()
rpost.type = Marker.CYLINDER
rpost.scale = Vector3(POST_DIAMETER, POST_DIAMETER, GOAL_HEIGHT)
rpost.color.r = 1.0
rpost.color.g = 1.0
rpost.color.b = 1.0
rpost.color.a = 1.0
rpost.pose.position = Point(0, -GOAL_WIDTH / 2, GOAL_HEIGHT / 2)
bar = Marker()
bar.type = Marker.CYLINDER
bar.scale = Vector3(POST_DIAMETER, POST_DIAMETER, GOAL_WIDTH)
bar.color.r = 1.0
bar.color.g = 1.0
bar.color.b = 1.0
bar.color.a = 1.0
bar.pose.position = Point(0, 0, GOAL_HEIGHT)
bar.pose.orientation = Quaternion(
math.sqrt(2) / 2, 0, 0,
math.sqrt(2) / 2)
return (lpost, rpost, bar)
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 visualize_pose(self,pos,orientation):
# Yellow Box for Vehicle
marker = Marker()
marker.header.stamp = rospy.Time.now()
marker.header.frame_id = 'map'
marker.ns = 'agent'
marker.id = 0
marker.type = marker.CUBE
marker.action = marker.ADD
marker.pose.position = pos
marker.pose.orientation = orientation
marker.scale = Vector3(x=0.7,y=0.42,z=1)
marker.color = ColorRGBA(r=1.0,g=1.0,a=1.0)
marker.lifetime = rospy.Duration(1.0)
self.pub_pose_marker.publish(marker)
# Red track for trajectory over time
marker = Marker()
marker.header.stamp = rospy.Time.now()
marker.header.frame_id = 'map'
marker.ns = 'agent'
marker.id = self.num_poses
marker.type = marker.CUBE
marker.action = marker.ADD
marker.pose.position = pos
marker.pose.orientation = orientation
marker.scale = Vector3(x=0.2,y=0.2,z=0.2)
marker.color = ColorRGBA(r=1.0,a=1.0)
marker.lifetime = rospy.Duration(10.0)
self.pub_pose_marker.publish(marker)
def visualize_subgoal(self,subgoal, subgoal_options=None):
markers = MarkerArray()
# Display GREEN DOT at NN subgoal
marker = Marker()
marker.header.stamp = rospy.Time.now()
marker.header.frame_id = 'map'
marker.ns = 'subgoal'
marker.id = 0
marker.type = marker.CYLINDER
marker.action = marker.ADD
marker.pose.position.x = subgoal[0]
marker.pose.position.y = subgoal[1]
marker.scale = Vector3(x=0.2,y=0.2,z=0)
marker.color = ColorRGBA(r=0.0,g=0.0,b=0.0,a=1.0)
marker.lifetime = rospy.Duration(2.0)
self.pub_goal_path_marker.publish(marker)
if subgoal_options is not None:
for i in xrange(len(subgoal_options)):
marker = Marker()
marker.header.stamp = rospy.Time.now()
marker.header.frame_id = 'map'
marker.ns = 'subgoal'
marker.id = i+1
# marker.type = marker.CUBE
marker.type = marker.CYLINDER
marker.action = marker.ADD
marker.pose.position.x = subgoal_options[i][0]
marker.pose.position.y = subgoal_options[i][1]
marker.scale = Vector3(x=0.2,y=0.2,z=0.2)
marker.color = ColorRGBA(r=0.0,g=0.0,b=255,a=1.0)
marker.lifetime = rospy.Duration(1.0)
self.pub_goal_path_marker.publish(marker)
def visualize_action(self, use_d_min):
# Display BLUE ARROW from current position to NN desired position
marker = Marker()
marker.header.stamp = rospy.Time.now()
marker.header.frame_id = 'map'
marker.ns = 'path_arrow'
marker.id = 0
marker.type = marker.ARROW
marker.action = marker.ADD
marker.points.append(self.pose.pose.position)
marker.points.append(self.desired_position.pose.position)
marker.scale = Vector3(x=0.1,y=0.2,z=0.2)
marker.color = ColorRGBA(b=1.0,a=1.0)
marker.lifetime = rospy.Duration(0.1)
self.pub_goal_path_marker.publish(marker)
# Display BLUE DOT at NN desired position
marker = Marker()
marker.header.stamp = rospy.Time.now()
marker.header.frame_id = 'map'
marker.ns = 'path_trail'
marker.id = self.num_poses
marker.type = marker.CUBE
marker.action = marker.ADD
marker.pose.position = copy.deepcopy(self.desired_position.pose.position)
marker.scale = Vector3(x=0.2,y=0.2,z=0.4)
marker.color = ColorRGBA(b=1.0,a=0.1)
marker.lifetime = rospy.Duration(100)
if self.desired_action[0] == 0.0:
marker.pose.position.x += 2.0*np.cos(self.desired_action[1])
marker.pose.position.y += 2.0*np.sin(self.desired_action[1])
self.pub_goal_path_marker.publish(marker)
def get_marker(
marker_type=None,
frame_id="world",
scale=1.0,
color="#ffffffff",
position=(0.0, 0.0, 0.0),
orientation=(0.0, 0.0, 0.0, 1.0),
):
""" Create a Marker visualization message.
Parameters
----------
marker_type : int, default Marker.LINE_STRIP
Type of the marker.
frame_id : str, default "world"
Name of the reference frame of the marker.
scale : float or iterable of float, len 3, default 1.0
Scale of the marker.
color : str, default "#ffffffff"
Color of the marker.
position : iterable, len 3, default (0.0, 0.0, 0.0)
Position of the marker wrt its reference frame.
orientation : iterable, len 4, default (0.0, 0.0, 0.0, 1.0)
Orientation of the marker wrt its reference frame.
Returns
-------
marker: Marker
Marker message.
"""
from geometry_msgs.msg import Point, Quaternion, Vector3
from visualization_msgs.msg import Marker
marker = Marker()
marker.type = marker_type or Marker.LINE_STRIP
marker.header.frame_id = frame_id
if isinstance(scale, float):
marker.scale = Vector3(scale, scale, scale)
else:
marker.scale = Vector3(*scale)
marker.color = hex_to_rgba(color)
marker.pose.orientation = Quaternion(*orientation)
marker.pose.position = Point(*position)
# TOD0: make configurable via vector
marker.points = []
return marker
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 callback_skeleton(self, data):
mk = Marker()
mk.header.frame_id = '/map'
mk.ns = 'skeletro'
mk.id = self.m_id
mk.type = Marker.SPHERE_LIST
mk.points = []
for j in data.joints:
if self.point_is_valid(j):
mk.points.append(j)
mk.scale = geometry_msgs.msg.Vector3(3, 3, 3)
mk.pose.position = geometry_msgs.msg.Point(0, 0, 0)
mk.pose.orientation = geometry_msgs.msg.Quaternion(0.0, 0.0, 0.0, 1.0)
# mk.lifetime = genpy.Duration(0.1)
mk.color = std_msgs.msg.ColorRGBA(1.0, 0.0, 0.0, 1.0)
linek = Marker()
linek.header.frame_id = '/map'
linek.ns = 'lines'
linek.id = self.m_id
linek.type = Marker.LINE_LIST
linek.scale = geometry_msgs.msg.Vector3(0.3, 0.3, 0.3)
linek.pose.position = geometry_msgs.msg.Point(0, 0, 0)
linek.pose.orientation = geometry_msgs.msg.Quaternion(
0.0, 0.0, 0.0, 1.0)
linek.points = []
self.vis_add_line(linek, data.joints, 0, 1)
self.vis_add_line(linek, data.joints, 1, 2)
self.vis_add_line(linek, data.joints, 3, 4)
self.vis_add_line(linek, data.joints, 4, 5)
self.vis_add_line(linek, data.joints, 6, 7)
self.vis_add_line(linek, data.joints, 7, 8)
self.vis_add_line(linek, data.joints, 9, 10)
self.vis_add_line(linek, data.joints, 10, 11)
self.vis_add_line(linek, data.joints, 12, 13)
self.vis_add_line(linek, data.joints, 13, 14)
# linek.lifetime = genpy.Duration(0.1)
linek.color = std_msgs.msg.ColorRGBA(0.0, 1.0, 0.0, 1.0)
msg = MarkerArray()
msg.markers = []
msg.markers.append(mk)
msg.markers.append(linek)
self.visualization_pub.publish(msg)
def publish(self, particles, best_particle=None, weights=None):
"""Publish particles pose / visualization
Args:
particles: [N,3] numpy array formatted (x,y,h)
best_particle : [3] numpy array formatted (x,y,h)
Returns:
None
"""
self.part_msg_.header.frame_id = 'map'
self.part_msg_.header.seq += 1
self.part_msg_.header.stamp = rospy.Time.now()
if best_particle is not None:
self.best_msg_.header.frame_id = 'map'
self.best_msg_.header.seq += 1
self.best_msg_.header.stamp = rospy.Time.now()
if weights is not None:
weights = (0.1 / weights.max()) * weights
# populate poses
#print 'length of particle : ', len(particles)
self.part_msg_.poses = [self.particle_to_pose(p) for p in particles]
self.part_pub_.publish(self.part_msg_)
# poses v2
self.mark_msg_.markers = []
n = len(particles)
for i in range(n):
mk = Marker(header=Header(stamp=rospy.Time.now(), frame_id='map'))
mk.ns = 'pcloud'
mk.id = i
mk.type = mk.ARROW
mk.action = mk.ADD
mk.pose = self.part_msg_.poses[i]
if weights is not None:
s = weights[i]
mk.scale = Vector3(s * 2, s / 3., s / 3.)
mk.color = ColorRGBA(1.0, 0.0, 1.0, s)
else:
mk.scale = Vector3(0.1, 0.02, 0.02)
mk.color = ColorRGBA(1.0, 0.0, 1.0, 1.0)
mk.lifetime = rospy.Duration(1.0) # stay alive for 1 sec
self.mark_msg_.markers.append(mk)
self.mark_pub_.publish(self.mark_msg_)
if best_particle is not None:
self.best_msg_.pose = self.particle_to_pose(best_particle)
self.best_pub_.publish(self.best_msg_)
def callbackGeneratedActions(self,actions):
zero_orientation = Quaternion(0, 0, 0, 1)
zero_position = Point(0, 0, 0)
zero_pose = Pose(zero_position, zero_orientation)
counter = 0
marker_array = MarkerArray()
for pose in actions.actions:
marker = Marker()
marker.header.stamp = rospy.get_rostime()
marker.header.frame_id = "world"
marker.ns = "model_visualizer_generated_actions"
marker.id = counter
counter = counter + 1
marker.action = Marker.ADD
marker.scale = Vector3(0.02,0.02,0.02)
marker.color.a = 1
marker.color.b = random.random()
marker.color.r = random.random()
marker.color.g = random.random()
marker.type = Marker.ARROW
marker.pose = zero_pose
marker.points.append( Point(0,0,0) )
marker.points.append( Point(pose.position.x, pose.position.y, pose.position.z) )
marker_array.markers.append(marker)
self.pub_array.publish(marker_array)
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 publish_goal(self):
# Publish the reference topic
msg = PoseStamped()
msg.header.frame_id = self._arm_interface.base_link
msg.header.stamp = rospy.Time.now()
msg.pose.position.x = self._last_goal.p.x()
msg.pose.position.y = self._last_goal.p.y()
msg.pose.position.z = self._last_goal.p.z()
msg.pose.orientation = Quaternion(*self._last_goal.M.GetQuaternion())
self._goal_pub.publish(msg)
marker = Marker()
marker.header.frame_id = self._arm_interface.base_link
marker.header.stamp = rospy.Time.now()
marker.ns = self._arm_interface.arm_name
marker.id = 0
marker.type = Marker.SPHERE
marker.action = Marker.MODIFY
marker.pose.position = Vector3(self._last_goal.p.x(),
self._last_goal.p.y(),
self._last_goal.p.z())
marker.pose.orientation = Quaternion(*self._last_goal.M.GetQuaternion())
marker.scale = Vector3(0.2, 0.2, 0.2)
marker.color.a = 1.0
marker.color.r = 1.0
marker.color.g = 0.0
marker.color.b = 0.0
self._goal_marker_pub.publish(marker)
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 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 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 perform(self, reevaluate=False):
pose_msg = self.blackboard.pathfinding.get_ball_goal(
self.target, self.distance)
self.blackboard.pathfinding.publish(pose_msg)
approach_marker = Marker()
approach_marker.pose.position.x = self.distance
approach_marker.type = Marker.SPHERE
approach_marker.action = Marker.MODIFY
approach_marker.id = 1
color = ColorRGBA()
color.r = 1.0
color.g = 1.0
color.b = 1.0
color.a = 1.0
approach_marker.color = color
approach_marker.lifetime = rospy.Duration(nsecs=0.5)
scale = Vector3(0.2, 0.2, 0.2)
approach_marker.scale = scale
approach_marker.header.stamp = rospy.Time.now()
approach_marker.header.frame_id = self.blackboard.world_model.base_footprint_frame
self.blackboard.pathfinding.approach_marker_pub.publish(
approach_marker)
if self.blackboard.pathfinding.status in [
GoalStatus.SUCCEEDED, GoalStatus.ABORTED
] or not self.blocking:
self.pop()
def output_to_rviz(array, scale, color):
print 'Inside output_to_rviz()'
global publisher
global rviz_id
# make MarkerArray message
markerArray = MarkerArray()
# loop throgh all instances of the array
for index in range(len(array)):
marker = Marker()
marker.id = rviz_id
marker.header.frame_id = "/map"
marker.type = marker.SPHERE
marker.action = marker.ADD
marker.lifetime = rospy.Duration(120.0)
marker.scale = scale
marker.color = color
# x and y are inverted due to nature of the map
marker.pose.position.x = array[index][1]
marker.pose.position.y = array[index][0]
markerArray.markers.append(marker)
# incremment rviz_id
rviz_id = rviz_id + 1
# Publish the MarkerArray
publisher.publish(markerArray)
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 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 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_, rot=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 make_sphere_marker(self, xy, radius, color, namespace):
# type: ([int,int], float, ColorRGBA, str) -> Marker
""" 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 """
marker = Marker()
marker.header.stamp = rospy.Time.now()
marker.header.frame_id = 'map'
marker.ns = namespace
marker.id = self.mark_id
self.mark_id += 1
marker.type = 2
marker.action = 0
marker.pose = geometry_msgs.msg.Pose()
marker.pose.orientation.w = 1
marker.pose.position.x = xy[0]
marker.pose.position.y = xy[1]
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 publish_goal(self):
# Publish the reference topic
msg = PoseStamped()
msg.header.frame_id = self._arm_interface.base_link
msg.header.stamp = rospy.Time.now()
msg.pose.position.x = self._last_goal.p.x()
msg.pose.position.y = self._last_goal.p.y()
msg.pose.position.z = self._last_goal.p.z()
msg.pose.orientation = Quaternion(*self._last_goal.M.GetQuaternion())
self._goal_pub.publish(msg)
marker = Marker()
marker.header.frame_id = self._arm_interface.base_link
marker.header.stamp = rospy.Time.now()
marker.ns = self._arm_interface.arm_name
marker.id = 0
marker.type = Marker.SPHERE
marker.action = Marker.MODIFY
marker.pose.position = Vector3(self._last_goal.p.x(),
self._last_goal.p.y(),
self._last_goal.p.z())
marker.pose.orientation = Quaternion(
*self._last_goal.M.GetQuaternion())
marker.scale = Vector3(0.2, 0.2, 0.2)
marker.color.a = 1.0
marker.color.r = 1.0
marker.color.g = 0.0
marker.color.b = 0.0
self._goal_marker_pub.publish(marker)
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 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 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 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 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 pub_marker(self, imu, delta):
'''
Publishes a new marker based on the current state.
Args:
imu [Imu]: The ROS message 'sensor_msgs.msg.Imu'.
delta [float]: Time delta between the timestamps of the messages.
'''
marker = Marker()
marker.id = imu.header.seq
marker.header.frame_id = self.frame_id
marker.header.stamp = imu.header.stamp
marker.header.seq = imu.header.seq
marker.type = Marker.LINE_STRIP
marker.action = marker.ADD
marker.color = self.color
marker.scale = self.scale
marker.pose.position = Point(*self.s)
marker.pose.orientation.w = 1.0
marker.points = [Point(*(-self.v * delta)), Point(0.0, 0.0, 0.0)]
#rospy.loginfo("{} Marker:\n{}".format(rospy.get_caller_id(), marker))
self.pub.publish(marker)
def output_rviz_message(self):
markers = MarkerArray()
spheres = Marker()
lines = Marker()
spheres.id = 1
spheres.header.frame_id = self.frame_id
spheres.type = Marker.SPHERE_LIST
spheres.action = 0
spheres.scale = Vector3(0.005, 0.005, 0.005)
spheres.color.b = 1.0
spheres.color.a = 1.0
for node in self.graph.nodes:
spheres.points.append(Point(*node))
lines.id = 2
lines.header.frame_id = self.frame_id
lines.type = Marker.LINE_LIST
lines.action = 0
lines.scale.x = 0.0025
lines.color.g = 1.0
lines.color.a = 1.0
for node_1, node_2 in self.graph.edges:
lines.points.extend([Point(*node_1), Point(*node_2)])
markers.markers = [spheres, lines]
return markers
def make_marker(marker_type, scale, r, g, b, a):
# make a visualization marker array for the occupancy grid
m = Marker()
m.action = Marker.ADD
m.header.frame_id = '/narko_base_link'
m.header.stamp = rospy.Time.now()
m.ns = 'marker_test_%d' % marker_type
m.id = 0
m.type = marker_type
m.pose.position.x = 0
m.pose.position.y = 0
m.pose.position.z = 0
m.pose.orientation.w = 0
m.pose.orientation.x = 0
m.pose.orientation.y = 0
m.pose.orientation.z = 1
m.scale = scale
m.color.r = 0
m.color.g = 1
m.color.b = 0
m.color.a = 0.9
m.color.r = r
m.color.g = g
m.color.b = b
m.color.a = a
return 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 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 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 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 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 publish_marker(self, marker_pose, colour, ID):
"""
Publishes a maker at the given maker pose, colors and ID
:@param marker_pose: the position to place the marker
:@type marker_pose: Pose()
:@param colour: Colour of marker. red, green, blue or yellow
:@type colour: String. (e.g. "yellow")
:@param ID: The ID of the ID of the marker.
:@type ID: int
"""
marker = Marker()
marker.header.seq = len(self.marker_array.markers) + 1
marker.id = ID
marker.header.frame_id = "odom"
marker.header.stamp = rospy.Time.now()
marker.pose = marker_pose
marker.scale = Vector3(0.2, 0.2, 0.2)
marker.color.a = 1.0 # must be non-zero
if colour == "red":
marker.color.r = 1.0
elif colour == "blue":
marker.color.b = 1.0
elif colour == "green":
marker.color.g = 1.0
elif colour == "yellow":
marker.color.r = 1.0
marker.color.g = 1.0
self.marker_array.markers.append(marker)
self.publish_beacon_pos.publish(self.marker_array)
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 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 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 plane(self, frame_id, p, n, r=1.0, key=None, draw_triad=False):
"""
plane: n . ( X - p ) = 0
This will be drawn as a disc of radius r about p.
"""
n = np.array(n)
p = np.array(p)
m = Marker()
m.header.frame_id = frame_id
m.ns = key or 'planes'
m.id = 0 if key else len(self.planes)
m.type = Marker.TRIANGLE_LIST
m.action = Marker.MODIFY
# Compute plane rotation
nz = n / np.linalg.norm(n)
nx = np.cross(nz, np.array([0,0,1])); nx = nx / np.linalg.norm(nx)
ny = np.cross(nz, nx); ny = ny / np.linalg.norm(ny)
R = kdl.Rotation(
kdl.Vector(*nx),
kdl.Vector(*ny),
kdl.Vector(*nz))
# define the pose of the plane marker
m.pose = Pose(Point(*(p)), Quaternion(*R.GetQuaternion()))
m.scale = Vector3(1, 1, 1)
# flatten the triangle list
for triangle in self.mesh_t:
m.points.extend([
Point(r*self.mesh_p[t][0],r*self.mesh_p[t][1],0)
for t in triangle])
m.color = ColorRGBA(0.8,0,0.8,0.9)
m.colors = [m.color] * len(m.points)
if draw_triad:
self.line(frame_id, p, nx, t=[0.0,r],key=key+'-x')
self.line(frame_id, p, ny, t=[0.0,r],key=key+'-y')
self.line(frame_id, p, nz, t=[0.0,r],key=key+'-z')
self.point(frame_id, p, 0.005, key=key+'-center-point')
key = key or element_key(m)
with self.mod_lock:
self.planes[key] = m
return key
def ScaleMarker(marker_template, control_scale=None, visual_scale=None):
"""Scale InteractiveMarker and/or a visual Marker associated with the InteractiveMarker.
@type marker_template: subclass of MarkerTemplate()
@param marker_template: The template object containing InteractiveMarkers.
@type control_scale: float
@param control_scale: The scale factor for the InteractiveMarker.
@type visual_scale: geometry_msgs/Vector3
@param visual_scale: The scale factor for the visualization Marker in the template.
"""
server = marker_template.server
menu_handler = marker_template.menu_handler
marker_name = marker_template.key
if server:
current_marker = server.get(marker_name)
if current_marker:
# rescale marker
marker = Marker()
marker = GetVisualMarker(current_marker)
if visual_scale is not None:
marker.scale = visual_scale
# push marker into visual control
visual = InteractiveMarkerControl()
visual.name = "visual"
visual.always_visible = GetVisualControl(current_marker).always_visible
visual.interaction_mode = GetVisualControl(current_marker).interaction_mode
visual.orientation = GetVisualControl(current_marker).orientation
visual.markers.append(marker)
new_marker = InteractiveMarker()
new_marker.header.frame_id = current_marker.header.frame_id
new_marker.name = current_marker.name
new_marker.description = current_marker.description
new_marker.pose = current_marker.pose
new_marker.scale = current_marker.scale
if control_scale is not None:
new_marker.scale = control_scale
new_marker.controls.append(visual)
for control in current_marker.controls:
if 'Translate' in control.name or 'Rotate' in control.name:
# todo rename Plane Translate so we don't need to do this extra check
if control.name not in ['TranslateXY', 'TranslateYZ','TranslateXZ']:
new_marker.controls.append(CreateTransRotControl(control.name))
# insert the updated marker into the server
server.insert(new_marker)
menu_handler.apply(server, marker_name)
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 create_arrow_marker(pose, m_id, color=ColorRGBA(1., 0., 0., 1.)):
m = Marker()
m.header.frame_id = "/base_link"
m.header.stamp = rospy.Time.now()
m.ns = "ell_pose_viz"
m.id = m_id
m.type = Marker.ARROW
m.action = Marker.ADD
m.scale = Vector3(0.19, 0.09, 0.02)
m.color = color
m.pose = PoseConv.to_pose_msg(pose)
return m
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 make_marker(frame_id, id, pose, scale, color, frame_locked):
msg = Marker()
msg.header.frame_id = frame_id
msg.header.stamp = rospy.Time.now()
msg.ns = "pouring_visualization"
msg.id = id
msg.action = Marker.ADD
msg.pose = pose
msg.scale = scale
msg.color = color
msg.frame_locked = frame_locked
return msg
def publish_vector(self, frame, loc, v, m_id):
m = Marker()
m.header.frame_id = frame
m.header.stamp = rospy.Time.now()
m.ns = "netft_zeroing"
m.id = m_id
m.type = Marker.ARROW
m.action = Marker.ADD
m.points.append(Point(*loc))
m.points.append(Point(*(loc + v)))
m.scale = Vector3(0.01, 0.02, 0.01)
m.color = self.colors[m_id]
self.vis_pub.publish(m)
def create(**args):
m = Marker(**args)
# make shure the marker is displayable
if not args.has_key('header'):
m.header.stamp = rospy.Time.now()
m.header.frame_id = '/base_link'
if not args.has_key('color'):
m.color = ColorRGBA(0.3, 0.3, 0.3, 1)
if not args.has_key('orientation'):
m.pose.orientation = Quaternion(0, 0, 0, 1)
if not args.has_key('scale'):
m.scale = Vector3(1, 1, 1)
return m
def publish_vector(self, loc, v, m_id):
m = Marker()
m.header.frame_id = "torso_lift_link"
m.header.stamp = rospy.Time()
m.ns = "force_torque"
m.id = m_id
m.type = Marker.ARROW
m.action = Marker.ADD
m.points.append(Point(*loc))
m.points.append(Point(*(loc + v)))
m.scale = Vector3(0.01, 0.02, 0.01)
m.color = self.colors[m_id]
self.vis_pub.publish(m)
def create_ear_marker(self, pose, m_id, color=ColorRGBA(0., 1., 1., 1.)):
m = Marker()
#m.header.frame_id = "/base_link"
m.header.frame_id = "/ellipse_frame"
m.header.stamp = rospy.Time.now()
m.ns = "head_markers"
m.id = m_id
m.type = Marker.CYLINDER
m.action = Marker.ADD
m.scale = ear_scale
m.color = color
m.pose = PoseConverter.to_pose_msg(pose)
return m
def publish_markers( self ):
for i,g in enumerate(self.poses):
m = Marker()
m.ns = 'sample_positions'
m.id = i
m.action = m.ADD
m.type = m.ARROW
m.header.frame_id = self.frame
m.header.stamp = rospy.Time.now()
m.scale = Vector3( 0.15, 1.0, 1.0 )
m.color = ColorRGBA( 0.2, 1.0, 0.2, 0.7 )
m.pose = g.target_pose.pose
self.pub.publish( m )
def create_arrow_marker(pose, m_id, color=ColorRGBA(1.0, 0.0, 0.0, 1.0)):
m = Marker()
if USE_ELLIPSE_FRAME:
m.header.frame_id = "/ellipse_frame"
else:
m.header.frame_id = "/base_link"
m.header.stamp = rospy.Time.now()
m.ns = "ell_pose"
m.id = m_id
m.type = Marker.ARROW
m.action = Marker.ADD
m.scale = Vector3(0.19, 0.09, 0.02)
m.color = color
m.pose = PoseConverter.to_pose_msg(pose)
return m
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 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 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
