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 publish_debug_info(self, footprint_point, distance, scan_point):
array = MarkerArray()
fp = Marker()
fp.id = 1
fp.type = fp.SPHERE
scale = 0.1
fp.pose.position.x,fp.pose.position.y, fp.pose.position.z = footprint_point[0], footprint_point[1], 0
fp.scale.x, fp.scale.y, fp.scale.z = scale, scale, scale
fp.color.r, fp.color.g, fp.color.b, fp.color.a = (0, 0, 1, 1)
fp.header.frame_id = "{}/base_link".format(self.robot.robot_name)
fp.frame_locked = True
fp.action = fp.ADD
fp.ns = "door_opening"
array.markers += [fp]
sp = Marker()
sp.id = 2
sp.type = sp.SPHERE
scale = 0.1
sp.pose.position.x,sp.pose.position.y, sp.pose.position.z = scan_point[0], scan_point[1], 0
sp.scale.x, sp.scale.y, sp.scale.z = scale, scale, scale
sp.color.r, sp.color.g, sp.color.b, sp.color.a = (1, 0, 0, 1)
sp.header.frame_id = "{}/base_link".format(self.robot.robot_name)
sp.frame_locked = False
sp.action = sp.ADD
sp.ns = "door_opening"
array.markers += [sp]
self.debug_vizualizer.publish(array)
def add_marker(self, array, track, color, tid):
m1 = Marker()
m1.header.frame_id = "camera_rgb_optical_frame"
m1.ns = "line"
m1.id = tid
m1.type = 4 #lines
m1.action = 0
m1.scale.x = .002
m1.color.a = 1.
m1.color.r = color[0]/255.
m1.color.g = color[1]/255.
m1.color.b = color[2]/255.
m1.points = track
array.append(m1)
m2 = Marker()
m2.header.frame_id = "camera_rgb_optical_frame"
m2.ns = "point"
m2.id = tid
m2.type = 8 #points
m2.action = 0
m2.scale.x = .008
m2.scale.y = .008
m2.color.a = 1.
m2.color.r = color[0]/255.
m2.color.g = color[1]/255.
m2.color.b = color[2]/255.
m2.points = [ track[-1] ]
array.append(m2)
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 publish(self, target_frame, timestamp):
ma = MarkerArray()
for id in self.marker_list:
marker = Marker()
marker.header.stamp = timestamp
marker.header.frame_id = target_frame
marker.ns = "landmark_kf"
marker.id = id
marker.type = Marker.CYLINDER
marker.action = Marker.ADD
Lkf = self.marker_list[id]
marker.pose.position.x = Lkf.L[0,0]
marker.pose.position.y = Lkf.L[1,0]
marker.pose.position.z = 0
marker.pose.orientation.x = 0
marker.pose.orientation.y = 0
marker.pose.orientation.z = 1
marker.pose.orientation.w = 0
marker.scale.x = max(3*sqrt(Lkf.P[0,0]),0.05)
marker.scale.y = max(3*sqrt(Lkf.P[1,1]),0.05)
marker.scale.z = 0.5;
marker.color.a = 1.0;
marker.color.r = 1.0;
marker.color.g = 1.0;
marker.color.b = 0.0;
marker.lifetime.secs=3.0;
ma.markers.append(marker)
marker = Marker()
marker.header.stamp = timestamp
marker.header.frame_id = target_frame
marker.ns = "landmark_kf"
marker.id = 1000+id
marker.type = Marker.TEXT_VIEW_FACING
marker.action = Marker.ADD
Lkf = self.marker_list[id]
marker.pose.position.x = Lkf.L[0,0]
marker.pose.position.y = Lkf.L[1,0]
marker.pose.position.z = 1.0
marker.pose.orientation.x = 0
marker.pose.orientation.y = 0
marker.pose.orientation.z = 1
marker.pose.orientation.w = 0
marker.text = str(id)
marker.scale.x = 1.0
marker.scale.y = 1.0
marker.scale.z = 0.2
marker.color.a = 1.0;
marker.color.r = 1.0;
marker.color.g = 1.0;
marker.color.b = 1.0;
marker.lifetime.secs=3.0;
ma.markers.append(marker)
self.marker_pub.publish(ma)
def pubViz(self, ast, bst):
rate = rospy.Rate(10)
for i in range(self.winSize):
msgs = MarkerArray()
#use1について
msg = Marker()
#markerのプロパティ
msg.header.frame_id = 'camera_link'
msg.header.stamp = rospy.Time.now()
msg.ns = 'j1'
msg.action = 0
msg.id = 1
msg.type = 8
msg.scale.x = 0.1
msg.scale.y = 0.1
msg.color = self.carray[2]
#ジョイントポイントを入れる処理
for j1 in range(self.jointSize):
point = Point()
point.x = self.pdata[0][ast+i][j1][0]
point.y = self.pdata[0][ast+i][j1][1]
point.z = self.pdata[0][ast+i][j1][2]
msg.points.append(point)
msg.pose.orientation.w = 1.0
msgs.markers.append(msg)
msg = Marker()
msg.header.frame_id = 'camera_link'
msg.header.stamp = rospy.Time.now()
msg.ns = 'j2'
msg.action = 0
msg.id = 2
msg.type = 8
msg.scale.x = 0.1
msg.scale.y = 0.1
msg.color = self.carray[1]
for j2 in range(self.jointSize):
point = Point()
point.x = self.pdata[1][bst+i][j2][0]
point.y = self.pdata[1][bst+i][j2][1]
point.z = self.pdata[1][bst+i][j2][2]
msg.points.append(point)
msg.pose.orientation.w = 1.0
msgs.markers.append(msg)
self.mpub.publish(msgs)
rate.sleep()
def 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 pubRviz(self, pos, joints):
msgs = MarkerArray()
for p in range(len(pos)):
msg = Marker()
msg.header.frame_id = 'camera_link'
msg.header.stamp = rospy.Time.now()
msg.ns = 'marker'
msg.action = 0
msg.id = p
msg.type = 4
msg.scale.x = 0.1
msg.scale.y = 0.1
msg.color = self.carray[2]
for i in range(len(pos[p])):
point = Point()
point.x = pos[p][i][0]
point.y = pos[p][i][1]
point.z = pos[p][i][2]
msg.points.append(point)
msg.pose.orientation.w = 1.0
msgs.markers.append(msg)
for j in range(len(joints)):
msg = Marker()
msg.header.frame_id = 'camera_link'
msg.header.stamp = rospy.Time.now()
msg.ns = 'joints'
msg.action = 0
msg.id = j
msg.type = 8
msg.scale.x = 0.1
msg.scale.y = 0.1
msg.color = self.carray[j]
#print "joints len:"+str(len(joints[j]))
for i in range(len(joints[j])):
point = Point()
point.x = joints[j][i][0]
point.y = joints[j][i][1]
point.z = joints[j][i][2]
msg.points.append(point)
msg.pose.orientation.w = 1.0
msgs.markers.append(msg)
self.mpub.publish(msgs)
def publishObstacles(self):
"""
Publishes the obstacles as markers
"""
mk = Marker()
mk.header.stamp = rospy.get_rostime()
mk.header.frame_id = '/base_link'
mk.ns='basic_shapes'
mk.id = 0
mk.type = Marker.POINTS
mk.scale.x = 0.3
mk.scale.y = 0.3
mk.scale.z = 0.3
mk.color.r = 1.0
mk.color.a = 1.0
for value in self.obstacle_map.obstacles_in_memory:
p = Point()
p.x = value[0]
p.y = value[1]
mk.points.append(p)
self.obs_pub.publish(mk)
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 input_handler(space):
global marker_color
global marker_pub
global matches
for str in matches:
if space.aux_payload.find(str) == -1:
# print "FAIL: str='%s' aux_payload='%s'" % (str, space.aux_payload)
return
# print space
# print
marker = Marker()
marker.header.frame_id = "/map"
marker.header.stamp = rospy.Time()
marker.ns = ""
marker.id = 0
marker.type = Marker.POINTS
marker.action = Marker.ADD
marker.pose.position.x = 0
marker.pose.position.y = 0
marker.pose.position.z = 0
marker.pose.orientation.x = 0
marker.pose.orientation.y = 0
marker.pose.orientation.z = 0
marker.pose.orientation.w = 0
marker.scale.x = .1
marker.scale.y = .1
marker.scale.z = .1
marker.color = marker_color
marker.points = []
# this line cuts off anything before the string "convex_set: " in the aux_payload
convex_set = space.aux_payload[ space.aux_payload.find("convex_set: ") :]
# this line cuts off that string at the next newline character
convex_set = convex_set.split('\n')[0]
# this line splits the string into pieces delineated by spaces
convex_set = convex_set.split(' ')
# print convex_set
for candidate in convex_set:
if len(candidate) == 0 or candidate[0] != '(':
continue
coords = candidate[1:-1].split(',')
# print coords
marker.points.append(Point(float(coords[0]), float(coords[1]), float(coords[2])))
# print marker.points[-1]
# print
# print marker.points
# print
marker_pub.publish(marker)
def 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 addPolygonFilled(self, points):
oid = self.oid+1
name = "/polygonFilled"+str(self.oid)
marker = Marker()
marker.id = self.oid
marker.ns = "/polygonFilled"
marker.header.frame_id = name
marker.type = marker.TRIANGLE_LIST
marker.action = marker.ADD
marker.scale.x = 1
marker.scale.y = 1
marker.scale.z = 1
marker.color.r = 1.0
marker.color.g = 1.0
marker.color.b = 1.0
marker.color.a = 1.0
marker.pose.orientation.w = 1.0
marker.pose.position.x = 0
marker.pose.position.y = 0
marker.pose.position.z = 0
marker.points = []
for i in range(0,len(points)-2,1):
pt = Point(points[0][0], points[0][1], points[0][2])
marker.points.append(pt)
pt = Point(points[i+1][0], points[i+1][1], points[i+1][2])
marker.points.append(pt)
pt = Point(points[i+2][0], points[i+2][1], points[i+2][2])
marker.points.append(pt)
self.markerArray.markers.append(marker)
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 new_marker(self, ns="/debug", frame="enu", time=None, type = Marker.CUBE , position=(0,0,0), orientation=(0,0,0,1), color=(1,0,0)):
marker = Marker()
marker.ns = ns
if time != None:
marker.header.stamp = time
marker.header.frame_id = frame
marker.type = type
marker.action = marker.ADD
marker.scale.x = 1.0
marker.scale.y = 1.0
marker.scale.z = 1.0
marker.color.r = color[0]
marker.color.g = color[1]
marker.color.b = color[2]
marker.color.a = 1.0
marker.id = self.last_id
marker.pose.orientation.x = orientation[0]
marker.pose.orientation.y = orientation[1]
marker.pose.orientation.z = orientation[2]
marker.pose.orientation.w = orientation[3]
marker.pose.position.x = position[0]
marker.pose.position.y = position[1]
marker.pose.position.z = position[2]
self.last_id += 1
self.markers.markers.append(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 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 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 pose_cb(self, pose):
pose = self.listener.transformPose(self.reference_frame,pose)
q = pose.pose.orientation
qvec = [q.x,q.y,q.z,q.w]
euler = euler_from_quaternion(qvec)
self.goal_x = pose.pose.position.x
self.goal_y = pose.pose.position.y
self.goal_theta = euler[2]
(ex,ey,etheta) = self.compute_error()
if ex < -self.dist_threshold:
self.goal_theta = norm_angle(etheta + pi)
print "New goal: %.2f %.2f %.2f" % (self.goal_x, self.goal_y, self.goal_theta)
marker = Marker()
marker.header = pose.header
marker.ns = "goal_marker"
marker.id = 10
marker.type = Marker.ARROW
marker.action = Marker.ADD
marker.pose = pose.pose
marker.scale.x = 0.5
marker.scale.y = 0.5
marker.scale.z = 1.0;
marker.color.a = 1.0;
marker.color.r = 1.0;
marker.color.g = 1.0;
marker.color.b = 0.0;
marker.lifetime.secs=-1.0
self.marker_pub.publish(marker)
def __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 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 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 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 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 publish_marker(self): # create marker marker = Marker() marker.header.frame_id = "/base_link" marker.header.stamp = rospy.Time.now() marker.ns = "color" marker.id = 0 marker.type = 2 # SPHERE marker.action = 0 # ADD marker.pose.position.x = 0 marker.pose.position.y = 0 marker.pose.position.z = 1.5 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 = self.color.a #Transparency marker.color.r = self.color.r marker.color.g = self.color.g marker.color.b = self.color.b # publish marker self.pub_marker.publish(marker)
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 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 __setTextMarker(self, id, waypoint, colors = [1,0,0,0]):
try:
marker = Marker()
marker.header.frame_id = '/map'
marker.header.stamp = rospy.Time.now()
marker.ns = 'patrol'
marker.id = id + len(self.__waypoints)
marker.type = marker.TEXT_VIEW_FACING
marker.action = marker.ADD
marker.text = str(id)
marker.scale.x = 0.2
marker.scale.y = 0.2
marker.scale.z = 0.2
marker.color.a = colors[0]
marker.color.r = colors[1]
marker.color.b = colors[2]
marker.color.g = colors[3]
marker.pose.orientation.w = 1.0
marker.pose.position.x = waypoint.target_pose.pose.position.x
marker.pose.position.y = waypoint.target_pose.pose.position.y
marker.pose.position.z = waypoint.target_pose.pose.position.z
return marker
except Exception as ex:
rospy.logwarn('PatrolNode.__setMarker- ', ex.message)
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 __draw_poses(self, poses, frame):
rospy.loginfo("Drawing the poses")
marker = Marker()
marker.header.stamp = rospy.Time.now()
marker.ns = "circle_around"
marker.type = Marker.POINTS
marker.pose.orientation.w = 1.0
marker.id = 1000
marker.action = Marker.ADD
marker.header.frame_id = frame
marker.scale.x = 0.05
marker.scale.y = 0.05
marker.scale.z = 0.05
marker.color.b = 1.0
marker.color.a = 1.0
for p in poses:
marker.points.append(Point(p[0], p[1], p[2]))
self.collision_objects__markers_pub.publish(marker)
def set_marker(self, goal_pose):
marker_data = Marker()
marker_data.header.frame_id = "map"
marker_data.header.stamp = rospy.Time.now()
marker_data.ns = "basic_shapes"
marker_data.id = 0
marker_data.action = Marker.ADD
marker_data.pose = goal_pose.target_pose.pose
marker_data.color.r = 1.0
marker_data.color.g = 0.0
marker_data.color.b = 0.0
marker_data.color.a = 1.0
marker_data.scale.x = 0.4
marker_data.scale.y = 0.05
marker_data.scale.z = 0.05
marker_data.lifetime = rospy.Duration()
marker_data.type = 0
return marker_data
def create_points_marker(mrkid,
points,
size=1e-3,
color=(1, 0, 0, 1),
ns='',
frame_id='base_link',
transform=None):
if transform is None:
transform = np.eye(4)
marker = Marker()
marker.header.stamp = get_safe_stamp()
marker.header.frame_id = frame_id
marker.ns = ns
marker.id = mrkid
marker.type = Marker.POINTS
marker.pose = criros.conversions.to_pose(transform)
marker.scale = Vector3(size, size, 0) # (width, height, unused)
marker.color = ColorRGBA(*color)
marker.action = Marker.ADD
# Append the points
for point in points:
marker.points.append(criros.conversions.to_point(point))
return marker
def create_marker_text(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.TEXT_VIEW_FACING
point_marker.action = Marker.ADD
point_marker.color.r = 1.0
point_marker.color.g = 1.0
point_marker.color.b = 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 visualize(self, states, colour, pub):
marker_array = MarkerArray()
marker = Marker()
marker.header.stamp = self.odom_msg.header.stamp
marker.header.frame_id = 'ego_vehicle'
marker.ns = 'predicted_trajectory'
marker.type = 4
marker.action = 0 # Adds an object (check it later)
for idx in range(states.shape[0]):
# state in both models have x and y first
P = Point()
P.x = states[idx, 0] # state in both models have x and y first
P.y = states[idx, 1]
P.z = 0
marker.points.append(P)
marker.scale.x = 2
marker.color.a = colour[0]
marker.color.r = colour[1]
marker.color.g = colour[2]
marker.color.b = colour[3]
# marker.frame_locked = True
# marker_array.markers.append(marker)
pub.publish(marker)
def send_marker(self, observation):
"""
Publishes a visualization marker of the transformed observation to use in rviz for example.
"""
marker = Marker()
marker.ns = 'hanoi_marker'
marker.id = observation.id
marker.action = 0 # add/modify
marker.text = "Tag {}".format(observation.id)
marker.header.frame_id = ORIGIN_FRAME
marker.header.stamp = rospy.Time.now()
marker.lifetime = rospy.Duration(1)
marker.type = 1 # cube
marker.pose = observation.pose.pose
marker.scale.x = 0.1
marker.scale.y = 0.1
marker.scale.z = 0.1
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0
marker.color.a = 1.0
self.marker_publisher.publish(marker)
def publish_lookahead(self, robot, lookahead):
marker = Marker()
marker.header.frame_id = "/map"
marker.header.stamp = rospy.Time.now()
marker.ns = "pure_pursuit"
marker.type = marker.LINE_STRIP
marker.action = marker.ADD
wp = Point()
wp.x, wp.y = robot[:2]
wp.z = 0
marker.points.append(wp)
wp = Point()
wp.x, wp.y = lookahead[0], lookahead[1]
wp.z = 0
marker.points.append(wp)
marker.id = 0
marker.scale.x = 0.2
marker.scale.y = 0.2
marker.scale.z = 0.2
marker.color.a = 1.0
marker.color.b = 1.0
marker.color.g = 1.0
self.pub_lookahead.publish(marker)
def getDroneMarker(self):
marker=Marker();
marker.id=1;
marker.ns="mesh_"+self.name;
marker.header.frame_id="world"
marker.type=marker.MESH_RESOURCE;
marker.action=marker.ADD;
marker.pose.position.x=self.state.pos.x
marker.pose.position.y=self.state.pos.y
marker.pose.position.z=self.state.pos.z
marker.pose.orientation.x=self.state.quat.x
marker.pose.orientation.y=self.state.quat.y
marker.pose.orientation.z=self.state.quat.z
marker.pose.orientation.w=self.state.quat.w
marker.lifetime = rospy.Duration.from_sec(0.0);
marker.mesh_use_embedded_materials=True
marker.mesh_resource="package://mader/meshes/quadrotor/quadrotor.dae"
marker.scale.x=1.0;
marker.scale.y=1.0;
marker.scale.z=1.0;
return marker
def publish_path_marker(self, path, markerid=0):
markers = Marker()
markers.header.frame_id = '/map'
markers.header.stamp = rospy.Time.now()
markers.type = Marker.LINE_STRIP
markers.action = Marker.ADD
markers.ns = 'edge'
markers.id = markerid
markers.color.r = 0.2 #+ markerid*0.3
markers.color.g = 0.3 #+ markerid*0.3
markers.color.b = 1.0
markers.color.a = 1.0
markers.scale.x = 0.3
markers.scale.y = 0.3
markers.scale.z = 0.3
markers.pose.orientation.w = 1.
for k, point in enumerate(path):
node_disp = Point(point.position.x, point.position.y,
point.position.z)
# import ipdb;ipdb.set_trace()
markers.points.append(node_disp)
time.sleep(0.1)
self.path_vis.publish(markers)
def generateMarker(self, bbox, i):
marker=Marker();
marker.id=i;
marker.ns="mesh";
marker.header.frame_id="world"
marker.type=marker.MESH_RESOURCE;
marker.action=marker.ADD;
marker.pose.position.x=0.0 #Will be updated later
marker.pose.position.y=0.0 #Will be updated later
marker.pose.position.z=0.0 #Will be updated later
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.from_sec(0.0);
marker.mesh_use_embedded_materials=True
marker.mesh_resource="package://panther/meshes/ConcreteDamage01b/model3.dae"
marker.scale.x=bbox[0];
marker.scale.y=bbox[1];
marker.scale.z=bbox[2];
return marker
def publish_nodes_marker(self, poses):
markers = Marker()
markers.header.frame_id = '/map'
markers.header.stamp = rospy.Time.now()
markers.type = Marker.CUBE_LIST
markers.action = Marker.ADD
markers.ns = 'nodes'
markers.id = 0
markers.color.r = 1.0
markers.color.g = 1.0
markers.color.b = 0.0
markers.color.a = 1.0
markers.scale.x = 1.0
markers.scale.y = 1.0
markers.scale.z = 1.0
for k, point in enumerate(poses):
node_disp = Point(point.position.x, point.position.y,
point.position.z)
# import ipdb;ipdb.set_trace()
markers.points.append(node_disp)
# import ipdb;ipdb.set_trace()
time.sleep(1.0)
self.node_vis.publish(markers)
def visualization_stl(num, position, color):
marker = Marker()
marker.header.frame_id = "/world"
marker.type = Marker.MESH_RESOURCE
marker.mesh_resource = "package://aubo_demo/stl_test/stl_document/0016.STL"
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 = num
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
return marker
def PublishSteerPoint(self, x, y):
marker = Marker()
marker.ns = "steer"
marker.id = 0
marker.action = marker.MODIFY
marker.header.frame_id = "/map"
marker.type = marker.CYLINDER
marker.pose.position.x = x
marker.pose.position.y = y
marker.pose.position.z = 0.51
marker.scale.x = 0.1
marker.scale.y = 0.1
marker.scale.z = 1
marker.color.g = 0
marker.color.r = 0
marker.color.b = 1.0
marker.color.a = 1
marker.pose.orientation.x = 0.0
marker.pose.orientation.y = 0.0
marker.pose.orientation.z = 0.0
marker.pose.orientation.w = 1.0
self.steer_pub.publish(marker)
def addCentroids(self, in_objects, out_markers):
for obj in in_objects.detections:
centroid_marker = Marker()
centroid_marker.lifetime = rospy.Duration(
self._params.marker_lifetime)
# Message headers
centroid_marker.header = in_objects.header
centroid_marker.type = Marker.SPHERE
centroid_marker.action = Marker.ADD
centroid_marker.ns = self._params.marker_namespace + "/centroids"
centroid_marker.id = self._marker_id
self._marker_id += 1
# Marker properties
centroid_marker.scale.x = self._params.centroid_scale
centroid_marker.scale.y = self._params.centroid_scale
centroid_marker.scale.z = self._params.centroid_scale
centroid_marker.color = self._params.centroid_color
centroid_marker.pose = obj.bbox.pose.pose
out_markers.markers.append(centroid_marker)
def Visualize(self):
m = Marker()
m.header.stamp = rospy.Time.now()
m.header.frame_id = self._fixed_frame
m.ns = "map"
m.id = 0
m.type = Marker.CUBE_LIST
m.action = Marker.ADD
m.scale.x = self._x_res
m.scale.y = self._y_res
m.scale.z = 0.01
for ii in range(self._x_num):
for jj in range(self._y_num):
p = Point(0.0, 0.0, 0.0)
(p.x, p.y) = self.VoxelCenter(ii, jj)
m.points.append(p)
m.colors.append(self.Colormap(ii, jj))
self._vis_pub.publish(m)
print("m =")
print(m)
def insert_detected_object(self,detection): object_label = detection.label object_pose = detection.pose object_bb = detection.bounding_box_lwh marker = Marker() marker.header = detection.pose.header marker.ns = "ExploreScene" marker.id = 0 marker.type = 1 marker.action = 0 marker.pose = object_pose.pose marker.scale.x = object_bb.x marker.scale.y = object_bb.y marker.scale.z = object_bb.z marker.color.a = 1.0 marker.color.r = 0.0 + self.color_inc; marker.color.g = 1.0 - self.color_inc; marker.color.b = 0.0 ; self.color_inc = self.color_inc + 0.1 self.vis_pub.publish( marker );
def updateMissionMarkers(self):
array_msg = MarkerArray()
array_msg.markers = []
for idx,task in enumerate(self.plan.plan):
marker_msg = Marker()
marker_msg.header.stamp = rospy.Time.now()
marker_msg.header.frame_id = "utm"
if task.action=="WP" or task.action=="HOME":
marker_msg.ns = "mission/markers"
marker_msg.id = idx
marker_msg.type = 2
marker_msg.action = 0
UTM = utm.toUtm(task.data[0],task.data[1])
marker_msg.pose = Pose(Vector3(UTM.easting,UTM.northing,0),Quaternion(0,0,0,1))
marker_msg.scale = Vector3(5,5,5)
if idx in self.plan._complete:
marker_msg.color = ColorRGBA(0,1,0,0.8) #GREEN
elif idx in self.plan._skipped:
marker_msg.color = ColorRGBA(1.0,1.0,0,0.8) #YELLOW
else:
marker_msg.color = ColorRGBA(1,0,0,0.8) #RED
array_msg.markers.append(marker_msg)
self.marker_pub.publish(array_msg)
def _init_markers(self):
self.marker_array_msg = MarkerArray()
self.marker_idx = 0
for i in range(self.max_markers):
marker = Marker()
marker.header.frame_id = self.global_frame
marker.id = self.marker_idx
marker.type = 2
marker.action = 2
marker.pose = Pose()
marker.color.r = 0.0
marker.color.g = 0.0
marker.color.b = 0.0
marker.color.a = 0.0
marker.scale.x = 0.1
marker.scale.y = 0.1
marker.scale.z = 0.1
marker.frame_locked = False
marker.ns = "Goal-%u" % i
self.marker_array_msg.markers.append(marker)
self.marker_idx += 1
self.marker_idx = 0
self.marker_array_pub.publish(self.marker_array_msg)
def update(self):
marker = Marker()
marker.scale.x = 1
marker.scale.y = 1
marker.scale.z = 1
marker.action = Marker.ADD
marker.header.frame_id = "base_link"
marker.header.stamp = rospy.Time()
marker.type = Marker.TEXT_VIEW_FACING
marker.ns = self.NS
marker.pose.position.x = self.X_POS
marker.id = self.ID
marker.color.a = 1
marker.color.b = 1
if self.voltage is not None:
marker.text += "Voltage {}".format(self.voltage)
if self.wrench is not None:
marker.text += "\nWrench {}".format(self.wrench)
if self.station_holding:
marker.text += "\nStation Holding"
if self.kill_alarm:
marker.text += "\nKILLED"
self.markers_pub.publish(marker)
def _status_callback(self, status):
viz = Marker()
viz.header.frame_id = '/map'
viz.header.stamp = rospy.Time.now()
viz.header.seq = self.seq
viz.action = viz.MODIFY
viz.ns = 'paths'
viz.id = 0
viz.type = viz.LINE_LIST
# viz.color = color
viz.scale = self.scale
for i_d in range(status.num_drones):
p_curr = Point()
p_curr.x = status.current_xs[i_d]
p_curr.y = status.current_ys[i_d]
p_target = Point()
p_target.x = status.target_xs[i_d]
p_target.y = status.target_ys[i_d]
viz.points.extend([p_curr, p_target])
viz.colors.extend([self.curr_color, self.target_color])
self.viz_pub.publish(viz)
rospy.loginfo('Captain: published markers')
def publish_balloon_visualize(self, balloon_id):
# published the balloon
marker = Marker()
marker.header.frame_id = "world"
marker.header.stamp = rospy.Time.now()
marker.ns = "baloon_visualization"
marker.id = balloon_id
marker.type = Marker.SPHERE
marker.pose.position.x = self.balloon_pos[balloon_id].x
marker.pose.position.y = self.balloon_pos[balloon_id].y
marker.pose.position.z = self.balloon_pos[balloon_id].z
marker.pose.orientation.x = 0
marker.pose.orientation.y = 0
marker.pose.orientation.z = 0
marker.pose.orientation.w = 1
marker.scale.x = .4
marker.scale.y = .4
marker.scale.z = .4
marker.color.r = 1.0
marker.color.g = 0.0
marker.color.b = 0.0
marker.color.a = 0.5
self.balloon_marker_pubs[balloon_id].publish(marker)
def set_point(self, x, y, r, g, b, size=0.02):
'''
Set Point at MarkArray
'''
global idx
marker = Marker()
marker.header.frame_id = "/map"
marker.id = idx # int(x*1000 + y*1000)
idx += 1
marker.ns = "tiles"
marker.header.stamp = rospy.get_rostime()
marker.type = marker.SPHERE
marker.action = marker.ADD
marker.scale.x = size
marker.scale.y = size
marker.scale.z = size
marker.color.a = 1.0
marker.color.r = r / 255.0
marker.color.g = g / 255.0
marker.color.b = b / 255.0
marker.pose.orientation.w = 1.0
(marker.pose.position.x, marker.pose.position.y) = (x, y)
self.markerArray.markers.append(marker)
def initObstMarker():
obstacle = Marker()
obstacle.header.frame_id = 'path_planner'
obstacle.header.stamp = rospy.get_rostime()
obstacle.ns = "path_planner"
obstacle.action = 0 # add/modify an object
obstacle.id = 111
obstacle.type = 8 # Points
obstacle.pose.orientation.w = 1.0
obstacle.pose.position.x = 0.0
obstacle.pose.position.y = 0.0
obstacle.pose.position.z = 0.0
obstacle.scale.x = obstacle.scale.y = obstacle.scale.z = 0.5
obstacle.color.r = 0.0
obstacle.color.g = 0.0
obstacle.color.b = 0.0
obstacle.color.a = 1.0
return obstacle
def tcp_marker(pose, ns, i, r, g, b):
m_p = Marker()
m_p.header.frame_id = '/world'
m_p.header.stamp = rospy.Time.now()
m_p.ns = ns
m_p.id = i
m_p.type=Marker.CUBE
m_p.action = Marker.ADD
m_p.pose.position.x = pose[0]
m_p.pose.position.y = pose[1]
m_p.pose.position.z = pose[2]
m_p.scale.x = 0.0005
m_p.scale.y = 0.001
m_p.scale.z = 0.006
m_p.pose.orientation.x = pose[3]
m_p.pose.orientation.y = pose[4]
m_p.pose.orientation.z = pose[5]
m_p.pose.orientation.w = pose[6]
m_p.color.a = 1.0
m_p.color.r = r
m_p.color.g = g
m_p.color.b = b
return m_p
def mark(all_lines):
marker = Marker()
marker.header.frame_id = "/base_laser_link"
marker.header.stamp = rospy.Time.now()
marker.ns = "rvizmark"
marker.action = Marker.ADD
marker.pose.orientation.w = 1.0
marker.type = Marker.LINE_STRIP
marker.scale.x = 0.1
marker.color.b = 1.0
marker.color.a = 1.0
i = 0
while i < len(all_lines):
print 'hiiiiiiiiiiiiiiiiiiiiiiii'
marker.id = i
startpt = Point(all_lines[i][0][0], all_lines[i][0][1], 0.0)
endpt = Point(all_lines[i][1][0], all_lines[i][1][1], 0.0)
marker.points.append(startpt)
marker.points.append(endpt)
i += 1
rospy.logdebug("in publish")
pub.publish(marker)
def __init__(self):
ellipse_rot = np.mat([[-1., 0., 0.], [0., -1., 0.], [0., 0., 1.]])
self.sspace = SpheroidSpace(0.15,
np.mat([0.78, -0.18, 0.1]).T, ellipse_rot)
self.colors = [ColorRGBA(1., 0., 0., 1.), ColorRGBA(0., 1., 0., 1.)]
self.vis_pub = rospy.Publisher("/force_torque_markers_array",
MarkerArray)
self.pose_pub = rospy.Publisher("/spheroid_poses", PoseStamped)
m = Marker()
m.header.frame_id = "torso_lift_link"
m.header.stamp = rospy.Time()
m.ns = "force_torque"
m.id = 0
m.type = Marker.ARROW
m.action = Marker.ADD
#m.points.append(Point(0, 0, 0))
m.scale = Vector3(0.01, 0.01, 0.01)
m.color = self.colors[0]
#self.vis_pub.publish(m)
self.m = m
self.ma = MarkerArray()
def Sample(self, x_indices, y_indices):
h = 0.05
points = self.compile_transform_vectorized(x_indices, y_indices, h)
marker = Marker()
marker.ns = "scan"
marker.action = marker.MODIFY
marker.header.frame_id = "/map"
marker.type = marker.POINTS
marker.points = points
marker.scale.x = 0.05
marker.scale.y = 0.05
marker.color.g = 0
marker.color.r = 1
marker.color.b = 1
marker.color.a = 1
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(1)
self.sample_space_pub.publish(marker)
def create_marker(self, frame, posx, posy, posz):
marker = Marker()
marker.header.frame_id = frame
marker.header.stamp = rospy.Time.now()
marker.ns = "my_namespace"
marker.id = 0
marker.type = Marker.SPHERE
marker.action = Marker.ADD
marker.pose.position.x = posx
marker.pose.position.y = posy
marker.pose.position.z = posz
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 = 1
marker.scale.y = 1
marker.scale.z = 1
marker.color.a = 1.0 # Don't forget to set the alpha!
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0
return marker
def marker_publisher(self, desired_pose):
wp_marker = Marker()
wp_marker.header.frame_id = desired_pose.header.frame_id
wp_marker.header.stamp = desired_pose.header.stamp
wp_marker.ns = "bezier"
wp_marker.action = wp_marker.ADD
wp_marker.type = wp_marker.SPHERE
wp_marker.id = self.wp_num
wp_marker.pose.position.x = desired_pose.pose.position.x
wp_marker.pose.position.y = desired_pose.pose.position.y
wp_marker.pose.position.z = desired_pose.pose.position.z
wp_marker.scale.x = 1
wp_marker.scale.y = 1
wp_marker.scale.z = 0.1
wp_marker.color.r = 1.0
wp_marker.color.g = 0
wp_marker.color.b = 0
wp_marker.color.a = 1.0
self.marker_pub.publish(wp_marker)
def gen_grid_cartesian(self, time):
marker = Marker()
marker.header.frame_id = self.odom_frame
marker.header.stamp = time
marker.lifetime = rospy.Duration(0.2)
marker.ns = "odometry/cartesian_grid"
marker.id = 0
marker.type = Marker.LINE_LIST
marker.action = Marker.ADD
marker.scale.x = self.line_width
marker.color = OdometryHelper.COLOR_GRAY
x_min = -self.x_size / 2.0
x_max = self.x_size / 2.0
y_min = -self.y_size / 2.0
y_max = self.y_size / 2.0
for x in np.arange(x_min, x_max, self.dx):
marker.points.append(Point(x=x, y=y_min, z=0.01))
marker.points.append(Point(x=x, y=y_max, z=0.01))
for y in np.arange(y_min, y_max, self.dy):
marker.points.append(Point(x=x_min, y=y, z=0.01))
marker.points.append(Point(x=x_max, y=y, z=0.01))
return marker
