def collision_callback(data):
global num_collisions
num_collisions = num_collisions + 1
has_collided = OverlayText(text=str('True'))
collision_pub.publish(has_collided)
text_collisions = OverlayText(text=str(num_collisions) + ' collision(s)')
num_collision_pub.publish(text_collisions)
def callback(msg):
for exclude_regex in exclude_regexes:
if re.match(exclude_regex, msg.msg):
return
global lines
if msg.name not in ignore_nodes:
if msg.name in nodes or len(nodes) == 0:
if len(nodes_regexp) == 0 or nodes_regexp_compiled.match(msg.name):
if reverse_lines:
lines = [colored_message(msg)] + lines
if len(lines) > line_buffer_length:
lines = lines[0:line_buffer_length]
else:
lines = lines + [colored_message(msg)]
if len(lines) > line_buffer_length:
lines = lines[-line_buffer_length:]
text = OverlayText()
text.left = 20
text.top = 20
text.width = 1200
text.height = 1200
text.fg_color.a = 1.0
text.fg_color.r = 0.3
text.text_size = 12
text.text = "\n".join(lines)
pub.publish(text)
def callback(msg):
text = OverlayText()
if msg.contact_state == GroundContactState.CONTACT_BOTH_GROUND:
text.text = "Double Stance Phase"
text.fg_color.a = 1.0
text.fg_color.r = 25 / 255.0
text.fg_color.g = 1
text.fg_color.b = 1
elif msg.contact_state == GroundContactState.CONTACT_LLEG_GROUND or msg.contact_state == GroundContactState.CONTACT_RLEG_GROUND:
text.text = "Single Stance Phase"
text.fg_color.a = 1.0
text.fg_color.r = 1
text.fg_color.g = 0
text.fg_color.b = 0
elif msg.contact_state == GroundContactState.CONTACT_UNSTABLE:
text.text = "Unstable"
text.fg_color.a = 1.0
text.fg_color.r = 1
text.fg_color.g = 0
text.fg_color.b = 0
elif msg.contact_state == GroundContactState.CONTACT_AIR:
text.text = "Air"
text.fg_color.a = 1.0
text.fg_color.r = 1
text.fg_color.g = 0
text.fg_color.b = 0
pub.publish(text)
def publishUsage(self):
overlay_text = OverlayText()
overlay_text.text = """
Left Analog: x/y
L2/R2 : +-z
L1/R1 : +-yaw
Left/Right : +-roll
Up/Down : +-pitch
circle : Go
cross : Reset/Cancel
triangle : Clear maps and look around ground
up/down : Move menu cursors
"""
overlay_text.width = 500
overlay_text.height = 500
overlay_text.text_size = 12
overlay_text.left = 10
overlay_text.top = 10
overlay_text.font = "Ubuntu Mono Regular"
overlay_text.bg_color.a = 0
overlay_text.fg_color.r = 25 / 255.0
overlay_text.fg_color.g = 1
overlay_text.fg_color.b = 1
overlay_text.fg_color.a = 1
self.usage_pub.publish(overlay_text)
def main(self):
# read QR code
self.pb_rviz_msg.publish(
OverlayText(
text='Started reading QR code and get position of each bins.'))
succeeded = self.cl_qrcode_reader()
# Get item
self.target_bin = 'bin_E'
self.pb_rviz_msg.publish(
OverlayText(
text='Getting item in bin name: {0}.'.format(self.target_bin)))
succeeded = self.cl_get_item()
# Release item
self.pb_rviz_msg.publish(OverlayText(text='Releasing item.'))
self.cl_release_item()
self.pb_rviz_msg.publish(OverlayText(text="baxter waiting"))
def motorStatesCallback(msg):
global g_publishers, g_text_publisher
#values = msg.position
values = msg.temperature
names = msg.name
allocatePublishers(len(values))
max_temparature = 0
max_temparature_name = ""
for name, val, pub in zip(names, values, g_publishers):
pub.publish(Float32(val))
if max_temparature < val:
max_temparature = val
max_temparature_name = name
if max_temparature:
if max_temparature > 70:
color = fatal_color
elif max_temparature > 50:
color = warning_color
else:
color = safe_color
text = OverlayText()
text.fg_color.r = color[0] / 255.0
text.fg_color.g = color[1] / 255.0
text.fg_color.b = color[2] / 255.0
text.fg_color.a = 1.0
text.bg_color.a = 0.0
text.text = "%dC -- (%s)" % (int(max_temparature),
max_temparature_name)
g_text_publisher.publish(text)
def changeTaskState(e):
send_text = OverlayText()
send_text.text = 'TaskState: ' + e.dst
send_text.top = 10
send_text.left = 10
send_text.width = 750
send_text.height = 50
send_text.bg_color.r = 0.9
send_text.bg_color.b = 0.9
send_text.bg_color.g = 0.9
send_text.bg_color.a = 0.1
if e.dst in tsm.fatal:
send_text.fg_color.r = 0.8
send_text.fg_color.g = 0.3
send_text.fg_color.b = 0.3
elif e.dst in tsm.warn:
send_text.fg_color.r = 0.8
send_text.fg_color.g = 0.5
send_text.fg_color.b = 0.1
else:
send_text.fg_color.r = 0.3
send_text.fg_color.g = 0.8
send_text.fg_color.b = 0.3
send_text.fg_color.a = 1
send_text.line_width = 1
send_text.text_size = 30
task_state_text_pub.publish(send_text)
def lowspeedDataUpdatedTimeCallnack(msg):
text = OverlayText()
now = rospy.Time.now()
diff = (now - msg.data).to_sec()
text.bg_color = TRANSPARENT_COLOR
text.text = "LOWSPEED DATA: Updated %0.1f secs before" % (diff)
text.fg_color = OK_COLOR
pub_lowspeed_data_updated_time.publish(text)
def onReconfigure(self, worlds_names):
"""
"""
#rospy.loginfo("reconfigure")
text = OverlayText()
text.action = 1
self.text_pub.publish(text)
pass
def __init__(self):
self.sub_vehicle_status = rospy.Subscriber("/vehicle_status",
VehicleStatus,
self.cb_vehicle_status)
self.msg_vehicle_status = VehicleStatus()
# Obstacle status
self.sub_detected_obstacle = rospy.Subscriber(
"/detected_obstacles", DetectedObstacles,
self.cb_detected_obstacles)
self.msg_detected_obstacles = None
self.pub_obstacle_text = rospy.Publisher("rei_vehicle_status/obstacle",
OverlayText,
queue_size=1)
# Obstacle report
self.msg_obstacle_text = OverlayText()
self.msg_obstacle_text.text_size = 24
self.msg_obstacle_text.width = 800
self.msg_obstacle_text.height = 120
self.msg_obstacle_text.bg_color.a = 0.5
self.msg_obstacle_text.fg_color.r = 1.0
self.msg_obstacle_text.fg_color.g = 0.0
self.msg_obstacle_text.fg_color.b = 0.0
self.msg_obstacle_text.fg_color.a = 1.0
self.msg_obstacle_text.font = "Noto Mono"
self.msg_obstacle_text.top = 500
# State report
self.msg_vehicle_state_text = OverlayText()
self.msg_vehicle_state_text.text_size = 14
self.msg_vehicle_state_text.width = 800
self.msg_vehicle_state_text.height = 100
self.msg_vehicle_state_text.bg_color.a = 0.5
self.msg_vehicle_state_text.font = "Noto Mono"
self.pub_vehicle_state_text = rospy.Publisher(
"rei_vehicle_status/state_text", OverlayText, queue_size=1)
self.msg_vehicle_state_text.fg_color.r = 1.0
self.msg_vehicle_state_text.fg_color.g = 0.0
self.msg_vehicle_state_text.fg_color.b = 0.0
self.msg_vehicle_state_text.fg_color.a = 1.0
self.msg_vehicle_state_text.bg_color.a = 0.5
# State report
self.msg_state_text = OverlayText()
self.msg_state_text.text_size = 14
self.msg_state_text.width = 800
self.msg_state_text.height = 100
self.msg_state_text.bg_color.a = 0.5
self.msg_state_text.font = "Noto Mono"
def publishOverlaytext(self, world_name):
"""
"""
situations_text = " <" + world_name + "> " + self.__overlay_name + "\n\r"
situations_text += "- facts\n\r"
situations_text += "id : description\n\r"
situations_text += "----------------\n\r"
fact_sorted = {}
fact_desc = {}
for situation in self.ctx.worlds()[world_name].timeline().situations():
if situation.end.data != situation.start.data:
if situation.end.data == rospy.Time(0):
if situation.description not in fact_desc:
fact_sorted[situation.start.data] = situation
fact_desc[situation.description] = situation
else:
if rospy.Time.now() - situation.end.data < rospy.Duration(
5.0):
if situation.description not in fact_desc:
fact_sorted[situation.start.data] = situation
fact_desc[situation.description] = situation
for key in sorted(fact_sorted.iterkeys()):
situations_text += fact_sorted[key].id[:5] + " : " + fact_sorted[
key].description + "\n\r"
situations_text += "\n\r- events\n\r"
situations_text += "id : description\n\r"
situations_text += "----------------\n\r"
event_sorted = {}
for situation in self.ctx.worlds()[world_name].timeline().situations():
if situation.start.data == situation.end.data:
if rospy.Time.now() - situation.end.data < rospy.Duration(5.0):
event_sorted[situation.start.data] = situation
for key in sorted(event_sorted.iterkeys()):
situations_text += event_sorted[key].id[:5] + " : " + event_sorted[
key].description + "\n\r"
text = OverlayText()
text.width = 400
text.height = 600
text.left = 10
text.top = 10
text.text_size = 12
text.line_width = 2
text.font = "DejaVu Sans Mono"
text.text = situations_text
text.fg_color = ColorRGBA(25 / 255.0, 1.0, 240.0 / 255.0, 1.0)
text.bg_color = ColorRGBA(0.0, 0.0, 0.0, 0.2)
if world_name not in self.__text_pub:
self.__text_pub[world_name] = rospy.Publisher(world_name +
"/timeline",
OverlayText,
queue_size=2)
self.__text_pub[world_name].publish(text)
def continuousDataUpdatedTimeCallnack(msg):
text = OverlayText()
now = rospy.Time.now()
diff = (now - msg.data).to_sec()
text.bg_color = TRANSPARENT_COLOR
text.text = "CONTINUOUS DATA: Updated %0.1f secs before" % (diff)
if diff <= 1.0:
text.fg_color = OK_COLOR
else:
text.fg_color = WARN_COLOR
pub_continuous_data_updated_time.publish(text)
def burstDataUpdatedTimeCallnack(msg):
text = OverlayText()
now = rospy.Time.now()
diff = (now - msg.data).to_sec()
text.bg_color = TRANSPARENT_COLOR
text.text = "BURST DATA: Updated %0.1f secs before" % (diff)
if diff <= 30.0:
text.fg_color = OK_COLOR
else:
text.fg_color = WARN_COLOR
pub_burst_data_updated_time.publish(text)
def publishOverlaytext(self, world_name):
"""
"""
situations_text = " FACTS\n\r"
situations_text += "id : description\n\r"
situations_text += "----------------\n\r"
fact_sorted = {}
fact_desc = {}
for situation_id, situation in self.worlds[
world_name].timeline.situations.items():
if situation.end.data != situation.start.data:
if situation.end.data == rospy.Time(0):
if situation.description not in fact_desc:
fact_sorted[situation.start.data] = situation
fact_desc[situation.description] = situation
else:
if rospy.Time.now() - situation.end.data < rospy.Duration(
5.0):
if situation.description not in fact_desc:
fact_sorted[situation.start.data] = situation
fact_desc[situation.description] = situation
for key in sorted(fact_sorted.iterkeys()):
situations_text += fact_sorted[key].id[:5] + " : " + fact_sorted[
key].description + "\n\r"
situations_text += "\n\rEVENTS\n\r"
situations_text += "id : description\n\r"
situations_text += "----------------\n\r"
event_sorted = {}
for situation_id, situation in self.worlds[
world_name].timeline.situations.items():
if situation.start.data == situation.end.data:
if rospy.Time.now() - situation.end.data < rospy.Duration(5.0):
event_sorted[situation.start.data] = situation
for key in sorted(event_sorted.iterkeys()):
situations_text += event_sorted[key].id[:5] + " : " + event_sorted[
key].description + "\n\r"
text = OverlayText()
text.width = 400
text.height = 600
text.left = 10
text.top = 10
text.text_size = 12
text.line_width = 2
text.font = "DejaVu Sans Mono"
text.text = situations_text
text.fg_color = ColorRGBA(25 / 255.0, 1.0, 240.0 / 255.0, 1.0)
text.bg_color = ColorRGBA(0.0, 0.0, 0.0, 0.2)
self.text_pub.publish(text)
def lowspeedDataUpdatedTimeCallnack(msg):
text = OverlayText()
now = rospy.Time.now()
diff = (now - msg.data).to_sec()
text.bg_color = TRANSPARENT_COLOR
if msg.data.to_sec() == 0:
text.text = "LOWSPEED DATA: Not received yet"
diff = 100
else:
text.text = "LOWSPEED DATA: Updated %0.1f secs before" % (diff)
text.fg_color = OK_COLOR
text.bg_color = UPDATED_TIME_BG_COLOR
pub_lowspeed_data_updated_time.publish(text)
def robotStatusBasicInfoCallback(msg):
text = OverlayText()
text.bg_color = TRANSPARENT_COLOR
if msg.data == FC2OCSLarge.ROBOT_IDLE:
text.text = "BURST: Robot was idle"
text.fg_color = OK_COLOR
elif msg.data == FC2OCSLarge.ROBOT_MOVING:
text.text = "BURST: Robot was moving"
text.fg_color = WARN_COLOR
else:
text.text = "BURST: Robot was in unnknown status"
text.fg_color = UNKNOWN_COLOR
pub_burst_robot_status.publish(text)
def onReconfigure(self, worlds_names):
"""
"""
text = OverlayText()
text.action = 1
for world_name in worlds_names:
if world_name not in self.__text_pub:
self.__text_pub[world_name] = rospy.Publisher(world_name +
"/timeline",
OverlayText,
queue_size=2)
self.__text_pub[world_name].publish(text)
pass
def timer_callback(self, event):
p = Popen(['rosnode', 'list'], stdout=PIPE)
p.wait()
nodelist_tmp = p.communicate()
# nodelist_tmp: ('/pepper_1556630474468299832\n/rosout\n', None)
nodelist = nodelist_tmp[0]
# nodelist: '/pepper_1556630474468299832\n/rosout\n'
nodelist = nodelist.split("\n")
# nodelist: ['/pepper_1556630474468299832', '/rosout', '']
# If '/pepper_robot' node is killed, need to kill jsk_pepper_startup.launch by killing another required node (see https://github.com/jsk-ros-pkg/jsk_robot/issues/1077)
if self.naoqi_driver_node_name in nodelist:
pass
else:
call(['rosnode', 'kill', self.pose_controller_node_name])
tmp = []
add_camera_node = False
for i in nodelist:
# play_audio_stream_node is highlighted because this node needs to take care of people's privacy
if i == '/play_audio_stream_node':
i = """<span style="color: red;">""" + i + """</span>"""
tmp.append(i)
# display camera node as "/pepper_robot/camera" because there are a lot of camera nodes
else:
if 'camera' in i:
if not add_camera_node:
i = "/pepper_robot/camera"
tmp.append(i)
add_camera_node = True
# ignore ''
elif i is not '':
tmp.append(i)
text = OverlayText()
text.left = 350
text.top = 0
text.width = 400
text.height = 300
text.fg_color.r = 25 / 255.0
text.fg_color.g = 255 / 255.0
text.fg_color.b = 255 / 255.0
text.fg_color.a = 0.8
text.bg_color.r = 0
text.bg_color.g = 0
text.bg_color.b = 0
text.bg_color.a = 0.8
text.text_size = 10
text.text = "\n".join(tmp)
self.pub.publish(text)
def publishModeText(message='test',
bg_color=[0.9, 0.9, 0.9, 0.1],
fg_color=[0.2, 0.2, 0.2, 0.1]):
send_text = OverlayText()
send_text.text = message
send_text.top = 50
send_text.left = 25
send_text.width = 1500
send_text.height = 200
send_text.bg_color.r, send_text.bg_color.g, send_text.bg_color.b, send_text.bg_color.a = bg_color
send_text.fg_color.r, send_text.fg_color.g, send_text.fg_color.b, send_text.fg_color.a = fg_color
#send_text.line_width = 1
send_text.text_size = 100
send_text_pub.publish(send_text)
def publish(self, text):
msg = OverlayText()
msg.text = text
msg.width = self.config.width
msg.height = self.config.height
msg.top = self.config.top
msg.left = self.config.left
msg.fg_color.a = self.config.fg_alpha
msg.fg_color.r = self.config.fg_red
msg.fg_color.g = self.config.fg_green
msg.fg_color.b = self.config.fg_blue
msg.bg_color.a = self.config.bg_alpha
msg.bg_color.r = self.config.bg_red
msg.bg_color.g = self.config.bg_green
msg.bg_color.b = self.config.bg_blue
msg.text_size = self.config.text_size
self.pub.publish(msg)
def callback(event):
target_label_value = rospy.get_param('~target_label_value', None)
lines = []
for label_value, label_name in enumerate(label_names):
color = 'green' if label_value == target_label_value else 'white'
lines.append('<span style="color: {};">{}: {}</span>'.format(
color, label_value, label_name))
text = OverlayText()
text.left = 20
text.top = 20
text.width = 1200
text.height = 1200
text.fg_color.a = 1.0
text.fg_color.r = 0.3
text.text_size = 12
text.text = '\n'.join(lines)
pub.publish(text)
def callback(msg):
global pub
count = msg.data
text = OverlayText()
color = (52, 152, 219)
text.fg_color.r = color[0] / 255.0
text.fg_color.g = color[1] / 255.0
text.fg_color.b = color[2] / 255.0
text.fg_color.a = 1.0
text.bg_color.a = 0.0
text.text = "Samples: %d" % (count)
text.width = 500
text.height = 100
text.left = 10
text.top = 10
text.text_size = 30
pub.publish(text)
def update_msg(self, topic, data):
self._lock.acquire()
if isinstance(data, str):
t = OverlayText
msg = OverlayText()
msg.action = OverlayText.ADD
msg.text = str(data)
elif isinstance(data, Real):
t = Float32
msg = Float32()
msg.data = float(data)
else:
raise ValueError("invalid msg type")
if not topic in self._pubs:
self._pubs[topic] = PublisherWrapper(topic, t, self._publish_rate,
self._queue_size)
self._pubs[topic].update_msg(msg)
self._lock.release()
def publishModeText(message):
send_text = OverlayText()
send_text.text = message
send_text.top = 130
send_text.left = 10
send_text.width = 750
send_text.height = 50
send_text.bg_color.r = 0.9
send_text.bg_color.b = 0.9
send_text.bg_color.g = 0.9
send_text.bg_color.a = 0.1
send_text.fg_color.r = 0.8
send_text.fg_color.g = 0.8
send_text.fg_color.b = 0.8
send_text.fg_color.a = 1
send_text.line_width = 1
send_text.text_size = 30
send_text_pub.publish(send_text)
def update_overlaytext(self, number=1.23, number2=20):
text = OverlayText()
text.width = 200
text.height = 400
text.left = 10
text.top = 10
text.text_size = 12
text.line_width = 2
text.font = "DejaVu Sans Mono"
text.text = """Distance= %s.
Angle=%s.
Counter = <span style="color: green;">%d.</span>
""" % (str(number), str(number2), self.counter)
text.fg_color = ColorRGBA(25 / 255.0, 1.0, 240.0 / 255.0, 1.0)
text.bg_color = ColorRGBA(0.0, 0.0, 0.0, 0.2)
return text
def __init__(self):
self.prompt_text = "HOTARU_TEB_PLANNER_NODE: "
self.pub_ov_text = rospy.Publisher("/rei_planner_text_viz",
OverlayText,
queue_size=1)
self.msg_text = OverlayText()
self.msg_text.font = "Monospace"
self.msg_text.text_size = 24
self.msg_text.width = 300
self.msg_text.bg_color.a = 0.4
self.msg_text.bg_color.r = 0.0
self.msg_text.bg_color.g = 0.0
self.msg_text.bg_color.b = 0.0
#
self.msg_text.fg_color.a = 1.0
self.sub_visualizer = rospy.Subscriber(
"/hotaru_planner/behav/sync_state_machine/current_state",
ReiStateMachineTransitionSignal, self.cbTransitionSignal)
self.pub_timer = rospy.Timer(rospy.Duration(0.1),
self.cbTimerPublishText)
def update_overlaytext(self, number=1.23, number2=1.23, number3=20):
text = OverlayText()
text.width = 200
text.height = 400
text.left = 10
text.top = 10
text.text_size = 12
text.line_width = 2
text.font = "DejaVu Sans Mono"
text.text = """x= %s
y= %s
Height(z)= %s
""" % (str(number), str(number2), str(number3))
text.fg_color = ColorRGBA(0 / 255.0, 100.0 / 255, 0.0 / 255.0, 1.0)
text.bg_color = ColorRGBA(0.0, 0.0, 0.0, 0.2)
return text
def joyCB(self, status, history):
msg = OverlayText()
if status.left:
self.left = self.left - 10
if status.right:
self.left = self.left + 10
if status.up:
self.top = self.top - 10
if status.down:
self.top = self.top + 10
msg.text_size = self.text_size
msg.width = self.width
msg.height = self.height
msg.left = self.left
msg.top = self.top
msg.line_width = self.line_width
msg.font = self.font
msg.fg_color = ColorRGBA(25 / 255.0, 1.0, 240.0 / 255.0, 1.0)
msg.bg_color = ColorRGBA(0.0, 0.0, 0.0, 0.2)
msg.text = self.usage
self.pub.publish(msg)
def odometry_callback(odometry):
# speed info
twist = odometry.twist.twist
speed = magnitude(twist.linear)
msg = Float32()
msg.data = speed
linear_speed_pub.publish(msg)
# position info
pos = odometry.pose.pose.position
position_text = OverlayText()
position_text.action = OverlayText.ADD
position_text.width = 128
position_text.height = 64
position_text.left = 0
position_text.top = 0
position_text.bg_color = ColorRGBA(0, 0, 0, 0.5)
position_text.fg_color = ColorRGBA(1, 1, 1, 1)
position_text.text_size = 8
position_text.text = "Current position: {:.2f} {:.2f} {:.2f}".format(pos.x, pos.y, pos.z)
position_pub.publish(position_text)
def __init__(self, mySide='r'):
self.mySide = mySide
self.map = tmp_targetsMap.getTargetsMap()
self.goalPub = rospy.Publisher('/move_base_simple/goal', PoseStamped, queue_size=5)
self.myStatePub = rospy.Publisher('my_state', OverlayText, queue_size=5)
self.warStateSub = rospy.Subscriber('/war_state', war_state, self.warStateCallback)
self.navStateSub = rospy.Subscriber('/move_base/status', GoalStatusArray, self.navStateCallback)
self.cvRectSub = rospy.Subscriber('/cv_rect', CvRect, self.cvRectSubCallback)
self.twist_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1000)
self.laserSub = rospy.Subscriber('/scan', LaserScan, self.laserCallback)
self.war_state = war_state()
self.war_state.target_names = []
self.war_state.target_owner = []
self.war_state.target_point = []
self.range_data = 0.0
self.my_state_text = OverlayText()
self.my_state_text.text = ""
self.r = 10.0 # rospy frequency
self.isFoundEnemy = False
self.isFoundEnemyTarget = False
self.rectData = CvRect()
self.navStatus = ""
# Parameters of PID controller
self.Kp = 0.005
self.Ki = 0.0
self.Kd = 0.00005
self.integral = 0.0
self.error = 0.0
self.error_pre = 0.0
self.dt = 1.0/self.r
self.target_001 = 320.0
# Parameters of DIST
self.threshold_r = 50.0 # threshold of red area width
self.threshold_g = 30.0 # threshold of green area width
