def spherelist_marker(points, size=[0.1, 0.1, 0.1], rgba=[1.0, 1.0, 1.0, 1.0],
ns="", id=0, frame_id="", lifetime=None, stamp=None):
marker = Marker()
if stamp is None:
marker.header.stamp = rospy.Time.now()
else:
marker.header.stamp = stamp
marker.header.frame_id = frame_id
marker.ns = ns
marker.id = id
marker.type = Marker.SPHERE_LIST
marker.action = Marker.ADD
for i in range(len(points)):
marker.points.append(points[i])
marker.scale.x = size[0]
marker.scale.y = size[1]
marker.scale.z = size[2]
marker.color.r = rgba[0]
marker.color.g = rgba[1]
marker.color.b = rgba[2]
marker.color.a = rgba[3]
if lifetime is None:
marker.lifetime = rospy.Duration()
else:
marker.lifetime = rospy.Duration(lifetime)
return marker
def arrow_marker(position, direction, shaft_radius=0.02, head_radius=0.04, rgba=[1.0, 1.0, 1.0, 1.0],
ns="", id=0, frame_id="", lifetime=None, stamp=None):
marker = Marker()
if stamp is None:
marker.header.stamp = rospy.Time.now()
else:
marker.header.stamp = stamp
marker.header.frame_id = frame_id
marker.ns = ns
marker.id = id
marker.type = Marker.ARROW
marker.action = Marker.ADD
#marker.pose.position.x = position[0]
#marker.pose.position.y = position[1]
#marker.pose.position.z = position[2]
marker.points = [Point(*tuple(position)), Point(*tuple(position+direction*1.0))]
marker.scale.x = shaft_radius
marker.scale.y = head_radius
marker.scale.z = 1.0
marker.color.r = rgba[0]
marker.color.g = rgba[1]
marker.color.b = rgba[2]
marker.color.a = rgba[3]
if lifetime is None:
marker.lifetime = rospy.Duration()
else:
marker.lifetime = rospy.Duration(lifetime)
return marker
def ellipsoid_marker(position, size=[0.1, 0.1, 0.1], rgba=[1.0, 1.0, 1.0, 1.0],
ns="", id=0, frame_id="", lifetime=None, stamp=None):
marker = Marker()
if stamp is None:
marker.header.stamp = rospy.Time.now()
else:
marker.header.stamp = stamp
marker.header.frame_id = frame_id
marker.ns = ns
marker.id = id
marker.type = Marker.SPHERE
marker.action = Marker.ADD
marker.pose.position.x = position[0]
marker.pose.position.y = position[1]
marker.pose.position.z = position[2]
marker.scale.x = size[0]
marker.scale.y = size[1]
marker.scale.z = size[2]
marker.color.r = rgba[0]
marker.color.g = rgba[1]
marker.color.b = rgba[2]
marker.color.a = rgba[3]
if lifetime is None:
marker.lifetime = rospy.Duration()
else:
marker.lifetime = rospy.Duration(lifetime)
return marker
def poseStampedToLabeledSphereMarker(cls, psdata, label, fmt="{label} %Y-%m-%d %H:%M:%S", zoffset=0.05):
"[poseStamped, meta] -> sphereMarker"
ps, meta = psdata
res = []
h = Header()
h.stamp = rospy.Time.now()
h.frame_id = ps.header.frame_id
sphere = Marker(type=Marker.SPHERE,
action=Marker.ADD,
header=h,
id=cls.marker_id)
sphere.scale.x = 0.07
sphere.scale.y = 0.07
sphere.scale.z = 0.07
sphere.pose = ps.pose
sphere.color = ColorRGBA(1.0,0,0,1.0)
sphere.ns = "db_play"
sphere.lifetime = rospy.Time(3)
cls.marker_id += 1
res.append(sphere)
text = Marker(type=Marker.TEXT_VIEW_FACING,
action=Marker.ADD,
header=h,
id=cls.marker_id)
text.scale.z = 0.1
text.pose = deepcopy(ps.pose)
text.pose.position.z += zoffset
text.color = ColorRGBA(1.0,1.0,1.0,1.0)
text.text = meta["inserted_at"].strftime(fmt).format(label=label)
text.ns = "db_play_text"
text.lifetime = rospy.Time(300)
cls.marker_id += 1
res.append(text)
return res
def mesh_marker(position, orientation=None, mesh_resource="", rgba=[1.0, 1.0, 1.0, 1.0],
ns="", id=0, frame_id="", lifetime=None, stamp=None):
marker = Marker()
if stamp is None:
marker.header.stamp = rospy.Time.now()
else:
marker.header.stamp = stamp
marker.header.frame_id = frame_id
marker.ns = ns
marker.id = id
marker.type = Marker.MESH_RESOURCE
marker.mesh_resource = mesh_resource
marker.action = Marker.ADD
marker.pose.position.x = position[0]
marker.pose.position.y = position[1]
marker.pose.position.z = position[2]
if orientation is not None:
marker.pose.orientation.x = orientation[0]
marker.pose.orientation.y = orientation[1]
marker.pose.orientation.z = orientation[2]
marker.pose.orientation.w = orientation[3]
#marker.points = [Point(*tuple(position)), Point(*tuple(position+direction*1.0))]
marker.scale.x = 1.0
marker.scale.y = 1.0
marker.scale.z = 1.0
marker.color.r = rgba[0]
marker.color.g = rgba[1]
marker.color.b = rgba[2]
marker.color.a = rgba[3]
if lifetime is None:
marker.lifetime = rospy.Duration()
else:
marker.lifetime = rospy.Duration(lifetime)
return marker
def publish_cluster(publisher, points, frame_id, namespace, cluster_id):
"""Publishes a marker representing a cluster.
The x and y arguments specify the center of the target.
Args:
publisher: A visualization_msgs/Marker publisher
points: A list of geometry_msgs/Point
frame_id: The coordinate frame in which to interpret the points.
namespace: string, a unique name for a group of clusters.
cluster_id: int, a unique number for this cluster in the given namespace.
"""
marker = Marker()
# TODO(jstn): Once the point clouds have the correct frame_id,
# use them here.
marker.header.frame_id = frame_id
marker.header.stamp = rospy.Time().now()
marker.ns = namespace
marker.id = 2 * cluster_id
marker.type = Marker.POINTS
marker.action = Marker.ADD
marker.color.r = random.random()
marker.color.g = random.random()
marker.color.b = random.random()
marker.color.a = 0.5
marker.points = points
marker.scale.x = 0.002
marker.scale.y = 0.002
marker.lifetime = rospy.Duration()
center = [0, 0, 0]
for point in points:
center[0] += point.x
center[1] += point.y
center[2] += point.z
center[0] /= len(points)
center[1] /= len(points)
center[2] /= len(points)
text_marker = Marker()
text_marker.header.frame_id = frame_id
text_marker.header.stamp = rospy.Time().now()
text_marker.ns = namespace
text_marker.id = 2 * cluster_id + 1
text_marker.type = Marker.TEXT_VIEW_FACING
text_marker.action = Marker.ADD
text_marker.pose.position.x = center[0] - 0.1
text_marker.pose.position.y = center[1]
text_marker.pose.position.z = center[2]
text_marker.color.r = 1
text_marker.color.g = 1
text_marker.color.b = 1
text_marker.color.a = 1
text_marker.scale.z = 0.05
text_marker.text = '{}'.format(cluster_id)
text_marker.lifetime = rospy.Duration()
_publish(publisher, marker, "cluster")
_publish(publisher, text_marker, "text_marker")
return marker
def visualize_cluster(self, cluster, label=None):
points = pc2.read_points(cluster.pointcloud, skip_nans=True)
point_list = [Point(x=x, y=y-0.3, z=z) for x, y, z, rgb in points]
if len(point_list) == 0:
rospy.logwarn('Point list was of size 0, skipping.')
return
marker_id = len(self._current_markers)
marker = Marker()
marker.header.frame_id = 'map'
marker.header.stamp = rospy.Time().now()
marker.ns = 'clusters'
marker.id = marker_id
marker.type = Marker.POINTS
marker.action = Marker.ADD
marker.color.r = random.random()
marker.color.g = random.random()
marker.color.b = random.random()
marker.color.a = 0.5 + random.random()
marker.points = point_list
marker.scale.x = 0.002
marker.scale.y = 0.002
marker.lifetime = rospy.Duration()
self.visualize_marker(marker)
if label is not None:
center = [0, 0, 0]
for point in point_list:
center[0] += point.x
center[1] += point.y
center[2] += point.z
center[0] /= len(point_list)
center[1] /= len(point_list)
center[2] /= len(point_list)
text_marker = Marker()
text_marker.header.frame_id = 'map'
text_marker.header.stamp = rospy.Time().now()
text_marker.ns = 'labels'
text_marker.id = marker_id + 1
text_marker.type = Marker.TEXT_VIEW_FACING
text_marker.action = Marker.ADD
text_marker.pose.position.x = center[1] - 0.05
text_marker.pose.position.y = center[1]
text_marker.pose.position.z = center[2]
text_marker.color.r = 1
text_marker.color.g = 1
text_marker.color.b = 1
text_marker.color.a = 1
text_marker.scale.z = 0.05
text_marker.text = label
text_marker.lifetime = rospy.Duration()
self.visualize_marker(text_marker)
def transformStampedArrayToLabeledLineStripMarker(cls, tsdata_lst, fmt="%Y-%m-%d %H:%M:%S", zoffset=0.05, label_downsample=1, discrete=False):
"[[transformStamped, meta],...] -> LineStrip / String"
res = []
# make line strip
points = []
colors = []
t_first = tsdata_lst[0][0]
prev_t = t_first.transform.translation
for ts, _ in tsdata_lst:
t = ts.transform.translation
dist = distanceOfVector3(prev_t, t) * 0.65
rgb = colorsys.hsv_to_rgb(dist, 1.0, 1.0)
points.append(Point(x=t.x, y=t.y, z=t.z))
colors.append(ColorRGBA(rgb[0],rgb[1],rgb[2],1.0))
prev_t = t
h = Header()
h.stamp = rospy.Time(0) #rospy.Time.now()
h.frame_id = "eng2" #t_first.child_frame_id
if discrete:
m_type = Marker.POINTS
else:
m_type = Marker.LINE_STRIP
m = Marker(type=m_type,
action=Marker.ADD,
header=h,
id=cls.marker_id)
cls.marker_id += 1
m.scale.x = 0.1
m.scale.y = 0.1
m.points = points
m.colors = colors
m.ns = "labeled_line"
m.lifetime = rospy.Time(3000)
res.append(m)
for t, t_meta in tsdata_lst[::label_downsample]:
text = Marker(type=Marker.TEXT_VIEW_FACING,
action=Marker.ADD,
header=h,
id=cls.marker_id)
cls.marker_id += 1
text.scale.z = 0.1
text.pose = T.poseFromTransform(t.transform)
text.pose.position.z += zoffset
text.color = ColorRGBA(0.0,0.0,1.0,1.0)
text.text = t_meta["inserted_at"].strftime(fmt)
text.ns = "labeled_line_text"
text.lifetime = rospy.Time(3000)
res.append(text)
return res
def 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 pub_arrow(self, pos1, pos2, color, mag_force):
marker = Marker()
marker.header.frame_id = self.frame
marker.header.stamp = rospy.Time.now()
marker.ns = "basic_shapes"
marker.id = 99999
marker.type = Marker.ARROW
marker.action = Marker.ADD
pt1 = Point()
pt2 = Point()
pt1.x = pos1[0,0]
pt1.y = pos1[1,0]
pt1.z = pos1[2,0]
pt2.x = pos2[0,0]
pt2.y = pos2[1,0]
pt2.z = pos2[2,0]
marker.points.append(pt1)
marker.points.append(pt2)
marker.scale.x = 0.05
marker.scale.y = 0.1
#marker.scale.z = scale[2]
marker.color.r = color[0]
marker.color.g = color[1]
marker.color.b = color[2]
marker.color.a = color[3]
marker.lifetime = rospy.Duration()
marker.text = mag_force
self.pub.publish(marker)
def to_marker(self):
"""
:return: a marker representing the map.
:type: Marker
"""
marker = Marker()
marker.type = Marker.CUBE_LIST
for x in range(0, len(self.field)):
for y in range(0, len(self.field[0])):
marker.header.frame_id = '/odom_combined'
marker.ns = 'suturo_planning/map'
marker.id = 23
marker.action = Marker.ADD
(x_i, y_i) = self.index_to_coordinates(x, y)
marker.pose.position.x = 0
marker.pose.position.y = 0
marker.pose.position.z = 0
marker.pose.orientation.w = 1
marker.scale.x = self.cell_size
marker.scale.y = self.cell_size
marker.scale.z = 0.01
p = Point()
p.x = x_i
p.y = y_i
marker.points.append(p)
c = self.get_cell_by_index(x, y)
marker.colors.append(self.__cell_to_color(c))
marker.lifetime = rospy.Time(0)
return marker
def __publish_marker_for_object(self, object, txt):
marker = Marker()
marker.header.frame_id = object.object.header.frame_id
marker.header.stamp = rospy.Time(0)
marker.ns = "scene_classifier_marker"
self.marker_id = self.marker_id + 1
marker.id = self.marker_id
marker.type = Marker.TEXT_VIEW_FACING
marker.action = Marker.ADD
marker.pose.position.x = object.c_centroid.x
marker.pose.position.y = object.c_centroid.y
marker.pose.position.z = object.c_centroid.z
marker.pose.orientation.x = 0.0
marker.pose.orientation.y = 0.0
marker.pose.orientation.z = 0.0
marker.pose.orientation.w = 1.0
marker.scale.x = 0.5
marker.scale.y = 0.1
marker.scale.z = 0.1
marker.color.a = 1.0
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0
marker.text = txt
marker.lifetime = rospy.Duration.from_sec(10)
self.marker_publisher.publish(marker)
def publish_markerArray(publisher, points, rgba=(1, 0, 0, 1), shape=Marker.CUBE, duration=rospy.Duration(360), ns='ns'):
#points is expected to be a list of tuples (x,y)
#It's recommended that the publisher is created with latch=True
_id = 0
ma = MarkerArray()
for p in points:
m = Marker()
m.header.frame_id = '/map'
m.header.stamp = rospy.get_rostime()
m.ns = ns
m.id = _id
m.type = shape
m.action = m.ADD
m.pose.position.x = p[0]
m.pose.position.y = p[1]
m.pose.orientation.w = 1
m.scale.x = 0.5
m.scale.y = 0.5
m.scale.z = 0.5
m.color.r = rgba[0]
m.color.g = rgba[1]
m.color.b = rgba[2]
m.color.a = rgba[3]
m.lifetime = duration
ma.markers.append(m)
_id += 1
publisher.publish(ma)
def draw_box_marker(
self, id, x, y, z, dimx, dimy, dimz, color=(1.0, 1.0, 1.0, 0.0), duration=0.0, frame_id="base_link"
):
marker = Marker()
marker.header.stamp = rospy.Time.now()
marker.ns = "object_detector"
marker.type = Marker.CUBE
marker.action = marker.ADD
marker.id = id
marker.header.frame_id = "base_link"
marker.pose.position.x = x
marker.pose.position.y = y
marker.pose.position.z = z
marker.pose.position.x = x
marker.pose.orientation.x = 0.0
marker.pose.orientation.y = 0.0
marker.pose.orientation.z = 0.0
marker.pose.orientation.w = 1.0
marker.scale.x = dimx
marker.scale.y = dimy
marker.scale.z = dimz
marker.color.a = color[0]
marker.color.r = color[1]
marker.color.g = color[2]
marker.color.b = color[3]
marker.lifetime = rospy.Duration(duration)
self.box_drawer.publish(marker)
def set_position_callback(self,marker,joy): print self.position_marker.ns print joy.buttons[3] == 1 print marker.ns == "PEOPLE" if (self.position_marker.ns == "NONE" and joy.buttons[3] == 1 and marker.ns == "PEOPLE"): self.position_marker = marker print "set position" ref_marker = Marker() ref_marker.header.frame_id = "base_footprint" ref_marker.header.stamp = rospy.get_rostime() ref_marker.ns = "robot" ref_marker.type = 9 ref_marker.action = 0 ref_marker.pose.position.x = self.position_marker.pose.position.x ref_marker.pose.position.y = self.position_marker.pose.position.y ref_marker.pose.position.z = self.position_marker.pose.position.z ref_marker.scale.x = .25 ref_marker.scale.y = .25 ref_marker.scale.z = .25 ref_marker.color.r = 1.0 ref_marker.color.g = 0.0 ref_marker.color.a = 1.0 ref_marker.lifetime = rospy.Duration(0) self.ref_viz_pub.publish(ref_marker) else: pass
def draw_bounding_box(self, id, box_msg, color=(1.0, 1.0, 0.0, 0.0), duration=0.0):
"""
Draws a bounding box as detectd by detect_bounding_box.
Parameters:
box_msg is a FindClusterBoundingBoxResponse msg.
color: a quadruple with alpha, r,g,b
duration: how long should the bounding box last. 0 means forever.
"""
marker = Marker()
marker.header.stamp = rospy.Time.now()
marker.ns = "object_detector"
marker.type = Marker.CUBE
marker.action = marker.ADD
marker.id = id
marker.header.frame_id = box_msg.pose.header.frame_id
marker.pose = box_msg.pose.pose
marker.scale.x = box_msg.box_dims.x
marker.scale.y = box_msg.box_dims.y
marker.scale.z = box_msg.box_dims.z
marker.color.a = color[0]
marker.color.r = color[1]
marker.color.g = color[2]
marker.color.b = color[3]
marker.lifetime = rospy.Duration(duration)
self.box_drawer.publish(marker)
def _makeMarker(self,tf,num):
marker = Marker()
marker.header.frame_id = "map"
marker.header.stamp = rospy.Time.now()
marker.ns = "kinect_challenge"
marker.id = num
marker.type= marker.ARROW
marker.action = marker.ADD
marker.pose.position.x = tf[0][0]
marker.pose.position.y = tf[0][1]
marker.pose.position.z = 0.5
marker.pose.orientation.x = tf[1][0]
marker.pose.orientation.y = tf[1][1]
marker.pose.orientation.z = tf[1][2]
marker.pose.orientation.w = tf[1][3]
marker.scale.x = 0.5
marker.scale.y = 0.05
marker.scale.z = 0.05
marker.color.r = 1.0
marker.color.g = 0.5
marker.color.b = 0.3
marker.color.a = 1.0
marker.lifetime = rospy.Duration()
return marker
def get_marker(self, line_seg, frame_id, scale=0.3):
p0, p1 = line_seg
v = np.asarray([p1.position.x - p0.position.x,
p1.position.y - p0.position.y,
p1.position.z - p0.position.z])
vnorm = np.linalg.norm(v)
if vnorm > 0.0:
v /= np.linalg.norm(v)
p2 = copy.deepcopy(p0)
p2.position.x += scale * v[0]
p2.position.y += scale * v[1]
p2.position.z += scale * v[2]
m = Marker()
m.header.stamp = rospy.Time.now()
m.header.frame_id = frame_id
m.ns = "beam"
m.type = Marker.ARROW
m.action = Marker.MODIFY
m.lifetime = rospy.Duration(1)
m.points = [p0.position, p1.position]
m.scale.x = 0.02
m.scale.y = 0.04
m.color.r = 1.0
m.color.a = 1.0
return m
def __init__(self):
self.layout = rospy.get_param('/thruster_layout/thrusters') # Add thruster layout from ROS param set by mapper
assert self.layout is not None, 'Could not load thruster layout from ROS param'
'''
Create MarkerArray with an arrow marker for each thruster at index node_id.
The position of the marker is as specified in the layout, but the length of the arrow
will be set at each thrust callback.
'''
self.markers = MarkerArray()
for i in xrange(len(self.layout)):
# Initialize each marker (color, scale, namespace, frame)
m = Marker()
m.header.frame_id = '/base_link'
m.type = Marker.ARROW
m.ns = 'thrusters'
m.color.a = 0.8
m.scale.x = 0.01 # Shaft diameter
m.scale.y = 0.05 # Head diameter
m.action = Marker.DELETE
m.lifetime = rospy.Duration(0.1)
self.markers.markers.append(m)
for key, layout in self.layout.iteritems():
# Set position and id of marker from thruster layout
idx = layout['node_id']
pt = numpy_to_point(layout['position'])
self.markers.markers[idx].points.append(pt)
self.markers.markers[idx].points.append(pt)
self.markers.markers[idx].id = idx
# Create publisher for marker and subscribe to thrust
self.pub = rospy.Publisher('/thrusters/markers', MarkerArray, queue_size=5)
self.thrust_sub = rospy.Subscriber('/thrusters/thrust', Thrust, self.thrust_cb, queue_size=5)
def to_marker_array(self):
markers = []
for x in range(0, len(self.field)):
for y in range(0, len(self.field[0])):
marker = Marker()
marker.header.stamp = rospy.get_rostime()
marker.header.frame_id = '/odom_combined'
marker.ns = 'suturo_planning/search_grid'
marker.id = x + (y * len(self.field))
marker.action = Marker.ADD
marker.type = Marker.CUBE
marker.pose.position.x = self.coordinates[x, y][0]
marker.pose.position.y = self.coordinates[x, y][1]
marker.pose.position.z = self.coordinates[x, y][2]
marker.pose.orientation.w = 1
marker.scale.x = 2.0 / len(self.coordinates)
marker.scale.y = marker.scale.x
marker.scale.z = 0.01
marker.color = SearchGrid._get_color_for_value(self.field[x][y])
marker.lifetime = rospy.Time(0)
markers.append(marker)
return MarkerArray(markers)
def _create_add_line(self, points, edges, point_group_name, id, red, green, blue):
all_points = []
for i, end_points in enumerate(edges):
for endpoint_index in end_points:
pt1, pt2 = Point(), Point()
pt1.x, pt1.y, pt1.z = points[i][0], points[i][1], points[i][2]
all_points.append(pt1)
pt2.x, pt2.y, pt2.z = points[endpoint_index][0], points[endpoint_index][1], points[endpoint_index][2]
all_points.append(pt2)
marker = Marker()
marker.header.frame_id = "odom_combined"
marker.header.stamp = rospy.get_rostime()
marker.ns = point_group_name
marker.id = id
marker.type = Marker.LINE_STRIP
marker.action = Marker.ADD
marker.points = all_points
marker.pose.position.x = 0.0
marker.pose.position.y = 0.0
marker.pose.position.z = 0.0
marker.pose.orientation.x = 0.0
marker.pose.orientation.y = 0.0
marker.pose.orientation.z = 0.0
marker.pose.orientation.w = 1.0
marker.scale.x = 0.003
marker.color.r = red
marker.color.g = green
marker.color.b = blue
marker.color.a = 1.0
marker.lifetime = rospy.Duration()
return 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_add_point(self, coords, point_group_name, id, red, green, blue, point_radius):
marker = Marker()
marker.header.frame_id = "odom_combined"
marker.header.stamp = rospy.get_rostime()
marker.ns = point_group_name
marker.id = id
marker.type = Marker.SPHERE
marker.action = Marker.ADD
marker.pose.position.x = coords[0]
marker.pose.position.y = coords[1]
marker.pose.position.z = coords[2]
marker.pose.orientation.x = 0.0
marker.pose.orientation.y = 0.0
marker.pose.orientation.z = 0.0
marker.pose.orientation.w = 1.0
marker.scale.x = point_radius
marker.scale.y = point_radius
marker.scale.z = point_radius
marker.color.r = red
marker.color.g = green
marker.color.b = blue
marker.color.a = 1.0
marker.lifetime = rospy.Duration()
return 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 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 drawlandmark(pub):
global pt_count
points_tag = Marker()
points_tag.header.frame_id = "/base_link"
points_tag.action = Marker.ADD
points_tag.header.stamp = rospy.Time.now()
points_tag.lifetime = rospy.Time(0)
points_tag.scale.x = 0.1;
points_tag.scale.y = 0.1;
points_tag.scale.z = 0.1;
points_tag.color.a = 1.0;
points_tag.color.r = 1.0;
points_tag.color.g = 0.0;
points_tag.color.b = 0.0;
points_tag.ns = "pts_line"
pt_count+=1
points_tag.id = pt_count
points_tag.type = Marker.POINTS
for x in range(6):
p1 = Point()
p1.x = tagnum_x[x]/100
p1.y = tagnum_y[x]/100
p1.z = 0
points_tag.points.append(p1)
pub.publish(points_tag)
def pub_body(self, pos, quat, scale, color, num, shape, text = ''):
marker = Marker()
marker.header.frame_id = self.frame
marker.header.stamp = rospy.Time.now()
marker.ns = "basic_shapes"
marker.id = num
marker.type = shape
marker.action = Marker.ADD
marker.pose.position.x = pos[0]
marker.pose.position.y = pos[1]
marker.pose.position.z = pos[2]
marker.pose.orientation.x = quat[0]
marker.pose.orientation.y = quat[1]
marker.pose.orientation.z = quat[2]
marker.pose.orientation.w = quat[3]
marker.scale.x = scale[0]
marker.scale.y = scale[1]
marker.scale.z = scale[2]
marker.color.r = color[0]
marker.color.g = color[1]
marker.color.b = color[2]
marker.color.a = color[3]
marker.lifetime = rospy.Duration()
marker.text = text
self.pub.publish(marker)
def publish_marker(cls, x_cord, y_cord, mid):
new_marker = Marker(type = Marker.CUBE, action=Marker.ADD)
new_marker.header.frame_id = "/base_laser_front_link"
new_marker.header.stamp = rospy.Time.now()
new_marker.ns = "basic_shapes"
new_marker.id = mid
new_marker.pose.position.x = x_cord
new_marker.pose.position.y = y_cord
new_marker.pose.position.z = 0.0
#pointxyz
new_marker.pose.orientation.x = 0
new_marker.pose.orientation.y = 0
new_marker.pose.orientation.z = 0.0
new_marker.pose.orientation.w = 1
new_marker.scale.x = 0.005
new_marker.scale.y = 0.005
new_marker.scale.z = 0.005
if mid == 0:
new_marker.color.r = 1
elif mid == 5:
new_marker.color.g = 1
elif mid == 10:
new_marker.color.b = 1
new_marker.color.a = 1
new_marker.text = "marker"
new_marker.lifetime = rospy.Duration(1)
SmachGlobalData.pub_marker.publish(new_marker)
def draw_table_rviz(self, id, table_msg, color = (1.0, 1.0, 1.0, 0.0), duration = 0.0):
marker = Marker()
marker.header.stamp = rospy.Time.now()
marker.ns = "object_detector"
marker.type = Marker.CUBE
marker.action = marker.ADD
marker.id = id
marker.header.frame_id = table_msg.pose.header.frame_id
marker.pose = table_msg.pose.pose
marker.pose.position.x = (table_msg.x_max-table_msg.x_min)/2 \
+ table_msg.x_min
marker.pose.position.y = (table_msg.y_max-table_msg.y_min)/2 \
+ table_msg.y_min
marker.scale.x = table_msg.x_max - table_msg.x_min
marker.scale.y = table_msg.y_max - table_msg.y_min
marker.scale.z = 0.01
marker.color.a = color[0]
marker.color.r = color[1]
marker.color.g = color[2]
marker.color.b = color[3]
marker.lifetime = rospy.Duration(duration)
self.box_drawer.publish(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 _visualize_breadcrumbs(self):
breadcrumbs_msg = Marker()
breadcrumbs_msg.type = Marker.POINTS
breadcrumbs_msg.scale.x = 0.05
breadcrumbs_msg.header.stamp = rospy.get_rostime()
breadcrumbs_msg.header.frame_id = "/map"
breadcrumbs_msg.color.a = 1
breadcrumbs_msg.color.r = 0
breadcrumbs_msg.color.g = 1
breadcrumbs_msg.color.b = 1
breadcrumbs_msg.lifetime = rospy.Time(1.0)
breadcrumbs_msg.id = 0
breadcrumbs_msg.action = Marker.ADD
for crumb in self._breadcrumbs:
breadcrumbs_msg.points.append(
kdl_conversions.kdl_vector_to_point_msg(crumb.pose.frame.p))
self._breadcrumb_pub.publish(breadcrumbs_msg)
def publish_marker(self, poses, quaternion=[0, 0, 0, 1.0], clear=False):
''' erase all and re-append '''
self.array = MarkerArray()
for pose in poses:
m = Marker()
m.lifetime = rospy.Duration.from_sec(0.1)
m.header.frame_id = 'map'
m.type = self.marker_type
m.action = m.ADD
m.scale.x = self.scale[0]
m.scale.y = self.scale[1]
m.scale.z = self.scale[2]
m.color.r = self.rgba[0]
m.color.g = self.rgba[1]
m.color.b = self.rgba[2]
m.color.a = self.rgba[3]
m.pose.orientation.x = quaternion[0]
m.pose.orientation.y = quaternion[1]
m.pose.orientation.z = quaternion[2]
m.pose.orientation.w = quaternion[3]
m.pose.position.x = pose[0]
m.pose.position.y = pose[1]
m.pose.position.z = 0.0
self.array.markers.append(m)
# ''' add new marker and remove the old one '''
# self.MARKER_MAX = len(poses)
# if self.COUNT > self.MARKER_MAX:
# self.array.markers.pop(0)
# self.array.markers.append(m)
# self.COUNT += 1
''' Renumber the marker IDs '''
id = 0
for ma in self.array.markers:
ma.id = id
id += 1
self.marker_pub.publish(self.array)
def rviz_visualization_stl(str):
str1 = 'package://aubo_demo/stl_test/'
str = str1 + str
color = ColorRGBA()
color.r = 1.0
color.g = 0.0
color.b = 0.0
color.a = 1.0
position = Point()
position.x = 0.0
position.y = 0.0
position.z = 0.0
marker = Marker()
marker.header.frame_id = "/world"
marker.type = Marker.MESH_RESOURCE
marker.mesh_resource = "package://aubo_demo/stl_test/stl_document/0015.STL"
# marker.mesh_resource=str
marker.action = Marker.ADD
marker.pose.orientation.w = 1.0
marker.scale.x = 1.0
marker.scale.y = 1.0
marker.scale.z = 1.0
marker.ns = 'arrow'
marker.id = 1
marker.lifetime = rospy.Duration()
marker.color.r = color.r
marker.color.g = color.g
marker.color.b = color.b
marker.color.a = color.a
marker.pose.position.x = position.x
marker.pose.position.y = position.y
marker.pose.position.z = position.z
marker_pub = rospy.Publisher("visualization_marker", Marker, queue_size=10)
rospy.init_node('markers')
rate = rospy.Rate(10)
while not rospy.is_shutdown():
marker_pub.publish(marker)
rate.sleep()
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 delete_callback(self, msg):
print("Deleting last face")
del detected[self.marker_id_counter - 1]
markers2 = MarkerArray()
self.marker_id_counter -= 1
for i, detec in enumerate(detected):
marker = Marker()
marker.header.stamp = rospy.Time.now()
marker.header.frame_id = "map"
marker.pose = detec.pose
marker.type = Marker.CUBE
marker.action = Marker.ADD
marker.frame_locked = False
marker.lifetime = rospy.Duration.from_sec(1)
marker.id = i
marker.scale = Vector3(0.1, 0.1, 0.1)
marker.color = ColorRGBA(1, 0, 0, 1)
markers2.markers.append(marker)
self.markers = markers2
def construct_landmark(lm_new, lm_pub):
pos = curr_global_pose.position
#tfPub = tf2_ros.StaticTransformBroadcaster()
t = TransformStamped()
t.header.stamp = rospy.Time.now()
t.header.frame_id = "map"
t.child_frame_id = lm_new
t.transform.translation.x = pos.y
t.transform.translation.y = pos.x
t.transform.translation.z = 0
t.transform.rotation.x = 0.0
t.transform.rotation.y = 0.0
t.transform.rotation.z = 0.0
t.transform.rotation.w = 1
#tfPub.sendTransform(t)
if already_known(lm_known, lm_new, pos):
return False
else:
lm_known[lm_new] = pos
# make and publish the marker
mrk = Marker()
mrk.header.frame_id = lm_new
mrk.header.stamp = rospy.Time.now()
mrk.ns = "/landmarkers"
mrk.id = int(random.random()*1000)
mrk.type = 9
mrk.action = 0
mrk.pose = globalPose
mrk.pose.orientation.w = 1
mrk.scale.x = 5
mrk.scale.y = 0
mrk.scale.z = 0
mrk.color.r = 0.2
mrk.color.g = 0.4
mrk.color.b = 1.0
mrk.color.a = 0.7
mrk.lifetime= rospy.Duration(0)
mrk.frame_locked = True
mrk.text = lm_new
lm_pub.publish(mrk)
return True
def _publish(self, time):
assert time != rospy.Time(0)
out_msg = ObstacleArray()
out_msg.header.stamp = time
out_msg.header.frame_id = 'map'
marker_array = MarkerArray()
for f in self._filters:
obstacle = f['filter'].get_state(time)
out_msg.obstacles.append(obstacle)
marker = Marker()
marker.header.stamp = time
marker.header.frame_id = 'map'
marker.ns = 'obstacles'
marker.id = uuid.UUID('{'+obstacle.odom.child_frame_id[len('obstacle-'):-len('/base_link')]+'}').int % (2**31)
marker.type = Marker.CYLINDER
marker.action = Marker.ADD
marker.pose.orientation.w = 1.0
marker.scale.x = obstacle.pipe_radius
marker.scale.y = obstacle.pipe_radius
marker.scale.z = obstacle.pipe_height
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0
marker.color.a = 1.0
marker.lifetime = rospy.Duration(0.2)
marker.pose.position.x = obstacle.odom.pose.pose.position.x
marker.pose.position.y = obstacle.odom.pose.pose.position.y
marker.pose.position.z = obstacle.pipe_height / 2.
marker_array.markers.append(marker)
self._marker_pub.publish(marker_array)
self._pub.publish(out_msg)
def append_marker(pos, color):
marker = Marker()
marker.header.frame_id = "/simmap"
marker.type = marker.SPHERE
marker.id = pos[0] * pos[1]
marker.lifetime = rospy.Duration(0)
marker.action = marker.ADD
marker.scale.x = 0.2
marker.scale.y = 0.2
marker.scale.z = 0.2
marker.color.a = 1.0
marker.color.r = color[0]
marker.color.g = color[1]
marker.color.b = color[2]
marker.pose.orientation.w = 1.0
marker.pose.position.x = pos[0]
marker.pose.position.y = pos[1]
marker.pose.position.z = 0
buoy_array.markers.append(marker)
def place_marker_pose(posestamped, pub, _id=0, _type=Marker.CUBE, ns='basic_shapes',\
r=0, g=1, b=0, a=1, xscale=.03, yscale=.03, zscale=.03,\
orientation=Quaternion(0,0,0,1), text=''):
marker = Marker(type=_type, action=Marker.ADD)
marker.ns = ns
marker.header.frame_id = posestamped.header.frame_id
marker.header.stamp = rospy.Time.now()
marker.pose = posestamped.pose
marker.scale.x = xscale
marker.scale.y = yscale
marker.scale.z = zscale
marker.color.r = r
marker.color.g = g
marker.color.b = b
marker.color.a = a
marker.id = _id
marker.text = text
marker.lifetime = rospy.Duration()
pub.publish(marker)
def publish_grid(self):
marker = Marker()
marker.header.frame_id = 'pegasus_map'
marker.ns = 'unvisited'
marker.id = 0
marker.type = marker.POINTS
marker.action = marker.ADD
marker.pose.orientation.w = 1
marker.points = self.unvisited_points
marker.lifetime = rospy.Duration()
marker.scale.x = 5
marker.scale.y = 5
marker.scale.z = 5
marker.color.g = 1
marker.color.r = 1
marker.color.b = 0
marker.color.a = 1
self.publisher_marker.publish(marker)
def evaluateObject_sim(objectData):
# evaluate the object classification
marker = Marker()
r, g, b, typ = evaluateColor(
evaluateClassification(objectData.classification))
#assign frame id
marker.header.frame_id = "/ego"
# assign the geometric shape of the object e.g. Cube
marker.type = typ
# assign the marker option ADD
marker.action = marker.ADD
# assign the size of the object
marker.scale.x = objectData.dimension.length
marker.scale.y = objectData.dimension.width
marker.scale.z = objectData.dimension.height
# assign the color of the object to the marker
marker.color.a = 1.0 # alpha Channel
marker.color.r = r # red
marker.color.g = g # green
marker.color.b = b # blue
# Convert RPY to quaternion
q = quaternion_from_euler(0, 0, np.deg2rad(objectData.geometric.yaw))
marker.pose.orientation.x = q[0]
marker.pose.orientation.y = q[1]
marker.pose.orientation.z = q[2]
marker.pose.orientation.w = q[3]
# Evaluate Position of each Object
marker.pose.position.x = car_ego_x + objectData.geometric.x
marker.pose.position.y = car_ego_y + objectData.geometric.y * (
-1) # change the coordinate-system
marker.pose.position.z = objectData.dimension.height / 2
# Lifetime of the object, after 0.1 second the object will disappear
marker.lifetime = rospy.Duration(0.1)
return marker
def set_up_wheel(self):
self.markers_wheel = MarkerArray()
it_id = 0
wheel_part = Marker()
wheel_part.header.frame_id = "/robotiq_force_torque_frame_id"
wheel_part.ns = "ur5"
wheel_part.id = it_id
wheel_part.type = Marker().CYLINDER
wheel_part.action = Marker().ADD
# wheel_part.scale = Vector3(0.08, 0.08, 0.035)
wheel_part.scale = Vector3(0.08, 0.08, 0.06)
wheel_part.color = ColorRGBA(100. / 265, 100. / 265, 100. / 265, 1)
wheel_part.pose.orientation = Quaternion(0, 0, 0, 1)
wheel_part.lifetime = rospy.Duration.from_sec(0)
wheel_part.frame_locked = True
self.markers_wheel.markers.append(wheel_part)
dz = wheel_part.scale.z / 2
wheel_part.pose.position = Point(0, 0, dz)
dz += wheel_part.scale.z / 2
it_id += 1
wheel_part = copy.deepcopy(wheel_part)
wheel_part.id = it_id
wheel_part.scale = Vector3(0.40, 0.40, 0.001)
wheel_part.color = ColorRGBA(70. / 265, 70. / 265, 70. / 265, 1.)
dz += wheel_part.scale.z / 2
wheel_part.pose.position = Point(0, 0, dz)
dz += wheel_part.scale.z / 2
self.markers_wheel.markers.append(wheel_part)
it_id += 1
wheel_part = copy.deepcopy(wheel_part)
wheel_part.id = it_id
wheel_part.scale = Vector3(0.40, 0.40, 0.014)
wheel_part.color = ColorRGBA(130. / 265, 130. / 265, 130. / 265, 1.)
dz += wheel_part.scale.z / 2
wheel_part.pose.position = Point(0, 0, dz)
dz += wheel_part.scale.z / 2
self.markers_wheel.markers.append(wheel_part)
def footPoseToMarker(pose,leg):
m = Marker()
m.header=Header()
m.header.frame_id = strings.map_frame
m.header.stamp = rospy.Time.now()
m.ns = ' '
m.type = Marker.CUBE
m.action = Marker.ADD
cos_theta = math.cos(pose.theta)
sin_theta = math.sin(pose.theta)
x_shift = cos_theta * foot_origin_shift_x - sin_theta * foot_origin_shift_y
if leg == strings.left:
y_shift = sin_theta * foot_origin_shift_x + cos_theta * foot_origin_shift_y
else:
y_shift = sin_theta * foot_origin_shift_x - cos_theta * foot_origin_shift_y
m.pose.position.x = pose.x + x_shift
m.pose.position.y = pose.y + y_shift
m.pose.position.z = footsize_z / 2.0
q=tf.transformations.quaternion_from_euler(0.0, 0.0, pose.theta)
m.pose.orientation.x = q[0]
m.pose.orientation.y = q[1]
m.pose.orientation.z = q[2]
m.pose.orientation.w = q[3]
m.scale.x = footsize_x
m.scale.y = footsize_y
m.scale.z = footsize_z
if leg == strings.right:
m.color.r = 0.0
m.color.g = 1.0
else:
m.color.r = 1.0
m.color.g = 0.0
m.color.b = 0.0
m.color.a = 0.6
m.lifetime = rospy.Duration()
return m
def ref_callback(self, msg):
self._ref[0, 0] = msg.x
self._ref[1, 0] = msg.xdot1
self._ref[2, 0] = msg.xdot2
self._ref[3, 0] = msg.xdot3
self._ref[4, 0] = msg.y
self._ref[5, 0] = msg.ydot1
self._ref[6, 0] = msg.ydot2
self._ref[7, 0] = msg.ydot3
self._ref[8, 0] = msg.z
self._ref[9, 0] = msg.zdot1
self._ref[10, 0] = msg.zdot2
self._ref[11, 0] = msg.zdot3
self._ref[12, 0] = msg.psi
self._ref[13, 0] = msg.psidot1
# Can mess with the viz later.
marker = Marker()
marker.header.frame_id = self._fixed_frame
marker.header.stamp = rospy.Time.now()
marker.ns = "reference"
marker.id = self._ref_viz_last_id
marker.type = Marker.SPHERE
marker.action = Marker.ADD
marker.pose.position.x = msg.x
marker.pose.position.y = msg.y
marker.pose.position.z = msg.z
marker.pose.orientation.x = 0.0
marker.pose.orientation.y = 0.0
marker.pose.orientation.z = 0.0
marker.pose.orientation.w = 1.0
marker.scale.x = 0.1
marker.scale.y = 0.1
marker.scale.z = 0.1
marker.color.a = 1.0
marker.color.r = 0.0
marker.color.g = 0.75
marker.color.b = 1.0
marker.lifetime = rospy.Duration(5)
self._ref_viz_pub.publish(marker)
self._ref_viz_last_id += 1
def publish_trajectory_markers(self, duration):
"""
Publishes markers to visualize the current trajectory.
Paremeters:
duration: how long should the markers visualization last. If this
function is called from a loop they last at least the loop rate.
"""
if len(self.trajectory.poses) == 0:
return
msg = MarkerArray()
marker_id = 0
#creating the path connecting the axes
path = Marker()
path.header.frame_id = self.trajectory.header.frame_id
path.ns = "path"
path.action = Marker.ADD
path.type = Marker.LINE_STRIP
path.lifetime = rospy.Duration(duration)
path.color.r = 1
path.color.g = 0
path.color.b = 1
path.color.a = 1
path.scale.x = 0.01
path.id = marker_id
marker_id += 1
for pose in self.trajectory.poses:
pos = PoseStamped()
pos.header.frame_id = self.trajectory.header.frame_id
pos.pose = pose
markers = utils.axis_marker(pos, marker_id, "axes")
msg.markers.extend(markers)
path.points.append(pose.position)
marker_id += 3 #3 axes
msg.markers.append(path)
self.visualizer_pub.publish(msg)
def publish_Bottle_Pos_Marker(posbouteille_x, posbouteille_y, id_num, cutted):
global ObjectCut
#setup de l'objet marqueur
rospy.loginfo(" ")
_cmd_pub = rospy.Publisher('bottle', Marker, queue_size=10)
marker_msg = Marker()
marker_msg.header.frame_id = "map"
marker_msg.header.stamp = rospy.Time.now()
marker_msg.ns = "my_point"
marker_msg.id = id_num
#changement de format de marqueur dans le cas d'une bouteille coupee
if cutted:
marker_msg.color.r = 1.0
marker_msg.color.g = 0.0
marker_msg.color.b = 0.0
ObjectCut = False
else:
marker_msg.color.r = 0.0
marker_msg.color.g = 1.0
marker_msg.color.b = 0.0
marker_msg.type = Marker().CUBE
marker_msg.action = Marker().ADD
#placement du marqueur a une distance d du robot dependant de la fonction detection()
marker_msg.pose.position.x = posbouteille_x
marker_msg.pose.position.y = posbouteille_y
marker_msg.pose.position.z = 0.0
#affichage marqueur RVIZ
marker_msg.color.a = 1.0
#definition de la taille du marqueur
marker_msg.scale.x = 0.15
marker_msg.scale.y = 0.15
marker_msg.scale.z = 0.15
marker_msg.lifetime = rospy.Duration(0)
_cmd_pub.publish(marker_msg)
def plot_blocked_edges(self, node_list):
"""
Plot blocked branches in the rrt tree.
"""
tree = MarkerArray()
id = 1
for node in self.node_list:
if node.parent and node.parent.cost == float('Inf'):
# edge between nodes
path = Marker()
path.header.frame_id = "map"
path.header.stamp = rospy.get_rostime()
path.ns = "markers"
path.id = id
id += 1
path.type = path.LINE_STRIP
path.action = path.ADD
path.scale.x = self.rviz_tuning_plt
path.color.a = 1.0
path.color.r = 1.0
path.color.g = 0.0
path.color.b = 0.0
path.lifetime = rospy.Duration()
path.pose.orientation.w = 1.0
p1 = Point()
p1.x = node.parent.x
p1.y = node.parent.y
p1.z = 0.03
path.points.append(p1)
p2 = Point()
p2.x = node.x
p2.y = node.y
p2.z = 0.03
path.points.append(p2)
tree.markers.append(path)
self.pub_blocked_edges.publish(tree)
def set_target_action_marker(action_list, target=False):
goal_marker = Marker()
goal_marker.action = goal_marker.ADD
goal_marker.header.frame_id = "table_gym"
# goal_marker.header.stamp = rospy.get_rostime()
# goal_marker.ns = "goal_marker"
goal_marker.lifetime = rospy.Duration(60)
# goal_marker.id = 0
goal_marker.type = goal_marker.LINE_STRIP
goal_marker.pose.position.x = 0.0
goal_marker.pose.position.y = 0.0
goal_marker.pose.position.z = 0.00
goal_marker.pose.orientation.x = 0.0
goal_marker.pose.orientation.y = 0.0
goal_marker.pose.orientation.z = 0.0
goal_marker.pose.orientation.w = 1.0
goal_marker.scale.x = 0.001
goal_marker.color.r = 1.0
goal_marker.color.g = 0.0
goal_marker.color.b = 0.0
goal_marker.color.a = 1.0
#### if this marker is target action
if target is True:
# goal_marker.id = 100000
goal_marker.color.r = 0.0
goal_marker.color.g = 0.0
goal_marker.color.b = 1.0
goal_marker.scale.x = 0.005
goal_marker.pose.position.z = 0.02
goal_marker.points = []
n = len(action_list)
for i in range(n):
point = geometry_msgs.msg.Point()
point.x = action_list[i][0]
point.y = action_list[i][1]
point.z = 0.0475
goal_marker.points.append(point)
return goal_marker
def PublishTreeArray(self, tree):
markarray = MarkerArray()
for i in range(1,len(tree)):
x_index = tree[i].x
y_index = tree[i].y
parent_index = tree[i].parent
parent_x = tree[parent_index].x
parent_y = tree[parent_index].y
node_x = self.occ_grid[x_index][y_index][1]
node_y = self.occ_grid[x_index][y_index][2]
pnode_x = self.occ_grid[parent_x][parent_y][1]
pnode_y = self.occ_grid[parent_x][parent_y][2]
z = 0.025
points = self.ConstructPointsArray(node_x, node_y, pnode_x, pnode_y, z)
marker = Marker()
marker.ns = "tree"
marker.action = marker.MODIFY
marker.header.frame_id = "/map"
marker.type = marker.LINE_STRIP
marker.points = points
marker.scale.x = 0.05
marker.color.g = 1.0
marker.color.a = 1.0
marker.pose.orientation.x = 0.0
marker.pose.orientation.y = 0.0
marker.pose.orientation.z = 0.0
marker.pose.orientation.w = 1.0
marker.lifetime = rospy.Duration(0.1)
markarray.markers.append(marker)
id = 0
for m in markarray.markers:
m.id = id
id += 1
self.tree_pub.publish(markarray)
def viz_cars(self, relevant_msg, header):
marker_msg = MarkerArray()
count = 0
for obj in relevant_msg.objects:
mk = Marker()
mk.header = header
mk.type = Marker.CUBE
mk.scale.x = 3.7
mk.scale.y = 1.6
mk.scale.z = 0.5
mk.color.a = 1
mk.color.r = 1
mk.color.g = 0
mk.color.b = 0
mk.pose = obj.pose
mk.id = count
mk.lifetime = rospy.Duration(0.05)
marker_msg.markers.append(mk)
count += 1
self.marker_pub.publish(marker_msg)
def publishTrace(time):
global viz_pub
global i
msg = Marker()
msg.header.frame_id = 'manipulator'
msg.header.stamp = rospy.Time.now()
msg.ns = "xd"
i+=1
msg.id = i
msg.lifetime = rospy.Duration(time+3.)
msg.type = Marker.SPHERE
msg.action = Marker.ADD
msg.scale.x = 0.01
msg.scale.y = 0.01
msg.scale.z = 0.01
msg.color.r = 0.
msg.color.g = 0.
msg.color.b = 1.
msg.color.a = 175./255.
viz_pub.publish(msg)
def np_array_to_marker(self, id, p, v=0, a=0):
marker = Marker()
marker.header.frame_id = "map"
marker.type = marker.CUBE
marker.action = marker.ADD
marker.id = id
marker.scale.x = self.resolution
marker.scale.y = self.resolution
marker.scale.z = 0.1
marker.color.r = v / 72.0
marker.color.g = 0
marker.color.b = 0.5
marker.color.a = a
marker.pose.orientation.w = 1.0
marker.pose.position.x = p[0]
marker.pose.position.y = p[1]
marker.pose.position.z = p[2]
marker.lifetime = rospy.Time(10)
return marker
def publishCube(pos, ori, scale, id):
m = Marker()
m.id = id
m.ns = 'table'
m.action = m.ADD
m.type = m.CUBE
m.header.stamp = rospy.Time.now()
m.header.frame_id = frame_name
m.lifetime = rospy.Duration(4.0)
m.color = ColorRGBA(0.6, 0.4, 0.2, 1)
m.pose.position = Point(*pos)
m.pose.orientation = Quaternion(*ori)
m.scale = Vector3(*scale)
pub.publish(m)
def create_marker_point(ns='debug', frame_id='map'):
"""
creation of a rviz marker of type point
:param ns: namespace to use
:param frame_id: reference frame to state
:return:
"""
point_marker = Marker()
point_marker.header.frame_id = frame_id
point_marker.ns = ns
point_marker.id = 1
point_marker.type = Marker.POINTS
point_marker.action = Marker.ADD
point_marker.color.r = 1.0
point_marker.color.a = 1.0
point_marker.scale.x = 0.005
point_marker.scale.y = 0.005
point_marker.scale.z = 0.005
point_marker.lifetime = rospy.Duration(0.1)
return point_marker
def pubMarker(self, pose):
if self.markers:
marker = Marker()
marker.header.frame_id = "odom"
marker.header.stamp = rospy.Time.now()
marker.ns = "wps"
marker.id = self.marker_id
marker.type = Marker.SPHERE
marker.action = Marker.ADD
marker.pose = pose
marker.scale.x = 1
marker.scale.y = 1
marker.scale.z = 1
marker.color.a = 1.0
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0
marker.lifetime = rospy.Duration(60)
self.vis_pub.publish(marker)
self.marker_id += 1
def publishCylinder(pos, ori, scale, color, id): m = Marker() m.id = id m.ns = 'baker' m.action = m.ADD m.type = m.CYLINDER m.header.stamp = rospy.Time.now() m.header.frame_id = frame_name m.lifetime = rospy.Duration(4.0) m.color = ColorRGBA(*color) m.pose.position = Point(*pos) m.pose.orientation = Quaternion(*ori) m.scale = Vector3(*scale) pub.publish(m)
def visualize(self):
m_array = MarkerArray()
for i, g in enumerate(self.gates):
m = Marker()
m.header = Header(frame_id='/enu', stamp=rospy.Time.now())
m.ns = 'gates'
m.id = i
m.type = Marker.LINE_STRIP
m.action = Marker.ADD
m.scale.x = 0.1
m.color.r = 0.5
m.color.b = 0.75
m.color.g = 0.1
m.color.a = 1.0
m.points.append(g[1].buoy1.position)
m.points.append(g[1].buoy2.position)
m.lifetime = rospy.Duration(self.lifetime)
m_array.markers.append(m)
self.gate_viz_pub.publish(m_array)
def publish_closest_person_marker(self, pose, frame_id):
if self.vis_marker_topic_pub.get_num_connections() > 0:
m = Marker()
m.header.stamp = rospy.Time.now()
m.header.frame_id = frame_id
m.ns = "velocity_costmaps"
m.id = 0
m.type = m.SPHERE
m.action = m.MODIFY
m.pose = pose
m.pose.position.z = 2.0
m.scale.x = .25
m.scale.y = .25
m.scale.z = .25
m.color.a = 1.0
m.color.r = 0.0
m.color.g = 1.0
m.color.b = 0.0
m.lifetime = rospy.Duration(1.)
self.vis_marker_topic_pub.publish(m)
def get_path_markers(self):
markers = []
for i, pose_stamped in enumerate(self.path.poses):
marker = Marker()
marker.header.frame_id = 'pegasus_map'
marker.ns = 'path_markers'
marker.id = 0
marker.type = marker.TEXT_VIEW_FACING
marker.action = marker.ADD
marker.pose.orientation.w = 1
marker.text = '%s' % (i, )
marker.lifetime = rospy.Duration()
marker.scale.z = 10
marker.color.g = 0
marker.color.r = 1
marker.color.b = 1
marker.color.a = 1
marker.pose = pose_stamped.pose
markers.append(marker)
return markers
def node_to_marker(self, id, node):
marker = Marker()
marker.header.frame_id = "map"
marker.type = marker.SPHERE
marker.action = marker.ADD
marker.id = id
marker.scale.x = 0.4
marker.scale.y = 0.4
marker.scale.z = 0.4
marker.color.r = node.gain / 72.0
marker.color.g = 0.0
marker.color.b = 0.5
marker.color.a = 1.0
marker.pose.orientation.w = 1.0
marker.pose.position.x = node.position.x
marker.pose.position.y = node.position.y
marker.pose.position.z = node.position.z
marker.lifetime = rospy.Time(1.2)
return marker
