def add_name_marker(self, center_x, center_y, who):
'''
This method will add name of the players under their scores
who: 1, for user
0, for computer
'''
marker = Marker()
marker.header.frame_id = "/map"
marker.header.stamp = rospy.Time.now()
marker.action = marker.ADD
marker.type = Marker.TEXT_VIEW_FACING
marker.scale.x = 0.3
marker.scale.y = 0.3
marker.scale.z = 0.3
marker.color.a = 1
marker.color.r = 1
marker.color.g = 1
marker.color.b = 1
marker.pose.orientation.w = 0
marker.pose.orientation.x = 0
marker.pose.orientation.y = 0
marker.pose.orientation.z = 0
marker.pose.position.x = center_x
marker.pose.position.y = center_y
marker.pose.position.z = 0.5
if who == 0:
marker.ns = 'computer_name'
marker.id = 6
marker.text = 'Computer'
else:
marker.ns = 'player_name'
marker.id = 7
marker.text = 'Player'
self.marker_publisher.publish(marker)
def service_cb(req):
marker = Marker() # Sphere
text_marker = Marker() # Text
marker.header.stamp = rospy.Time.now()
marker.header.frame_id = "base_link"
text_marker.header.stamp = rospy.Time.now()
text_marker.header.frame_id = "base_link"
marker.action = Marker.ADD
marker.type = Marker.SPHERE
text_marker.type = Marker.TEXT_VIEW_FACING
marker.pose.position = req.point
marker.pose.orientation.w = 1.0
marker.scale.x = marker.scale.y = marker.scale.z = 0.02
text_marker.pose.position.y = -0.9
text_marker.pose.position.x = 0.9
text_marker.scale.z = 0.06
if req.primitive == 0: # suck 1
marker.color.b = marker.color.a = 1.0 # blue
elif req.primitive == 1: # suck 2
marker.color.g = marker.color.a = 1.0 # green
else: # grasp
marker.color.r = marker.color.a = 1.0
text_marker.color.r = text_marker.color.g = text_marker.color.b = text_marker.color.a = 1.0
if req.primitive == 0 and req.valid: # suck 1
text_marker.text = "suck_1"
elif req.primitive == 1 and req.valid: # suck 2
text_marker.text = "suck_2"
elif req.primitive >= 2 and req.valid: # GRASP
angle = round(req.angle * 180 / pi)
text_marker.text = "grasp," + str(int(angle))
elif not req.valid: # INVALID
text_marker.text = "invalid target"
pub_marker.publish(marker)
pub_text_marker.publish(text_marker)
return viz_markerResponse()
def make_marker(self, barcode_msg):
ean = barcode_msg.barcode
m = Marker()
m.header = barcode_msg.barcode_pose.header
m.pose = barcode_msg.barcode_pose.pose
m.action = Marker.ADD
m.type = Marker.CUBE
m.scale = Vector3(.06, .06, .06)
m.color = ColorRGBA(1, 0, 0, 1.0)
text = Marker()
text.header = barcode_msg.barcode_pose.header
text.action = Marker.ADD
text.type = Marker.TEXT_VIEW_FACING
text.text = ean
text.scale = Vector3(0, 0, .06)
text.color = ColorRGBA(1, 0, 0, 1)
text.pose.position.x = barcode_msg.barcode_pose.pose.position.x
text.pose.position.y = barcode_msg.barcode_pose.pose.position.y
text.pose.position.z = barcode_msg.barcode_pose.pose.position.z + 0.05
m.text = ean
m.ns = ean
text.ns = ean
m.id = 2
self.marker_pub.publish(m)
self.marker_pub.publish(text)
def publish_waypoint_markers(self):
pub_waypoint_markers = rospy.Publisher('waypoint_markers', MarkerArray)
marker_array = MarkerArray()
for i in range(len(self.waypoints)):
waypoint = self.waypoints[i]
waypoint_marker = Marker()
waypoint_marker.id = i
waypoint_marker.header.frame_id = "/map"
waypoint_marker.header.stamp = rospy.Time.now()
if (waypoint.type == 'P'):
waypoint_marker.type = 5 # Line List
waypoint_marker.text = 'waypoint_%s_point' % i
waypoint_marker.color.r = 176.0
waypoint_marker.color.g = 224.0
waypoint_marker.color.b = 230.0
waypoint_marker.color.a = 0.5
waypoint_marker.scale.x = 0.5
c = waypoint.coordinate
waypoint_marker.points.append(Point(c.x, c.y, c.z))
waypoint_marker.points.append(Point(c.x, c.y, c.z + 3.0))
else:
waypoint_marker.type = 3 # Cylinder
waypoint_marker.text = 'waypoint_%s_cone' % i
waypoint_marker.color.r = 255.0
waypoint_marker.color.g = 69.0
waypoint_marker.color.b = 0.0
waypoint_marker.color.a = 1.0
waypoint_marker.scale.x = 0.3
waypoint_marker.scale.y = 0.3
waypoint_marker.scale.z = 0.5
waypoint_marker.pose.position = waypoint.coordinate
marker_array.markers.append(waypoint_marker)
if self.current_waypoint_idx != len(self.waypoints):
current_waypoint_marker = Marker()
current_waypoint_marker.id = 999
current_waypoint_marker.header.frame_id = "/map"
current_waypoint_marker.header.stamp = rospy.Time.now()
current_waypoint_marker.type = 2 # Sphere
current_waypoint_marker.text = 'current_waypoint'
current_waypoint_marker.color.r = 255.0
current_waypoint_marker.color.g = 0.0
current_waypoint_marker.color.b = 0.0
current_waypoint_marker.color.a = 1.0
current_waypoint_marker.scale.x = 0.3
current_waypoint_marker.scale.y = 0.3
current_waypoint_marker.scale.z = 0.3
current_waypoint = self.waypoints[self.current_waypoint_idx]
current_waypoint_marker.pose.position.x = current_waypoint.coordinate.x
current_waypoint_marker.pose.position.y = current_waypoint.coordinate.y
current_waypoint_marker.pose.position.z = 1.0
marker_array.markers.append(current_waypoint_marker)
pub_waypoint_markers.publish(marker_array)
def ArrowLabel():
global count
global state
global frame
global start
global goal
global waypoint
global label
pub = rospy.Publisher('label', MarkerArray, queue_size = 10)
marker_data = Marker()
marker_data.header.frame_id = frame
marker_data.header.stamp = rospy.Time.now()
marker_data.ns = 'basic_shapes'
marker_data.action = Marker.ADD
marker_data.lifetime = rospy.Duration()
marker_data.type = Marker.TEXT_VIEW_FACING
marker_data.color.r = 1.0
marker_data.color.g = 0.2
marker_data.color.b = 0.5
marker_data.color.a = 1.0
marker_data.scale.x = 1.0
marker_data.scale.y = 1.0
marker_data.scale.z = 1.0
if state == 'goal' :
pose = goal
marker_data.text = 'Goal'
marker_data.id = -1
elif state == 'start' :
pose = start
marker_data.text = 'Start'
marker_data.id = -2
else :
pose = waypoint.poses[count]
marker_data.text = str(count)
marker_data.id = count
count +=1
marker_data.pose.position.x = pose.position.x
marker_data.pose.position.y = pose.position.y
marker_data.pose.orientation.z = pose.orientation.z
marker_data.pose.orientation.w = pose.orientation.w
label.markers.append(marker_data)
pub.publish(label)
def text_marker(marker_id, _point, txt):
#waypoints = list()
#waypoints.append(Pose(Point(2.680, 1.600, 0.000), 0.1))
#waypoints.append(Pose(Point(2.680, -1.600, 0.000), 0.1))
#waypoints.append(Pose(Point(-2.680, 1.600, 0.000), 0.1))
#waypoints.append(Pose(Point(-2.680, -1.600, 0.000), 0.1))
#set up markeri
marker = Marker()
marker_scale = 0.6
marker_lifetime = 0
marker_ns = 'point name'
marker_color= {'r':0.0, 'g':1.0, 'b':0.0, 'a':1.0}
marker.ns = marker_ns
marker.id = marker_id
# marker.type = Marker.SPHERE
marker.type = Marker.TEXT_VIEW_FACING
marker.action = Marker.ADD
marker.header.frame_id = '/map'
marker.lifetime = rospy.Duration(marker_lifetime)
marker.scale.x = marker_scale
marker.scale.y = marker_scale
marker.scale.z = marker_scale
marker.color.r = marker_color['r']
marker.color.g = marker_color['g']
marker.color.b = marker_color['b']
marker.color.a = marker_color['a']
marker.text = txt
marker.header.stamp = rospy.Time.now()
marker.pose.position.x = _point.x
marker.pose.position.y = _point.y
marker.pose.position.z = _point.z
print txt
return marker
def draw_marker(
self,
ps,
id=None,
type=Marker.CUBE,
ns="default_ns",
rgba=(0, 1, 0, 1),
scale=(0.03, 0.03, 0.03),
text="",
duration=0,
):
"""
If markers aren't showing up, it's probably because the id's are being overwritten. Make sure to set
the ids and namespace.
"""
if id is None:
id = self.get_unused_id()
marker = Marker(type=type, action=Marker.ADD)
marker.ns = ns
marker.header = ps.header
marker.pose = ps.pose
marker.scale = gm.Vector3(*scale)
marker.color = stdm.ColorRGBA(*rgba)
marker.id = id
marker.text = text
marker.lifetime = rospy.Duration(duration)
self.pub.publish(marker)
self.ids.add(id)
return MarkerHandle(marker, self.pub)
def get_vehicle_marker(object, header, marker_id=0, is_player=False):
"""
Return a marker msg
:param object: carla agent object (pb2 object (vehicle, pedestrian or traffic light))
:param header: ros header (stamp/frame_id)
:param marker_id: a marker id (int32)
:param is_player: True if player else False (usefull to change marker color)
:return: a ros marker msg
"""
marker = Marker(header=header)
marker.color.a = 0.3
if is_player:
marker.color.g = 1
marker.color.r = 0
marker.color.b = 0
else:
marker.color.r = 1
marker.color.g = 0
marker.color.b = 0
marker.id = marker_id
marker.text = "id = {}".format(marker_id)
update_marker_pose(object, marker)
return marker
def control_points_callback(self, msg):
subdiv_points = msg
for i in range(self.subdivisions):
subdiv_points = self.subdivide(subdiv_points)
self.curve_pub.publish(subdiv_points)
marker = Marker()
marker.header.frame_id = "map"
marker.header.stamp = rospy.Time.now()
marker.ns = "subdivision"
marker.id = 0 # self.subdivisions
marker.action = Marker.ADD
marker.type = Marker.LINE_STRIP
marker.pose.orientation.w = 1.0
marker.scale.x = 0.05
marker.scale.y = 0.05
marker.scale.z = 0.05
marker.color.r = 1.0
marker.color.g = 1.0
marker.color.b = 1.0
marker.color.a = 1.0
marker.text = str(self.subdivisions)
for pt in subdiv_points.points:
gpt = Point()
gpt.x = pt.x
gpt.y = pt.y
marker.points.append(gpt)
marker.points.append(marker.points[0])
self.marker_pub.publish(marker)
def make_label(text, position, id = 0 ,duration = 5.0, color=[1.0,1.0,1.0]):
""" Helper function for generating visualization markers.
Args:
text (str): Text string to be displayed.
position (list): List containing [x,y,z] positions
id (int): Integer identifying the label
duration (rospy.Duration): How long the label will be displayed for
color (list): List of label color floats from 0 to 1 [r,g,b]
Returns:
Marker: A text view marker which can be published to RViz
"""
marker = Marker()
marker.header.frame_id = '/world'
marker.id = id
marker.type = marker.TEXT_VIEW_FACING
marker.text = text
marker.action = marker.ADD
marker.scale.x = 0.05
marker.scale.y = 0.05
marker.scale.z = 0.05
marker.color.a = 1.0
marker.color.r = color[0]
marker.color.g = color[1]
marker.color.b = color[2]
marker.lifetime = rospy.Duration(duration)
marker.pose.orientation.w = 1.0
marker.pose.position.x = position[0]
marker.pose.position.y = position[1]
marker.pose.position.z = position[2]
return marker
def addFlag():
# get the cooprations of turtlebot in map using tf
# listener = tf.TransformListener()
# listener.waitForTransform("/odom", "/base_link", rospy.Time(0), rospy.Duration(1))
# (position, rotation) = listener.lookupTransform("/odom", "/base_link", rospy.Time(0))
global poseX, poseY, poseZ
marker_pub = rospy.Publisher("warning_marker",Marker,queue_size=1)
shape = Marker.TEXT_VIEW_FACING
for i in range(5):
marker = Marker()
marker.header.frame_id = 'map'
marker.color.r, marker.color.b, marker.color.g = 1, 0, 0
marker.color.a = 1
marker.ns = "WarningFlag"
marker.id = 1024
marker.pose.position.x, marker.pose.position.y, marker.pose.position.z = poseX, poseY , poseZ+0.5#position[0], position[1]-0.7, 0
marker.scale.x, marker.scale.y, marker.scale.z = 0.01, 0.01, 0.2
marker.type = shape
marker.text = "WARNING"
marker.lifetime = rospy.Duration(5)
marker_pub.publish(marker)
rospy.sleep(1)
def create_delay_text_marker(self, idx, header, dis):
marker = Marker()
marker.header.frame_id = header.frame_id
marker.header.stamp = header.stamp
marker.ns = self.inputTopic + "_d"
marker.action = Marker.ADD
marker.id = idx + 500 # message.track.id
marker.type = Marker.TEXT_VIEW_FACING
marker.scale.z = 1
marker.lifetime = rospy.Duration(1.0)
marker.color.r = 0
marker.color.g = 1
marker.color.b = 0
marker.color.a = 1.0
marker.text = "%.3fms" % ((rospy.get_rostime() - header.stamp).to_sec() * 1000.0)
marker.pose.position.x = dis
marker.pose.position.y = 0
marker.pose.position.z = 7
marker.pose.orientation.x = 0.0
marker.pose.orientation.y = 0.0
marker.pose.orientation.z = 0.0
marker.pose.orientation.w = 1.0
return marker
def updateText(idx, pcd, theta, centroid, prediction):
marker = Marker()
marker.header.frame_id = "/base_link"
marker.header.stamp = rospy.Time.now()
marker.id = idx
marker.type = Marker.TEXT_VIEW_FACING
marker.action = Marker.ADD
R = numpy.array([[numpy.cos(theta), -numpy.sin(theta)],
[numpy.sin(theta), numpy.cos(theta)]])
new_pcd = R.dot(
(pcd[:, :2] - centroid[:2]).transpose()).transpose() + centroid[:2]
marker.pose.position.x = new_pcd[:, 0].mean()
marker.pose.position.y = new_pcd[:, 1].mean()
marker.pose.position.z = pcd[:, 2].max()
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.text = prediction
marker.scale.x = marker.scale.y = marker.scale.z = 0.5
marker.color.r = class_to_color_rgb[prediction][0] / 255.0
marker.color.g = class_to_color_rgb[prediction][1] / 255.0
marker.color.b = class_to_color_rgb[prediction][2] / 255.0
marker.color.a = 1.0
pubMarker.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 displayETA(self):
marker = Marker()
marker.header.frame_id = "base_footprint"
marker.header.stamp = rospy.Time()
marker.id = 1234
marker.type = Marker.TEXT_VIEW_FACING
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.4
marker.scale.y = 0.1
marker.scale.z = 0.2
marker.color.a = 1.0 # Don't forget to set the alpha!
marker.color.r = 0.02
marker.color.g = 0.84
marker.color.b = 1.0
marker.text = "ETA to destination: {}".format(
self.current_plan_duration)
self.vis_pub.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 __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 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 create_line_marker(p1, p2, frame_id):
h = Header()
h.frame_id = frame_id #tie marker visualization to laser it comes from
h.stamp = rospy.Time.now() # Note you need to call rospy.init_node() before this will work
#create marker:person_marker, modify a red cylinder, last indefinitely
mark = Marker()
mark.header = h
mark.ns = "unit_vector"
mark.id = 0
mark.type = Marker.LINE_STRIP
mark.action = 0
mark.scale.x = 0.2
mark.color = color = ColorRGBA(0.2, 0.5, 0.7, 1.0)
mark.text = "unit_vector"
points = []
pt1 = Point(p1[0], p1[1], p1[2])
pt2 = Point(p2[0], p2[1], p2[2])
# print "Pt 1 ", pt1
# print "Pt 2 ", pt2
points.append(pt1)
points.append(pt2)
mark.points = points
return mark
def display_particles(poses_pub):
markerArray = MarkerArray()
#print(poses_pub[1])
#print('-------')
for i in range(len(poses_pub)):
marker = Marker()
marker.header.frame_id = "map"
marker.header.stamp = rospy.Time()
marker.ns = "turtle_bot"
marker.type = Marker.SPHERE
marker.action = Marker.ADD
marker.id = i
marker.text = "testing"
marker.pose.position.x = poses_pub[i][0]
marker.pose.position.y = poses_pub[i][1]
marker.pose.position.z = 1
marker.pose.orientation.w = 1
marker.color.r = 1.0
marker.color.g = 0.0
marker.color.b = 0.0
marker.color.a = 1.0
marker.scale.z = 0.1
marker.scale.y = 0.1
marker.scale.x = 0.1
markerArray.markers.append(marker)
map_publisher.publish(markerArray)
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 get_current_position_marker(self, link, offset=None, root="", scale=1, color=(0,1,0,1), idx=0):
(mesh, pose) = self.get_link_data(link)
marker = Marker()
if offset==None :
marker.pose = pose
else :
marker.pose = toMsg(fromMsg(offset)*fromMsg(pose))
marker.header.frame_id = root
marker.header.stamp = rospy.get_rostime()
marker.ns = self.robot_name
marker.mesh_resource = mesh
marker.type = Marker.MESH_RESOURCE
marker.action = Marker.MODIFY
marker.scale.x = scale
marker.scale.y = scale
marker.scale.z = scale
marker.color.r = color[0]
marker.color.g = color[1]
marker.color.b = color[2]
marker.color.a = color[3]
marker.text = link
marker.id = idx
marker.mesh_use_embedded_materials = True
return 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_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_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 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_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 test():
rospy.init_node('intersect_plane_test')
marker_pub = rospy.Publisher('table_marker', Marker)
pose_pub = rospy.Publisher('pose', PoseStamped)
int_pub = rospy.Publisher('intersected_points', PointCloud2)
tfl = tf.TransformListener()
# Test table
table = Table()
table.pose.header.frame_id = 'base_link'
table.pose.pose.orientation.x, table.pose.pose.orientation.y, table.pose.pose.orientation.z, table.pose.pose.orientation.w = (0,0,0,1)
table.x_min = -0.5
table.x_max = 0.5
table.y_min = -0.5
table.y_max = 0.5
# A marker for that table
marker = Marker()
marker.header.frame_id = table.pose.header.frame_id
marker.id = 1
marker.type = Marker.LINE_STRIP
marker.action = 0
marker.pose = table.pose.pose
marker.scale.x, marker.scale.y, marker.scale.z = (0.005,0.005,0.005)
marker.color.r, marker.color.g, marker.color.b, marker.color.a = (0.0,1.0,1.0,1.0)
marker.frame_locked = False
marker.ns = 'table'
marker.points = [
Point(table.x_min,table.y_min, table.pose.pose.position.z),
Point(table.x_min,table.y_max, table.pose.pose.position.z),
Point(table.x_max,table.y_max, table.pose.pose.position.z),
Point(table.x_max,table.y_min, table.pose.pose.position.z),
Point(table.x_min,table.y_min, table.pose.pose.position.z),
]
marker.colors = []
marker.text = ''
# marker.mesh_resource = ''
marker.mesh_use_embedded_materials = False
marker.header.stamp = rospy.Time.now()
# A test pose
pose = PoseStamped()
pose.header = table.pose.header
pose.pose.position.x, pose.pose.position.y, pose.pose.position.z = (0,0,0.5)
pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w = quaternion_from_euler(0,-pi/5,pi/5)
intersection = cast_ray(pose, table, tfl)
cloud = xyz_array_to_pointcloud2(np.array([[intersection.point.x, intersection.point.y, intersection.point.z]]))
cloud.header = pose.header
while not rospy.is_shutdown():
marker_pub.publish(marker)
pose_pub.publish(pose)
int_pub.publish(cloud)
rospy.loginfo('published')
rospy.sleep(0.1)
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 poseupdate():
#set up marker
marker_scale = 0.4
marker_lifetime = 0
marker_ns = 'waypoint'
marker_id = 0
marker_color= {'r':1.0, 'g':0.0, 'b':0.0, 'a':1.0}
marker_pub = rospy.Publisher('waypoint_marker', Marker)
marker = Marker()
marker.ns = marker_ns
marker.id = marker_id
marker.type = Marker.CUBE_LIST
marker.action = Marker.ADD
marker.text = "marker"
marker.lifetime = rospy.Duration(marker_lifetime)
marker.scale.x = marker_scale
marker.scale.y = marker_scale
marker.scale.z = marker_scale
marker.color.r = marker_color['r']
marker.color.g = marker_color['g']
marker.color.b = marker_color['b']
marker.color.a = marker_color['a']
marker.header.frame_id = '/map'
marker.header.stamp = rospy.Time.now()
marker.points = list()
# for waypoint in waypoints:
# p = Point()
# p = waypoint.position
# marker.points.append(p)
# i = 0
# while i < 4 and not rospy.is_shutdown():
# marker_pub.publish(marker)
# i += 1
p1 = Point(0.502, 11.492, 0.000) #room1 Position(0.718, -0.571, 0.000),Orientation(0.000, 0.000, -0.008, 1.000) = Angle: -0.015
p2 = Point(-3.66308784485,13.2560949326, 0.000) #room2 Orientation(0.000, 0.000, -0.741, 0.672) = Angle: -1.668
p3 = Point(-0.00961661338806, 9.34885692596, 0.000) #room3 Position(0.782, -2.700, 0.000), Orientation(0.000, 0.000, -0.052, 0.999) = Angle: -0.105
p4 = Point(-4.05501270294,10.2383213043, 0.000) #room4 Position(-0.343, -6.387, 0.000), Orientation(0.000, 0.000, -0.667, 0.745) = Angle: -1.459
p5 = Point(-3.27898073196, 6.35970401764, 0.000) #door1 , Position(-1.134, -1.524, 0.000), Orientation(0.000, 0.000, -0.704, 0.710) = Angle: -1.562
p6 = Point( -1.94060504436, 0.0072660446167, 0.000) #door2 Position(-0.203, -3.766, 0.000), Orientation(0.000, 0.000, -0.717, 0.697) = Angle: -1.599
p7 = Point(1.34751307964, 0.948370933533, 0.000) #desk Position(-1.579, -3.107, 0.000), Orientation(0.000, 0.000, 0.012, 1.000) = Angle: 0.023
p8 = Point(-1.64714360237,4.02495145798, 0.000) #cooridor Position(-0.175, -4.631, 0.000), Orientation(0.000, 0.000, -0.717, 0.697) = Angle: -1.599
p9 = Point(-3.74548912048,3.87098288536,0.000)
marker.points.append(p1)
marker.points.append(p2)
marker.points.append(p3)
marker.points.append(p4)
marker.points.append(p5)
marker.points.append(p6)
marker.points.append(p7)
marker.points.append(p8)
marker.points.append(p9)
while not rospy.is_shutdown():
marker_pub.publish(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 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 publishState(self):
self.pubState.publish(str(self.state))
marker = Marker()
marker.header.frame_id = "/map"
marker.header.stamp = rospy.get_rostime()
marker.type = marker.TEXT_VIEW_FACING
marker.action = marker.ADD
marker.scale.x = 1.0
marker.scale.y = 0.2
marker.scale.z = 0.5
marker.color.a = 1.0
marker.color.r = 1.0
marker.color.g = 0.0
marker.color.b = 0.0
marker.pose.orientation.w = 1.0
marker.pose.position.x = self.infoLoc["x"]
marker.pose.position.y = self.infoLoc["y"]
marker.pose.position.z = 0.0
marker.id = 0
if self.state in [State.RUNNING]:
if not self.sendStart:
marker.text = "Start"
self.sendStart = True
self.timeSend = rospy.get_rostime()
elif (rospy.get_rostime() - self.timeSend).to_sec() > 0.2:
# marker.type = marker.SPHERE
marker.action = marker.DELETE
else:
marker.text = "Start"
if self.state == State.FLEEING:
timeFleeing = TIME_FLEEING - (rospy.get_rostime() - self.timeStartFleeing).to_sec()
if timeFleeing > 0:
marker.text = str(timeFleeing)
else:
marker.action = marker.DELETE
self.state = State.RUNNING
if self.state in [State.GAME_OVER, State.STOPPED, State.SETUP]:
marker.text = "GAME OVER!"
if self.state in [State.INIT]:
marker.text = "Initializing Game"
if self.state in [State.PAUSED]:
marker.text = "Game Paused"
if self.state in [State.WON]:
marker.text = "Congratulations, you have won the Game! Now get back to you real job!"
self.pubInfo.publish(marker)
def create_status_marker(self, text): marker = Marker() marker.header.frame_id = 'base_footprint' marker.header.stamp = rospy.Time.now() marker.pose.position.z += 1 marker.pose.orientation.w = 1 marker.type = Marker.TEXT_VIEW_FACING marker.text = text marker.frame_locked = True marker.ns = 'status' marker.scale.z = 0.3 marker.color.r = 0.8 marker.color.g = 0.5 marker.color.b = 0.1 marker.color.a = 1.0 return marker
def create_marker(self, marker_id, all_tags):
marker = Marker()
marker.header.stamp = rospy.Time.now()
marker.header.frame_id = "/map"
marker.type = marker.TEXT_VIEW_FACING
marker.id = marker_id
marker.action = marker.ADD
marker.ns = 'tags'
#marker.lifetime = 0
marker.scale.x = 0.1
marker.scale.y = 0.1
marker.scale.z = 0.1
marker.color.a = 1
marker.pose = self.pp.pose
tag_str = [all_tags[i] for i in self.pp.tags]
marker.text = '\n'.join(tag_str)
return marker
def init_markers(self):
# Set up our waypoint markers
marker_scale = 0.6
marker_lifetime = 0 # 0 is forever
marker_ns = 'waypoints'
marker_id = 0
marker_color = {'r': 1.0, 'g': 0.0, 'b': 0.0, 'a': 1.0}
# Define a marker publisher list.
location_list = list()
location_list = self.locations.keys()
self.marker_pub_list = list()
for i in location_list:
self.marker_pub_list.append(rospy.Publisher('waypoint_' + i, Marker, queue_size=5))
# Define a marker list.
self.marker_list = list()
for location in self.locations:
marker = Marker()
marker.ns = marker_ns
marker.id = marker_id
marker.type = Marker.TEXT_VIEW_FACING
marker.action = Marker.ADD
marker.lifetime = rospy.Duration(marker_lifetime)
marker.scale.x = marker_scale
marker.scale.y = marker_scale
marker.scale.z = marker_scale
marker.color.r = marker_color['r']
marker.color.g = marker_color['g']
marker.color.b = marker_color['b']
marker.color.a = marker_color['a']
marker.header.frame_id = 'map'
marker.header.stamp = rospy.Time.now()
# rospy.loginfo(self.locations['foyer'])
marker.pose = copy.deepcopy(self.locations[location])
marker.pose.position.z = 0.65
marker.text = location
# rospy.loginfo(self.locations['foyer'])
self.marker_list.append(marker)
def offset_for_target(self):
# Move off of center of shape to be centered with large target
move = self.navigator.move.forward(self.target_offset_meters)
marker = Marker()
marker.scale.x = 1;
marker.scale.y = .1;
marker.scale.z = .1;
marker.action = Marker.ADD;
marker.header.frame_id = "enu"
marker.header.stamp = self.navigator.nh.get_time()
marker.pose = self.navigator.move.goal.goal
marker.type = Marker.ARROW
marker.text = "Offset goal"
marker.ns = "detect_deliver"
marker.id = 3
marker.color.b = 1
marker.color.a = 1
self.markers.markers.append(marker)
yield move
def transformStampedArrayToLabeledArrayMarker(cls, tsdata_lst, fmt="%Y-%m-%d %H:%M:%S", zoffset=0.05, label_downsample=1, discrete=False):
"[[transformStamped, meta],...] -> LineStrip / String"
h = Header()
h.stamp = rospy.Time(0) #rospy.Time.now()
h.frame_id = "eng2" #t_first.child_frame_id
res = []
t_first = tsdata_lst[0][0]
prev_t = t_first.transform.translation
for ts, _ in tsdata_lst:
m = Marker(type=Marker.ARROW,
action=Marker.ADD,
header=h,
id=cls.marker_id)
cls.marker_id += 1
t = ts.transform.translation
dist = distanceOfVector3(prev_t, t) * 0.65
rgb = colorsys.hsv_to_rgb(dist, 1.0, 1.0)
m.points = [Point(x=prev_t.x,y=prev_t.y,z=prev_t.z),
Point(x=(prev_t.x + t.x) / 2.,y=(prev_t.y + t.y) /2.,z=(prev_t.z + t.z) /2.)]
m.ns = "labeled_line"
m.lifetime = rospy.Time(3000)
m.scale.x, m.scale.y, m.scale.z = 0.02, 0.06, 0.1
m.color = ColorRGBA(rgb[0],rgb[1],rgb[2],1.0)
if dist < 0.65:
res.append(m)
prev_t = t
for t, t_meta in tsdata_lst[::label_downsample]:
text = Marker(type=Marker.TEXT_VIEW_FACING,
action=Marker.ADD,
header=h,
id=cls.marker_id)
cls.marker_id += 1
text.scale.z = 0.1
text.pose = T.poseFromTransform(t.transform)
text.pose.position.z += zoffset
text.color = ColorRGBA(0.0,0.0,1.0,1.0)
text.text = t_meta["inserted_at"].strftime(fmt)
text.ns = "labeled_line_text"
text.lifetime = rospy.Time(3000)
res.append(text)
return res
def 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 _visualize_location(self, base_pose, location):
"""
Visualize a marker on the base_pose with the text 'location' and rotated to the correct side.
:param base_pose: kdl Frame (in map) where the waypoint is located
:param location: The name of the location as a label
:return:
"""
# Convert KDL object to geometry message
base_pose = kdl_frame_to_pose_msg(base_pose)
# Create the marker
m = Marker()
if location == "one":
m.id = 1
m.color.r = 1
elif location == "two":
m.id = 2
m.color.g = 1
elif location == "three":
m.id = 3
m.color.b = 1
else:
m.id = 4
m.color.r = 1
m.color.g = 1
m.color.b = 1
m.color.a = 1
m.pose = base_pose
m.header.frame_id = "map"
m.header.stamp = rospy.Time.now()
m.type = 0 # Arrow
m.scale.x = 1.0
m.scale.y = 0.2
m.scale.z = 0.2
m.action = 0
m.ns = "arrow"
self._waypoint_pub.publish(m)
m.type = 9
m.text = location
m.ns = "text"
m.pose.position.z = 0.5
self._waypoint_pub.publish(m)
