def switch_lights_parser(self, lights_dict):
"""
Parse and use action to switch lights
Parameters
----------
lights_dict: dictionary of light switching parameters
"""
light_name = lights_dict["target_light"]
diffuse = ColorRGBA()
if lights_dict["on"] == True:
diffuse.r = 0.5
diffuse.g = 0.5
diffuse.b = 0.5
diffuse.a = 1.0
attenuation_linear = 0.01
attenuation_quadratic = 0.001
elif lights_dict["on"] == False:
diffuse.r = 0.0
diffuse.g = 0.0
diffuse.b = 0.0
diffuse.a = 0.0
attenuation_linear = 0.00
attenuation_quadratic = 0.0000
else:
rospy.logwarn("invalid light on state parsed")
return
self.set_light_properties(light_name, diffuse, 0.0, attenuation_linear,
attenuation_quadratic)
return
def get_traj_msg(paths, opt_ind):
ma = MarkerArray()
for id, path in enumerate(paths):
m = Marker()
m.header.frame_id = "/map"
m.header.stamp = rospy.Time.now()
m.id = (id + 1) * 10
m.type = m.POINTS
m.lifetime.secs = 1
c = ColorRGBA()
if opt_ind == id:
m.scale.x = 0.5
m.scale.y = 0.5
m.scale.z = 0.5
c.r = 0 / 255.0
c.g = 0 / 255.0
c.b = 255 / 255.0
c.a = 1
else:
m.scale.x = 0.2
m.scale.y = 0.2
m.scale.z = 0.2
c.r = 255 / 255.0
c.g = 0 / 255.0
c.b = 0 / 255.0
c.a = 0.2
for x, y in zip(path.x, path.y):
p = Point()
p.x, p.y = x, y
m.points.append(p)
m.colors.append(c)
ma.markers.append(m)
return ma
def on_radar_tracks(self, msg):
marker = Marker()
marker.header.frame_id = "middle_radar_link"
if rospy_compat.use_ros_1:
marker.header.stamp = msg.header.stamp
marker.ns = "radar_tracks"
marker.id = 1
marker.type = Marker.POINTS
marker.action = Marker.MODIFY
marker.scale.x = 0.2
marker.scale.y = 0.2
marker.scale.z = 0.1
marker.color.r = 1.0
marker.color.a = 1.0
marker.points = []
for track in msg.radar_tracks:
p = Point()
p.x = float(track.lng_dist)
p.y = -float(track.lat_dist)
p.z = 0.0
c = ColorRGBA()
c.a = float(track.valid_count) / RADAR_VALID_MAX
if track.valid == True:
c.r = 1.0
else:
c.a = 1.0
c.r = c.g = c.b = 0.5
marker.points.append(p)
marker.colors.append(c)
self.radar_rviz_pub.publish(marker)
def PlotLines(self, path, num):
Predict_Distance = num * 2
Forward_Distance = num
print '\n\ninto PlotLines'
if len(self.path) >= (num + Predict_Distance):
Front1 = copy.deepcopy(path[:(num + Predict_Distance)])
Front2 = copy.deepcopy(path[:(num + Forward_Distance)])
color = ColorRGBA()
color.a = 1
color.r = 1
colors = [color, color]
color = ColorRGBA()
color.b = 1
color.a = 1
colors.extend([color, color])
points = [
Front1[0].pose.position, Front1[-1].pose.position,
Front2[0].pose.position, Front2[-1].pose.position
]
lines = self.visual_test(points, Marker.LINE_LIST, colors,
self.scale)
lines_pub = rospy.Publisher("/straight_lines",
Marker,
queue_size=1)
lines_pub.publish(lines)
self.FrontMAX(Predict_Distance, Forward_Distance)
print 'Predict_Distance: ', Predict_Distance, 'Forward_Distance: ', Forward_Distance
def Colormap(self, ii, jj):
p = self._visualMap[ii, jj]
c = ColorRGBA()
if p == 2:
c.r = 0.1
c.g = 1.0
c.b = 0.1
c.a = 0.75
elif p == 3:
c.r = 1.0
c.g = 1.0
c.b = 0.1
c.a = 0.75
elif p == 4:
c.r = 1.0
c.g = 1.0
c.b = 1.0
c.a = 0.75
elif p == 5:
c.r = 1.0
c.g = 0.1
c.b = 1.0
c.a = 0.75
else:
c.r = p
c.g = 0.1
c.b = 1.0 - p
c.a = 0.3
return c
def get_marker_pc_for_track(self, lat_dist, lng_dist, valid_count):
p = Point()
p.x = float(lng_dist)
p.y = -float(lat_dist)
p.z = 0.0
c = ColorRGBA()
c.a = float(valid_count) / RADAR_VALID_MAX
if valid_count > 0:
c.r = 1.0
else:
c.a = 1.0
c.r = c.g = c.b = 0.5
return p, c
def visualize_occugrid(self, time):
"""
Visualizes occupancy grid for all grids in time
"""
if self.occupancy_grids is not None:
marker = Marker()
marker.header.frame_id = "/world"
marker.header.stamp = rospy.Time.now()
marker.id = 0
marker.ns = "visualize"
marker.type = marker.CUBE_LIST
marker.action = marker.ADD
marker.scale.x = self.res
marker.scale.y = self.res
marker.scale.z = self.human_height
for t in range(time):
grid = self.interpolate_grid(t)
if grid is not None:
for i in range(len(grid)):
(row, col) = self.state_to_coor(i)
real_coord = self.sim_to_real_coord([row, col])
color = ColorRGBA()
color.a = np.sqrt(
(1 - (time - 1) / self.fwd_tsteps) * grid[i])
color.r = np.sqrt(grid[i])
color.g = np.minimum(np.absolute(np.log(grid[i])),
5.0) / 5.0
color.b = 0.9 * np.minimum(
np.absolute(np.log(grid[i])),
5.0) / 5.0 + 0.5 * np.sqrt(grid[i])
if grid[i] < self.prob_thresh:
color.a = 0.0
marker.colors.append(color)
pt = Vector3()
pt.x = real_coord[0]
pt.y = real_coord[1]
pt.z = self.human_height / 2.0
marker.points.append(pt)
#print "real_coord: ", real_coord
#print "coord prob: ", grid[i]
self.grid_vis_pub.publish(marker)
def pixel_to_color(x, y, image): color = ColorRGBA() color.r = image[x][y][0] color.g = image[x][y][1] color.b = image[x][y][2] color.a = 1.0 return color
def __init__(self, node):
self._node = node
self._node.get_logger().info('Init ObjectMarker()')
# Initial default marker properties
self.marker = Marker()
self.marker.color.r = 0.0
self.marker.color.g = 1.0
self.marker.color.b = 0.0
self.marker.color.a = 1.0
self.marker.scale.x = 0.0
self.marker.scale.y = 0.0
self.marker.scale.z = 0.0
line_color = ColorRGBA()
line_color.r = 1.0
line_color.g = 1.0
line_color.b = 0.0
line_color.a = 1.0
self.marker.colors.append(line_color)
self.marker.colors.append(line_color)
# Marker topic on Rviz
self._markers_pub = self._node.create_publisher(
MarkerArray, '/object_map/Markers')
def StraightLine(self, num):
Front = copy.deepcopy(self.path[:num * 2])
FrontLine = CVlib.SLF()
result = FrontLine.OLS(Front)
print 'StraightLine length: ', num, 'path length: ', len(self.path)
#print result
if len(self.path) < 2 * num:
pass
else:
blue = ColorRGBA()
blue.b = 1
blue.a = 1
point_marker = Marker()
point_marker.header.frame_id = '/map'
point_marker.header.stamp = rospy.Time.now()
point_marker.ns = ''
point_marker.action = Marker.ADD
point_marker.id = 0
point_marker.type = Marker.POINTS
point_marker.scale.x = 0.05
point_marker.scale.y = 0.05
for i in range(num):
point_marker.points.append(self.path[num + i].pose.position)
point_marker.colors.append(blue)
point_marker.lifetime = rospy.Duration(0.5)
line_pub = rospy.Publisher("/straight_line", Marker, queue_size=1)
line_pub.publish(point_marker)
def __init__(self, parent=None, width=5, height=5, dpi=100):
#pl.ion()
self.fig = Figure(figsize=(width, height), dpi=dpi)
self.canvas = FigureCanvas(self.fig)
self.canvas.setParent(parent)
self.canvas.mpl_connect('button_press_event', self.on_click)
self.rho_area = self.fig.add_subplot(111)
#self.rho_area = self.fig.add_subplot(211)
self.rho_area.set_title("rho", fontsize=11)
self.fig.tight_layout()
#ROS
rospy.init_node('cor_joints', anonymous=True)
self.mpub = rospy.Publisher('visualization_marker_array',
MarkerArray,
queue_size=10)
#rvizのカラー設定(未)
self.carray = []
clist = [[1, 0, 0, 1], [0, 1, 0, 1], [1, 1, 0, 1], [1, 0.5, 0, 1]]
for c in clist:
color = ColorRGBA()
color.r = c[0]
color.g = c[1]
color.b = c[2]
color.a = c[3]
self.carray.append(color)
self.jidx = [
[11, 3, 2, 10, 1, 0],
[10, 4, 5, 6],
[10, 7, 8, 9],
]
def adddisplay(points):
pub = rospy.Publisher("visualization_marker", Marker, queue_size=10)
marker = Marker()
marker.header.frame_id = "/world"
marker.type = marker.POINTS
marker.action = marker.ADD
marker.id = 1
marker.scale.x = 0.25
marker.scale.y = 0.25
marker.scale.z = 0.25
result_v = addVoronoi(points)
result = result_v
storepoint=[]
for i in range(len(result)):
for j in range(len(result[i])):
point = Point()
color = ColorRGBA()
color.b = 1
color.g = 1
color.r = 0
color.a = 1
point.x = result[i][j,0]
point.y = result[i][j,1]
point.z = result[i][j,2]
if point.z >= 0:
marker.points.append(point)
marker.colors.append(color)
storepoint.append(point)
pub.publish(marker)
return storepoint
def color(r,g,b,a=1): out = ColorRGBAMsg() out.r = r out.g = g out.b = b out.a = a return out
def compute_rgb(data):
# print(data)
color = ColorRGBA()
if data < 1.0 / 3:
color.r = 1.0
color.g = 3.0 * data
color.b = 0.0
if data >= 1.0 / 3 and data < 1.0 / 2:
color.r = 3 - 6.0 * data
color.g = 1.0
color.b = 0.0
if data >= 1.0 / 2 and data < 2.0 / 3:
color.r = 0.0
color.g = 1.0
color.b = 6.0 * data - 3
if data >= 2.0 / 3 and data < 5.0 / 6:
color.r = 0.0
color.g = 5 - 6.0 * data
color.b = 1.0
if data >= 5.0 / 6 and data <= 1.0:
color.r = (900.0 * data - 750) / 255.0
color.g = 0.0
color.b = 1.0
color.a = 1.0
return color
def show_text_in_rviz_mullti_line(marker_pub, text):
line_color = ColorRGBA() # a nice color for my line (royalblue)
line_color.r = 0.254902
line_color.g = 0.411765
line_color.b = 0.882353
line_color.a = 1.0
start_point = Point() # start point
start_point.x = 0.2
start_point.y = 0.0
start_point.z = 0.2
end_point = Point() # end point
end_point.x = 20
end_point.y = 20
end_point.z = 20
marker3 = Marker()
marker3.id = 3
marker3.header.frame_id = 'world'
marker3.type = Marker.LINE_STRIP
marker3.ns = 'Testline'
marker3.action = Marker.ADD
marker3.scale.x = 0.8
marker3.points.append(start_point)
marker3.points.append(end_point)
marker3.colors.append(line_color)
marker3.colors.append(line_color)
marker_pub.publish(marker3)
def draw_footprint(self, points):
# print 'PRINT ARIS FOOTPRINT'
marker = Marker()
marker.header.stamp = rospy.Time.now()
marker.header.frame_id = "/girona500"
marker.ns = "aris_foot_print"
marker.id = 1
marker.type = 4 # SPHERE
marker.action = 0 # Add/Modify an object
marker.pose.position.x = 0
marker.pose.position.y = 0
marker.pose.position.z = 0
marker.scale.x = 0.1
marker.scale.y = 0.1
marker.scale.z = 0.1
marker.color.r = 0.0
for i in points:
p = Point()
p.x = i[0]
p.y = i[1]
p.z = i[2]
color = ColorRGBA()
color.r = 0.0
color.g = 0.5
color.b = 0.5
color.a = 0.7
marker.points.append(p)
marker.colors.append(color)
marker.lifetime = rospy.Duration(0.5)
marker.frame_locked = False
self.pub_aris_footprint.publish(marker)
def to_msg(self):
msg = ColorRGBA()
msg.r = self.r / 255.0
msg.g = self.g / 255.0
msg.b = self.b / 255.0
msg.a = 1.0
return msg
def callback(joy): motor = ColorRGBA() color = ColorRGBA() if joy.buttons[0]>0: color.r = 0 color.g = 0 color.b = 150 if joy.buttons[1]>0: color.r = 0 color.g = 150 color.b = 0 if joy.buttons[2]>0: color.r = 150 color.g = 0 color.b = 0 if joy.buttons[3]>0: color.r = 150 color.g = 80 color.b = 0 y=joy.axes[3]*100.0 x=joy.axes[2]*-100.0 ##Ouputs throttle from -100 to 100 f = math.fabs(y/100.0) left = y*f+(1-f)*x right = y*f -(1-f)*x #Change to 0..127..255 motor.b = left*254.0/200.0 +127.0 motor.a = left*254.0/200.0 +127.0 motor.r = right*254.0/200.0 +127.0 motor.g = right*254.0/200.0 +127.0 pub_motor.publish(motor) pub_color.publish(color)
def meshToRos(mesh):
triangles = np.asarray(mesh.triangles)
vertices = np.asarray(mesh.vertices)
vertex_colors = np.asarray(mesh.vertex_colors)
out_msg = Marker()
out_msg.type = out_msg.TRIANGLE_LIST
out_msg.action = out_msg.ADD
out_msg.id = 1
out_msg.scale.x = 1.0
out_msg.scale.y = 1.0
out_msg.scale.z = 1.0
out_msg.pose.position.x = 0
out_msg.pose.position.y = 0
out_msg.pose.position.z = 0
out_msg.pose.orientation.w = 1
out_msg.pose.orientation.x = 0
out_msg.pose.orientation.y = 0
out_msg.pose.orientation.z = 0
for triangle in triangles:
for vertex_index in triangle:
curr_point = Point()
curr_point.x = vertices[vertex_index][0]
curr_point.y = vertices[vertex_index][1]
curr_point.z = vertices[vertex_index][2]
curr_point_color = ColorRGBA()
curr_point_color.r = vertex_colors[vertex_index][2]
curr_point_color.g = vertex_colors[vertex_index][1]
curr_point_color.b = vertex_colors[vertex_index][0]
curr_point_color.a = 1
out_msg.points.append(curr_point)
out_msg.colors.append(curr_point_color)
return out_msg
def getLineColor(self):
result = ColorRGBA()
result.a = 1
result.r = 0.8
result.g = 0.1
result.b = 0.1
return result
def pub_color(self, r, g, b, a):
color_msg = ColorRGBA()
color_msg.r = r
color_msg.g = g
color_msg.b = b
color_msg.a = a
self.color_pub.publish(color_msg)
def perform(self, reevaluate=False):
pose_msg = self.blackboard.pathfinding.get_ball_goal(
self.target, self.distance)
self.blackboard.pathfinding.publish(pose_msg)
approach_marker = Marker()
approach_marker.pose.position.x = self.distance
approach_marker.type = Marker.SPHERE
approach_marker.action = Marker.MODIFY
approach_marker.id = 1
color = ColorRGBA()
color.r = 1.0
color.g = 1.0
color.b = 1.0
color.a = 1.0
approach_marker.color = color
approach_marker.lifetime = rospy.Duration(nsecs=0.5)
scale = Vector3(0.2, 0.2, 0.2)
approach_marker.scale = scale
approach_marker.header.stamp = rospy.Time.now()
approach_marker.header.frame_id = self.blackboard.world_model.base_footprint_frame
self.blackboard.pathfinding.approach_marker_pub.publish(
approach_marker)
if self.blackboard.pathfinding.status in [
GoalStatus.SUCCEEDED, GoalStatus.ABORTED
] or not self.blocking:
self.pop()
def __init__(self):
super(CCA, self).__init__()
#UIの初期化
self.initUI()
#ファイル入力
#self.jsonInput()
#ROSのパブリッシャなどの初期化
rospy.init_node('ros_cca_table', anonymous=True)
self.mpub = rospy.Publisher('visualization_marker_array', MarkerArray, queue_size=10)
self.ppub = rospy.Publisher('joint_diff', PointStamped, queue_size=10)
self.carray = []
clist = [[1,1,0,1],[0,1,0,1],[1,0,0,1]]
for c in clist:
color = ColorRGBA()
color.r = c[0]
color.g = c[1]
color.b = c[2]
color.a = c[3]
self.carray.append(color)
def goal_parts_cb(self, msg: PoseWithCertaintyArray):
arr = []
i = 0
for post in msg.poses:
post_marker = Marker()
pose = Pose()
pose.position = post.pose.pose.position
post_marker.pose = pose
post_marker.pose.position.z = self.post_height / 2
post_marker.pose.orientation = Quaternion()
post_marker.pose.orientation.x = 0
post_marker.pose.orientation.y = 0
post_marker.pose.orientation.z = 0
post_marker.pose.orientation.w = 1
post_marker.type = Marker.CYLINDER
post_marker.action = Marker.MODIFY
post_marker.id = i
post_marker.ns = "rel_goal_parts"
color = ColorRGBA()
color.r = 1.0
color.g = 1.0
color.b = 1.0
color.a = 1.0
post_marker.color = color
post_marker.lifetime = rospy.Duration(nsecs=self.goal_lifetime)
scale = Vector3(self.post_diameter, self.post_diameter,
self.post_height)
post_marker.scale = scale
post_marker.header = msg.header
arr.append(post_marker)
i += 1
self.goal_parts_marker.markers = arr
self.marker_array_publisher.publish(self.goal_parts_marker)
def gen_color(r, g, b):
c = ColorRGBA()
c.r = float(r)
c.g = float(g)
c.b = float(b)
c.a = 1.0
return c
def to_msg(self):
msg = ColorRGBA()
msg.r = self.r / 255.0
msg.g = self.g / 255.0
msg.b = self.b / 255.0
msg.a = 1.0
return msg
def make_color_msg(self, rgba):
color = ColorRGBA()
color.r = rgba[0]
color.g = rgba[1]
color.b = rgba[2]
color.a = 1 if len(rgba) < 4 else rgba[3]
return color
def findNextGoalPt(self, speed, angle=0.0):
"""
Finds the point on the subsampled trajectory that is closest to the desired
lookahead distance (lookahead is proportional to speed), and returns that as a local goal.
"""
lookahead = self.getLookaheadDist(speed)
rospy.loginfo('Lookahead Distance: %f' % lookahead)
self.curr_traj_goal_pt = self.findPtAtDist(lookahead)
self.goal_pose = self.traj_pts[self.curr_traj_goal_pt]
if (self.curr_traj_goal_pt >= len(self.traj_pts) - 5):
self.seeking_end_pt = True
if self.end_time == 0.0:
self.end_time = rospy.get_time()
for p in self.robot_path:
pt = Point32()
pt.x = p[0]
pt.y = p[1]
pt.z = 0.0
color = ColorRGBA()
color.r = 1.0 - p[2]
color.g = p[2]
color.b = 0.0
color.a = 0.75
self.robot_path_marker.points.append(pt)
self.robot_path_marker.colors.append(color)
self.robot_path_pub.publish(self.robot_path_marker)
return
def createMarkerVolume(name,
points=[],
color=[1.0, 1.0, 1.0],
map_resolution=0.01,
base_frame='world'):
marker = Marker()
marker.header.frame_id = base_frame
marker.type = Marker.CUBE_LIST
marker.id = 0
marker.action = marker.ADD
marker.ns = name
marker.scale.x = map_resolution
marker.scale.y = map_resolution
marker.scale.z = map_resolution
for p in points:
pp = Point()
pp.x = p[0]
pp.y = p[1]
pp.z = p[2]
marker.points.append(pp)
cc = ColorRGBA()
cc.r = color[0]
cc.g = color[1]
cc.b = color[2]
cc.a = 1.0
marker.colors.append(cc)
return marker
def NewColor(r, g, b, a=1.0):
col = ColorRGBA()
col.r = r
col.g = g
col.b = b
col.a = a
return col
def changeColor(self):
command = ColorRGBA()
command.r = int(self.redBox.checkState())
command.g = int(self.greenBox.checkState())
command.b = int(self.blueBox.checkState())
command.a = 0
self.pub2.publish(command)
def make_edge_marker(self):
self.edge_marker = Marker()
self.edge_marker.header.frame_id = self.map_data['MAP_FRAME']
self.edge_marker.header.stamp = rospy.get_rostime()
self.edge_marker.id = 0
self.edge_marker.action = Marker().ADD
self.edge_marker.type = Marker().LINE_LIST
self.edge_marker.lifetime = rospy.Duration()
self.edge_marker.pose.orientation = Quaternion(w=1, x=0, y=0, z=0)
self.edge_marker.scale.x = 0.8
self.edge_marker.ns = "edge"
self.edge_marker.frame_locked = True
for edge in self.map_data['EDGE']:
p0 = Point()
p0.x = self.map_data['NODE'][self.get_index_from_id(
edge['node_id'][0])]['point']['x']
p0.y = self.map_data['NODE'][self.get_index_from_id(
edge['node_id'][0])]['point']['y']
p1 = Point()
p1.x = self.map_data['NODE'][self.get_index_from_id(
edge['node_id'][1])]['point']['x']
p1.y = self.map_data['NODE'][self.get_index_from_id(
edge['node_id'][1])]['point']['y']
self.edge_marker.points.append(p0)
self.edge_marker.points.append(p1)
color = ColorRGBA()
color.r = 0.0
color.g = 0.0
color.b = 1.0
color.a = 0.7
self.edge_marker.colors.append(color)
self.edge_marker.colors.append(color)
def SetColour(colour):
rgb_colour = ColorRGBA()
rgb_colour.a = 1.0
if colour == "RED":
rgb_colour.r = 1.0
rgb_colour.g = 0.0
rgb_colour.b = 0.0
return rgb_colour
if colour == "GREEN":
rgb_colour.r = 0.0
rgb_colour.g = 1.0
rgb_colour.b = 0.0
return rgb_colour
if colour == "BLUE":
rgb_colour.r = 0.0
rgb_colour.g = 0.0
rgb_colour.b = 1.0
return rgb_colour
if colour == "RANDOM":
rgb_colour.r = random.random()
rgb_colour.g = random.random()
rgb_colour.b = random.random()
return rgb_colour
assert (), "COLOUR not supported, changed to default RED type"
rgb_colour.r = 1.0
rgb_colour.g = 0.0
rgb_colour.b = 0.0
return rgb_colour
def __init__(self):
super(CCA, self).__init__()
#UIの初期化
self.initUI()
#ROSのパブリッシャなどの初期化
rospy.init_node('roscca', anonymous=True)
self.mpub = rospy.Publisher('visualization_marker_array', MarkerArray, queue_size=10)
self.ppub = rospy.Publisher('joint_diff', PointStamped, queue_size=10)
#rvizのカラー設定(未)
self.carray = []
clist = [[1,1,0,1],[0,1,0,1],[1,0,0,1]]
for c in clist:
color = ColorRGBA()
color.r = c[0]
color.g = c[1]
color.b = c[2]
color.a = c[3]
self.carray.append(color)
self.fnames=["20151222_a1.json","20151222_a2.json","20151222_a3.json","20151222_b1.json","20151222_b2.json","20151222_b3.json"]
#self.fnames=["20151222_a1.json"]
self.winSizes = [90]
def PubcmdCB(self, event):
robot_orien_pub = rospy.Publisher("/robot_orientation",
Marker,
queue_size=1)
color = ColorRGBA()
scale = Point()
scale.x = 0.05
scale.y = 0.05
scale.z = 0.05
color.r = 2.0
color.g = 0.0
color.b = 0.0
color.a = 1.0
point_marker = Marker()
point_marker.header.frame_id = '/map'
point_marker.header.stamp = rospy.Time.now()
point_marker.ns = 'robot_uni_marker'
point_marker.action = Marker.ADD
point_marker.id = 0
point_marker.type = Marker.ARROW
point_marker.scale.x = scale.x * 5
point_marker.scale.y = scale.y / 2
point_marker.scale.z = scale.z / 2
point_marker.pose = self.marker.pose
point_marker.pose.position.z = self.height + 0.1
point_marker.color = color
point_marker.lifetime = rospy.Duration(0.1)
robot_orien_pub.publish(point_marker)
def __init__(self):
super(CCA, self).__init__()
#UIの初期化
self.initUI()
#クラス間のデータ渡しテスト
self.test = 100
#ROSのパブリッシャなどの初期化
rospy.init_node('roscca', anonymous=True)
self.mpub = rospy.Publisher('visualization_marker_array', MarkerArray, queue_size=10)
self.ppub = rospy.Publisher('joint_diff', PointStamped, queue_size=10)
#rvizのカラー設定(未)
self.carray = []
clist = [[1,1,0,1],[0,1,0,1],[1,0,0,1]]
for c in clist:
color = ColorRGBA()
color.r = c[0]
color.g = c[1]
color.b = c[2]
color.a = c[3]
self.carray.append(color)
def hex_to_color_msg(hex_str):
rgb = struct.unpack('BBB', hex_str.decode('hex'))
msg = ColorRGBA()
msg.r = rgb[0]
msg.g = rgb[1]
msg.b = rgb[2]
msg.a = 1
return msg
def robot_position_marker(self): color = ColorRGBA() scale = Point() scale.x = 0.1 scale.y = 0.1 scale.z = 0.1 color.r = 1.0 color.a = 1.0 self.robot_position = self.visual_test(self.pose, Marker.CUBE, color, scale)
def get_color(self, obj):
if obj in self.objects:
return self.objects[obj]
else:
c = ColorRGBA()
c.r, c.g, c.b = self.COLORS[len(self.objects) % len(self.COLORS)]
c.a = 1.0
self.objects[obj] = c
return c
def static_area_makers(self): color = ColorRGBA() scale = Point() scale.x = 0.05 scale.y = 0.05 color.r = 1.0 color.g = 1.0 color.b = 0.0 color.a = 1.0 self.points_marker = self.visual_test(self.static_area, Marker.POINTS, color, scale)
def create_trajectory_marker(self, traj):
# create an interactive marker for our server
int_marker = InteractiveMarker()
int_marker.header.frame_id = "/map"
int_marker.name = traj.uuid
int_marker.description = traj.uuid
pose = Pose()
pose.position.x = traj.pose[0]['position']['x']
pose.position.y = traj.pose[0]['position']['y']
int_marker.pose = pose
line_marker = Marker()
line_marker.type = Marker.LINE_STRIP
line_marker.scale.x = 0.05
# random.seed(traj.uuid)
# val = random.random()
# line_marker.color.r = r_func(val)
# line_marker.color.g = g_func(val)
# line_marker.color.b = b_func(val)
# line_marker.color.a = 1.0
line_marker.points = []
MOD = 1
for i, point in enumerate(traj.pose):
if i % MOD == 0:
x = point['position']['x']
y = point['position']['y']
p = Point()
p.x = x - int_marker.pose.position.x
p.y = y - int_marker.pose.position.y
line_marker.points.append(p)
line_marker.colors = []
for i, vel in enumerate(traj.vel):
if i % MOD == 0:
color = ColorRGBA()
val = vel / traj.max_vel
color.r = r_func(val)
color.g = g_func(val)
color.b = b_func(val)
color.a = 1.0
line_marker.colors.append(color)
# create a control which will move the box
# this control does not contain any markers,
# which will cause RViz to insert two arrows
control = InteractiveMarkerControl()
control.markers.append(line_marker)
int_marker.controls.append(control)
return int_marker
def set_light(self,parameter_name,blocking=False):
ah = action_handle("set", "light", parameter_name, blocking, self.parse)
if(self.parse):
return ah
else:
ah.set_active(mode="topic")
rospy.loginfo("Set light to <<%s>>",parameter_name)
# get joint values from parameter server
if type(parameter_name) is str:
if not rospy.has_param(self.ns_global_prefix + "/light/" + parameter_name):
rospy.logerr("parameter %s does not exist on ROS Parameter Server, aborting...",self.ns_global_prefix + "/light/" + parameter_name)
return 2
param = rospy.get_param(self.ns_global_prefix + "/light/" + parameter_name)
else:
param = parameter_name
# check color parameters
if not type(param) is list: # check outer list
rospy.logerr("no valid parameter for light: not a list, aborting...")
print "parameter is:",param
ah.error_code = 3
return ah
else:
if not len(param) == 3: # check dimension
rospy.logerr("no valid parameter for light: dimension should be 3 (r,g,b) and is %d, aborting...",len(param))
print "parameter is:",param
ah.error_code = 3
return ah
else:
for i in param:
#print i,"type1 = ", type(i)
if not ((type(i) is float) or (type(i) is int)): # check type
#print type(i)
rospy.logerr("no valid parameter for light: not a list of float or int, aborting...")
print "parameter is:",param
ah.error_code = 3
return ah
else:
rospy.logdebug("accepted parameter %f for light",i)
# convert to ColorRGBA message
color = ColorRGBA()
color.r = param[0]
color.g = param[1]
color.b = param[2]
color.a = 1 # Transparency
# publish color
self.pub_light.publish(color)
ah.set_succeeded()
ah.error_code = 0
return ah
def clear_area_makers(self): color = ColorRGBA() scale = Point() scale.x = 0.05 scale.y = 0.05 color.r = 0.0 color.g = 1.0 color.b = 0.0 color.a = 1.0 self.ClearPoints_marker = self.visual_test(self.clear_area, Marker.POINTS, color, scale) print 'finish build markers', len(self.ClearPoints_marker.points)
def visual_markers(self): color=ColorRGBA() scale=Point() scale.x=0.05 scale.y=0.05 color.r=0.0 color.g=1.0 color.b=0.0 color.a=0.5 self.line_marker=self.visual_test(self.cut_points, Marker.LINE_LIST, color, scale)#标记出区域划分（testing） color=ColorRGBA() scale=Point() scale.x=0.1 scale.y=0.1 color.r=0.0 color.g=0.0 color.b=1.0 color.a=1.0 self.centre_points_marker=self.visual_test(self.centre_points,Marker.POINTS, color, scale)
def LaserDataMarker(self,LaserData): ####################visual_test color=ColorRGBA() scale=Point() scale.x=0.04 scale.y=0.04 color.r=0.0 color.g=2.0 color.b=0.0 color.a=1.0 self.laser_points_marker=self.visual_test(LaserData, Marker.POINTS, color, scale)
def changeColor():
pub = rospy.Publisher('light_controller/command', ColorRGBA)
rospy.init_node('light_test')
rospy.sleep(1)
#color in rgb color space ranging from 0 to 1
red = ColorRGBA()
red.r = 1
red.g = 0
red.b = 0
red.a = 1
yellow = ColorRGBA()
yellow.r = 0.4
yellow.g = 1
yellow.b = 0
yellow.a = 1
green = ColorRGBA()
green.r = 0
green.g = 1
green.b = 0
green.a = 1
blue = ColorRGBA()
blue.r = 0
blue.g = 0
blue.b = 1
blue.a = 1
white = ColorRGBA()
white.r = 0.3
white.g = 1
white.b = 0.3
white.a = 1
for color in [red,yellow,green,white,blue,green]:
rospy.loginfo("Setting rgb to [%lf, %lf, %lf] with a [%lf]",color.r,color.g,color.b,color.a)
pub.publish(color)
time.sleep(3)
def flag_makers(self): color = ColorRGBA() scale = Point() pose = Pose() scale.x = 0.01 scale.y = 0.01 scale.z = 0.2 pose = self.pose pose.position.z += 0.5 color.r = 1.0 color.a = 1.0 self.flag_marker = self.visual_test(pose, Marker.TEXT_VIEW_FACING, color, scale)
def changeColor():
pub = rospy.Publisher('light_controller/command_mode', LightMode, queue_size=1)
rospy.init_node('light_test')
light_mode = LightMode()
#color in rgb color space ranging from 0 to 1
red = ColorRGBA()
red.r = 1
red.g = 0
red.b = 0
red.a = 1
yellow = ColorRGBA()
yellow.r = 0.4
yellow.g = 1
yellow.b = 0
yellow.a = 1
green = ColorRGBA()
green.r = 0
green.g = 1
green.b = 0
green.a = 1
blue = ColorRGBA()
blue.r = 0
blue.g = 0
blue.b = 1
blue.a = 1
white = ColorRGBA()
white.r = 0.3
white.g = 1
white.b = 0.3
white.a = 1
for color in [red,yellow,green,white,blue,green]:
rospy.loginfo("Setting rgb to %s [%d, %d, %d]",color.r,color.g,color.b,color.a)
light_mode.colors= 27*[color]
pub.publish(light_mode)
time.sleep(3)
def create_plan_marker(self, plan, plan_values):
# create an interactive marker for our server
int_marker = InteractiveMarker()
int_marker.header.frame_id = "/map"
int_marker.name = plan.ID
int_marker.description = plan.ID
pose = Pose()
#pose.position.x = traj.pose[0]['position']['x']
#pose.position.y = traj.pose[0]['position']['y']
int_marker.pose = pose
line_marker = Marker()
line_marker.type = Marker.LINE_STRIP
line_marker.scale.x = 0.1
# random.seed(float(plan.ID))
# val = random.random()
# line_marker.color.r = r_func(val)
# line_marker.color.g = g_func(val)
# line_marker.color.b = b_func(val)
# line_marker.color.a = 1.0
line_marker.points = []
for view in plan.views:
x = view.get_ptu_pose().position.x
y = view.get_ptu_pose().position.y
z = 0.0 # float(plan.ID) / 10
p = Point()
p.x = x - int_marker.pose.position.x
p.y = y - int_marker.pose.position.y
p.z = z - int_marker.pose.position.z
line_marker.points.append(p)
line_marker.colors = []
for i, view in enumerate(plan.views):
color = ColorRGBA()
val = float(i) / len(plan.views)
color.r = r_func(val)
color.g = g_func(val)
color.b = b_func(val)
color.a = 1.0
line_marker.colors.append(color)
# create a control which will move the box
# this control does not contain any markers,
# which will cause RViz to insert two arrows
control = InteractiveMarkerControl()
control.markers.append(line_marker)
int_marker.controls.append(control)
return int_marker
def __init__(self):
#ROSのパブリッシャなどの初期化
#rospy.init_node('ccaviz', anonymous=True)
self.mpub = rospy.Publisher('visualization_marker_array', MarkerArray, queue_size=10)
self.carray = []
clist = [[1, 0, 0, 1], [0, 1, 0, 1], [1, 1, 0, 1], [1, 0.5, 0, 1]]
for c in clist:
color = ColorRGBA()
color.r = c[0]
color.g = c[1]
color.b = c[2]
color.a = c[3]
self.carray.append(color)
def setLightCyanCircle_cb(req):
rospy.wait_for_service('/light_torso/set_light')
try:
set_light_torso = rospy.ServiceProxy("/light_torso/set_light",SetLightMode)
light_mode = LightMode()
cyan_color = ColorRGBA()
cyan_color.r = 0.0
cyan_color.g = 1.0
cyan_color.b = 0.5
cyan_color.a = 0.4
light_mode.colors.append(cyan_color)
light_mode.mode = 7
resp = set_light_torso(light_mode)
print resp
return
except rospy.ServiceException, e:
print "Service call failed: %s"%e
def __init__(self):
self.jsonInput()
super(CCA, self).__init__()
self.initUI()
rospy.init_node('ros_cca', anonymous=True)
self.mpub = rospy.Publisher('visualization_marker_array', MarkerArray, queue_size=10)
self.carray = []
clist = [[1,1,0,1],[0,1,0,1],[1,0,0,1]]
for c in clist:
color = ColorRGBA()
color.r = c[0]
color.g = c[1]
color.b = c[2]
color.a = c[3]
self.carray.append(color)
def __init__(self):
#ROSのパブリッシャなどの初期化
#rospy.init_node('ccaviz', anonymous=True)
fname = "/home/uema/catkin_ws/src/pose_cca/datas/20160520_a2.json"
poses1, poses2, dts, dtd = self.json_pose_input(fname)
self.poses1, self.poses2 = self.select_datas(poses1, poses2)
#print poses1.shape
self.mpub = rospy.Publisher('visualization_marker_array', MarkerArray, queue_size=10)
self.carray = []
clist = [[1, 0, 0, 1], [0, 1, 0, 1], [1, 1, 0, 1], [1, 0.5, 0, 1]]
for c in clist:
color = ColorRGBA()
color.r = c[0]
color.g = c[1]
color.b = c[2]
color.a = c[3]
self.carray.append(color)
def marker(self,data): self.point_marker=Marker() color=ColorRGBA() color.r=1.0 color.a=1 self.point_marker.header.frame_id='/map' self.point_marker.header.stamp=rospy.Time.now() self.point_marker.ns='path_test' self.point_marker.action=Marker.ADD self.point_marker.id=0 self.point_marker.type=Marker.POINTS self.point_marker.scale.x=0.05 self.point_marker.scale.y=0.05 self.point_marker.points=data for i in data: self.point_marker.colors.append(color) self.point_marker.lifetime=rospy.Duration(0.2)
def createPointsInMarker(self, data): # track_no_idx = 0 # for i in range(0,768,12): for track_no_idx in self.track_no_idxs: # if not self.rangerate[track_no_idx] == 81.91: point = Point() dist = self.dist [track_no_idx] #data.data[i+9] * .1 angle = self.angle[track_no_idx] #data.data[i+11]* .1 angle_rad = math.radians(angle) point.x = - dist * math.sin(angle_rad) point.y = - dist * math.cos(angle_rad) point.z = 0 color = ColorRGBA() if self.movingflags[track_no_idx]: color.r = 1.0 color.g = 0.0 color.b = 0.0 else: color.r = 0.0 color.g = 1.0 color.b = 0.0 color.a = 1.0 self.marker.points.append( point) self.marker.colors.append( color)
if remove_im() == True:
object_added = False
if object_added == False:
add_object = rospy.ServiceProxy('/interaction_primitives/add_object', AddObject)
rospy.loginfo('Calling %s service','/interaction_primitives/add_object')
# color of the bounding box
color = ColorRGBA()
color.r = 1
color.g = 1
color.b = 0
color.a = 1
try:
add_object(frame_id = '/map',
name = 'unknown_object',
description = 'Unknown object to grasp',
pose = est_result.pose,
bounding_box_lwh = est_result.bounding_box_lwh,
color = color,
use_material = False)
object_added = True
rospy.loginfo('IM added')
#!/usr/bin/env python
import rospy
import yaml
import rospkg
import pprint
from visualization_msgs.msg import Marker, MarkerArray
from geometry_msgs.msg import Point
from std_msgs.msg import ColorRGBA
neutral = ColorRGBA()
neutral.a = 0.0
private = ColorRGBA()
private.a = 1.0
private.r = 1.0
private.g = 0.0
private.b = 0.0
public = ColorRGBA()
public.a = 1.0
public.g = 1.0
color_map = {
0: neutral,
1: private,
2: public
}
if __name__ == '__main__':
rospy.init_node('draw_lines')
def load_data(self, path=""):
if path == "":
path = roslib.packages.get_pkg_dir('ollieRosTools')+"/matlab/Data/exported/"
# Load Trajectory
with open(path+'trajectory/TCAM.csv', 'rb') as f:
reader = csv.reader(f)
for row in reader:
self.TCAM.append(row)
self.TCAM = np.array(self.TCAM)
self.TCAM =xyzWXYZ_xyzXYZW(self.TCAM.astype(np.double))
# Load IMU Trajectory
with open(path+'/trajectory/TOBJ.csv', 'rb') as f:
reader = csv.reader(f)
for row in reader:
self.TOBJ.append(row)
self.TOBJ = np.array(self.TOBJ)
self.TOBJ = xyzWXYZ_xyzXYZW(self.TOBJ.astype(np.double))
# Load cloud
with open(path+'points3d/cloud.csv', 'rb') as f:
reader = csv.reader(f)
for row in reader:
self.CLOUD.append(row)
self.CLOUD = np.array(self.CLOUD)
self.CLOUD = self.CLOUD.astype(np.double)
mincol = np.min(self.CLOUD,0)
maxcol = np.max(self.CLOUD,0)
col = self.CLOUD-mincol
col = col/np.abs(mincol - maxcol )
m = MarkerMSG()
m.action = m.ADD
m.frame_locked = True
m.ns = "WorldPointsGT"
m.header.frame_id = "/world"
m.type = m.POINTS
m.pose.orientation.w = 1.0
m.pose.orientation.z = 0
m.pose.orientation.y = 0
m.pose.orientation.x = 0
m.id = 0
m.pose.position.x = 0
m.pose.position.y = 0
m.pose.position.z = 0
#m.color.a = 0.6
#m.color.r = 1.0
#m.color.b = 0.0
#m.color.g = 0.0
m.scale.x = 0.05
m.scale.y = 0.05
m.scale.z = 0.05
for wp,co in zip(self.CLOUD, col):
p = PointMSG()
p.x = wp[1]
p.y = wp[2]
p.z = wp[3]
c = ColorMSG()
c.a = 0.75
c.r = co[1]
c.g = co[2]
c.b = co[3]
m.points.append(p)
m.colors.append(c)
self.CLOUD = m
# Load Camera
with open(path+'meta/cam2img.csv', 'rb') as f:
reader = csv.reader(f)
for row in reader:
self.CAM2IMG.append(row)
self.CAM2IMG = np.array(self.CAM2IMG)
self.CAM2IMG = self.CAM2IMG.astype(np.double)
self.CAM2IMG = np.array(self.CAM2IMG)
self.camInfo = CameraInfoMSG()
self.camInfo.width = self.CAM2IMG[0,2]*2
self.camInfo.height = self.CAM2IMG[1,2]*2
self.camInfo.distortion_model = "plumb_bob"
self.camInfo.P = self.CAM2IMG.flatten()
self.camInfo.K = self.CAM2IMG[0:3,0:3].flatten()
self.camInfo.R = np.eye(3).flatten()
self.camInfo.D = [0.,0.,0.,0.,0.]
self.camInfo.header.frame_id = "/synCamGT"
# Load IMU Cam transform
with open(path+'meta/imu2cam.csv', 'rb') as f:
reader = csv.reader(f)
for row in reader:
self.OBJ2CAM.append(row)
self.OBJ2CAM = np.array(self.OBJ2CAM)
self.OBJ2CAM = xyzWXYZ_xyzXYZW(self.OBJ2CAM.astype(np.double))[0]
self.OBJ2CAMmsg = toTransformMsg(self.OBJ2CAM)
#Load KPS
files = sorted(glob.glob(path+'features/f2d*.csv'))
for file in files:
f2dmsg = []
with open(file, 'rb') as fi:
reader = csv.reader(fi)
f2d = []
for row in reader:
f2d.append(row)
f2d = np.array(f2d)
f2d = f2d.astype(np.double)
for f in f2d:
p= PointMSG()
p.x = f[1]
p.y = f[2]
p.z = f[0] #id
f2dmsg.append(p)
self.KPS.append(f2dmsg)
#Load images
files = sorted(glob.glob(path+'images/img*.png'))
for file in files:
self.IMAGES.append(cv2.imread(file))
def imageCallback(self,data): #Convert image to CV2 supported numpy array bridge=CvBridge() cv_image = bridge.imgmsg_to_cv(data, "mono8") searched = np.array(cv_image, dtype=np.uint8) #Create copy and clear searched space in copy (leaving map only) searchedCopy=copy.copy(searched) searchedCopy[searchedCopy==255]=0 #Take Sobel Derivatives of searched and map only sobel_x=np.uint8(np.absolute(cv2.Sobel(searched,cv2.CV_16S,1,0,ksize=1))) sobel_y=np.uint8(np.absolute(cv2.Sobel(searched,cv2.CV_16S,0,1,ksize=1))) sobel_xy=cv2.addWeighted(sobel_x,0.5,sobel_y,0.5,0) sobel_x_base=np.uint8(np.absolute(cv2.Sobel(searchedCopy,cv2.CV_16S,1,0,ksize=1))) sobel_y_base=np.uint8(np.absolute(cv2.Sobel(searchedCopy,cv2.CV_16S,0,1,ksize=1))) sobel_xy_base=cv2.addWeighted(sobel_x_base,0.5,sobel_y_base,0.5,0) ret,sobel_xy_thres = cv2.threshold(sobel_xy,0,255,cv2.THRESH_BINARY) ret,sobel_xy_base_thres = cv2.threshold(sobel_xy_base,0,255,cv2.THRESH_BINARY) #Subtract Sobel Derivatives (Leaves Frontiers Only) sobelCombined=sobel_xy_base_thres-sobel_xy_thres ret,sobelCombined_thresh = cv2.threshold(sobelCombined,0,255,cv2.THRESH_BINARY) #Dialate Frontiers To Form Continuous Contours dialate=cv2.dilate(sobelCombined_thresh,np.ones((3,3),'uint8')) #Find Contours (make copy since dialate destorys original image) dialateCopy=copy.copy(dialate) contours,hier=cv2.findContours(dialateCopy,cv2.RETR_LIST,cv2.CHAIN_APPROX_NONE) #Convert the image back to mat format for publishing as ROS image frontiers = cv.fromarray(dialate) #Create List Data for Marker Message centroids = [] colors = [] #Filter Frontier Contour by number of pixels for i in contours: if len(i) > 50: moments=cv2.moments(i) cx = int(moments['m10']/moments['m00']) cy = int(moments['m01']/moments['m00']) centroidPoint = Point() centroidColor = ColorRGBA() centroidPoint.x = cx*self.mapRes+self.mapOrigin.position.x centroidPoint.y = cy*self.mapRes+self.mapOrigin.position.y centroidPoint.z = 0; centroidColor.r = 0 centroidColor.g = 0 centroidColor.b = 1 centroidColor.a = 1 centroids.append(centroidPoint) colors.append(centroidColor) #Pack Marker Message markerMsg = Marker() markerMsg.header.frame_id = "/map" markerMsg.header.stamp = rospy.Time.now() markerMsg.ns = "" markerMsg.id = 0 markerMsg.type = 7 #Sphere List Type markerMsg.action = 0 #Add Mode markerMsg.scale.x = 0.5 markerMsg.scale.y = 0.5 markerMsg.scale.z = 0.5 markerMsg.points = centroids markerMsg.colors = colors #Publish Marker and Image messages self.imagePub.publish(bridge.cv_to_imgmsg(frontiers, "mono8")) self.markerPub.publish(markerMsg)
def __init__(self,node_name, maintain_last_plot):
rospy.init_node(node_name, anonymous=False)
self.maintain_last_plot = maintain_last_plot
# self.createMarkerArray()
self.marker = Marker()
# for i in range (10):
self.marker.header.frame_id = "/map"
self.marker.header.stamp = rospy.Time.now()
self.marker.ns = 'radar_obj'
self.marker.id = 0 # i
self.marker_text = Marker()
self.marker_text.header.frame_id = "/map"
self.marker_text.header.stamp = rospy.Time.now()
self.marker_text.ns = 'radar_text'
self.marker_text.id = 0 # i
self.marker.type = Marker.CUBE_LIST
# self.marker.type = Marker.CUBE
self.marker_text.type =Marker.TEXT_VIEW_FACING
self.marker.action = Marker.ADD
self.marker_text.action = Marker.ADD
self.marker_text.pose.position.x = 0
self.marker_text.pose.position.y = -5
self.marker_text.pose.position.z = 0
self.marker.pose.orientation.x = 0.0
self.marker.pose.orientation.y = 0.0
self.marker.pose.orientation.z = 0.0
self.marker.pose.orientation.w = 1.0
self.marker_text.pose.orientation.x = 0.0
self.marker_text.pose.orientation.y = 0.0
self.marker_text.pose.orientation.z = 0.0
self.marker_text.pose.orientation.w = 1.0
self.marker.scale.x = 0.10
self.marker.scale.y = 0.10
self.marker.scale.z = 0.10
self.marker_text.scale.z = 1.10
self.marker_text.scale.y = 1.10
self.marker_text.scale.x = 1.10
self.marker.color.r = 0.0
self.marker.color.g = 1.0
self.marker.color.b = 0.0
self.marker.color.a = 1.0
self.marker_text.color.r = 0.0
self.marker_text.color.g = 1.0
self.marker_text.color.b = 0.0
self.marker_text.color.a = 1.0
self.marker_text.text = 'THIS IS AweSome But NODATA yet'
for x in range (-100,100):
point = Point()
# point.x = 0
point.x = x/10.
point.y = 0
point.z = 0
color = ColorRGBA()
color.r = 0.0
color.g = 1.0
color.b = 0.0
color.a = 1.0
self.marker.points.append( point)
self.marker.colors.append( color)
self.pub_marker = rospy.Publisher ('radar_packet/viz/marker' , Marker , queue_size=100)
self.pub_marker_text = rospy.Publisher ('radar_packet/viz/marker_text' , Marker , queue_size=100)
self.pub_marker_array = rospy.Publisher ('radar_packet/viz/marker_array' , MarkerArray , queue_size=1000)
solved_name_res = 'can0'
self.sub_data = rospy.Subscriber ('radar_packet/'+solved_name_res+'/processed', GenericObjCanData, self.onIncomingData)
# self.pub_marker.publish(self.marker)
# self.pub_marker_text.publish(self.marker_text)
self.data_counter = 0
self.filtered_first_track = False
rate = rospy.Rate(1)
while not rospy.is_shutdown():
# pub_marker.publish(marker)
# pub_marker.publish(marker_text)
# # marker.pose.position.x = marker.pose.position.x + 1
# self.createTextMarkerDel()
# self.pub_marker_text.publish(self.marker_text)
self.createTextMarker()
if not self.data_counter == 0:
self.marker_text.text = 'GOT DATA'
else:
self.marker_text.text = 'NODATA'
# if self.maintain_last_plot:
# self.createCubeMarkerDel()
# self.pub_marker.publish(self.marker)
# self.createCubeMarker()
# if not self.maintain_last_plot:
# self.pub_marker.publish(self.marker)
# self.createMarkerArray()
# self.pub_marker_array.publish(self.marker_array)
self.data_counter = 0
self.pub_marker_text.publish(self.marker_text)
rate.sleep()
