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 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):
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 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 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 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 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 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
text.bg_color = ROBOT_STATE_BG_COLOR
pub_burst_robot_status.publish(text)
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 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 changeObjState(e):
send_text = OverlayText()
send_text.text = 'ObjState: '+e.dst
send_text.top = 70
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 osm.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 osm.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.2
send_text.fg_color.g = 0.7
send_text.fg_color.b = 0.7
send_text.fg_color.a = 1
send_text.line_width = 1
send_text.text_size = 30
obj_state_text_pub.publish(send_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 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 __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 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
text.bg_color = ROBOT_STATE_BG_COLOR
pub_burst_robot_status.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 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 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 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 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 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 continuousDataUpdatedTimeCallnack(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 = "CONTINUOUS DATA: Not received yet"
diff = 100
else:
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
text.bg_color = UPDATED_TIME_BG_COLOR
pub_continuous_data_updated_time.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 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(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 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 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 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 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 publishModeText(message):
send_text = OverlayText()
send_text.text = message
send_text.top = 100
send_text.left = 0
send_text.width = 300
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.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;
send_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
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)
if diff <= 1.0:
text.fg_color = OK_COLOR
else:
text.fg_color = WARN_COLOR
text.bg_color = UPDATED_TIME_BG_COLOR
pub_lowspeed_data_updated_time.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 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(msg):
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):
lines = [colored_message(msg)] + lines
if len(lines) > line_buffer_length:
lines = lines[0: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 __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 __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
def __init__(self, hz, lookahead_points=40,
global_frame="map",
robot_frame="base_link",
lane_width=3.0,
skip_points=3,
lethal_obstacle_val=250.0,
non_lethal_val_scale=20.0,
sigma_x = 3.5,
sigma_y = 3.0,
visualize=False):
# Locks
self.planner_lock = Lock()
self.obstacle_lock = Lock()
# TF
self.global_frame = global_frame
self.robot_frame = robot_frame
self.tf_buffer = tf2_ros.Buffer()
self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
self.trans = None
self.trans_global = None
self.skip_points = skip_points
#
self.pub_final_trajectory = rospy.Publisher("/final_waypoints", Lane, queue_size=1)
# State
self.cnt_obstacles = 0
self.lanes = None
self.poly_vertices = None
self.current_pose = None
self.closest_waypoint = None
self.current_wps = []
self.current_cvs = []
self.cvs = []
self.current_cvs_2d = None
self.visualize = visualize
self.hz = hz
self.planning_point = 0
self.feasible_plan = False
# Obstacle
self.prev_obstacle_event = rospy.Time.now()
self.states = {"RELAY": 0, "REPLANNING": 1, "AVOID": 2}
self.ref_lane_length = 0
self.current_state = self.states["RELAY"]
# Store trajectories
self.coarse_trajectory = None
self.final_trajectory = None
self.final_lane = Lane()
self.obstacle_list = []
# Subscribers
# TODO: Hybrid state machine
self.lane_polygon = DynamicLanePolygon(lane_width, 7, 15,
lethal_val_scale=lethal_obstacle_val,
non_lethal_val_scale=non_lethal_val_scale,
sigma_x=sigma_x, sigma_y=sigma_y)
self.lookahead_points = lookahead_points
self.planner = DynamicLaneAstarPlanner(None, self.lane_polygon)
self.interpolator_final_waypoints = RationalBezierCurve()
self.sub_current_pose = rospy.Subscriber("/current_pose", PoseStamped, self.cb_current_pose)
self.sub_current_lane = rospy.Subscriber("/base_waypoints", Lane, self.cb_lane_waypoints)
self.sub_closest_waypoint = rospy.Subscriber("/closest_waypoint", Int32, self.cb_closest_waypoint)
#
self.sub_obstacles = rospy.Subscriber("/detected_obstacles", DetectedObstacles, self.cb_obstacle_detection)
# Obstacle deque
self.obstacle_deque = deque()
# 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"
self.pub_state_text = rospy.Publisher("hotaru/state_text", OverlayText, queue_size=1)
# Handle visualization
if visualize:
# Visualize selected trajectory
self.pub_viz_selected_trajectory_points = rospy.Publisher("/trajectory_lookahead/visualization", MarkerArray, queue_size=1)
self.viz_markers = MarkerArray()
self.viz_marker = Marker()
self.viz_marker.header.frame_id = self.robot_frame
self.viz_marker.type = Marker.LINE_STRIP
self.viz_marker.color.r = 1.0
self.viz_marker.color.g = 1.0
self.viz_marker.color.a = 0.5
self.viz_marker.scale.x = 0.6
# Visualize current lane
self.pub_viz_current_lane_point = rospy.Publisher("/current_lane/visualization", Marker, queue_size=1)
self.viz_current_lane_marker = Marker()
self.viz_current_lane_marker.type = Marker.SPHERE
self.viz_current_lane_marker.color.r = 1.0
self.viz_current_lane_marker.color.b = 1.0
self.viz_current_lane_marker.color.a = 0.4
self.viz_current_lane_marker.scale.x = 0.5
self.viz_current_lane_marker.scale.y = 0.5
self.viz_current_lane_marker.scale.z = 0.5
self.viz_current_lane_marker.header.frame_id = self.robot_frame
# Final trajectory
self.pub_viz_final_trajectory = rospy.Publisher("/final_trajectory/visualization", MarkerArray, queue_size=1)
self.viz_marker_array_final_trajectory = MarkerArray()
# Visualize orientation
self.viz_marker_orientation_field = Marker()
self.viz_marker_orientation_field.type = Marker.LINE_LIST
self.viz_marker_orientation_field.ns = "orientation_field"
self.viz_marker_orientation_field.color.r = 0.2
self.viz_marker_orientation_field.color.g = 0.9
self.viz_marker_orientation_field.color.b = 0.4
self.viz_marker_orientation_field.color.a = 0.95
self.viz_marker_orientation_field.scale.x = 0.4
self.viz_marker_orientation_field.header.frame_id = self.global_frame
self.viz_marker_array_final_trajectory.markers.append(self.viz_marker_orientation_field)
# Tesselation visualization
self.viz_marker_tesselation = MarkerArray()
# Visualize tesselation grid
self.pub_viz_tesselation_points = rospy.Publisher("/tesselation_grid/visualization",
MarkerArray, queue_size=1)
rospy.init_node("tf_diff")
src1 = rospy.get_param("~src1")
src2 = rospy.get_param("~src2")
listener = tf.TransformListener()
r = rospy.Rate(1)
pub = rospy.Publisher("diff_text", OverlayText)
while not rospy.is_shutdown():
try:
(pos, rot) = listener.lookupTransform(src1, src2, rospy.Time(0))
pos_diff = np.linalg.norm(pos)
# quaternion to rpy
rpy = euler_from_quaternion(rot)
rot_diff = np.linalg.norm(rpy)
print (pos_diff, rot_diff)
msg = OverlayText()
msg.width = 1000
msg.height = 200
msg.left = 10
msg.top = 10
msg.text_size = 20
msg.line_width = 2
msg.font = "DejaVu Sans Mono"
msg.text = """%s <-> %s
pos: %f
rot: %f
""" % (src1, src2, pos_diff, rot_diff)
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.0)
pub.publish(msg)
except:
def callback(self, front_MarkerArray, back_MarkerArray, TwistStamped):
# print("front",len(front_MarkerArray.markers)/4)
# print("back",len(back_MarkerArray.markers)/4)
# # Concat front and back MarkerArray Messages
add_MarkerArray = copy.deepcopy(front_MarkerArray)
for i in range(len(back_MarkerArray.markers)):
add_MarkerArray.markers.append(back_MarkerArray.markers[i])
# print("add",len(add_MarkerArray.markers)/4)
# print("done")
if len(add_MarkerArray.markers) == 0:
return
header = add_MarkerArray.markers[0].header
frame = header.seq
boxes = BoundingBoxArray() #3D Boxes with JSK
boxes.header = header
texts = PictogramArray() #Labels with JSK
texts.header = header
obj_ori_arrows = MarkerArray() #arrow with visualization_msgs
velocity_markers = MarkerArray() #text with visualization_msgs
obj_path_markers = MarkerArray() # passed path
warning_line_markers = MarkerArray()
dets = np.zeros((0, 9)) # (None, 9) : 9는 사용할 3d bbox의 파라미터 개수
obj_box_info = np.empty((0, 7))
obj_label_info = np.empty((0, 2))
# frame을 rviz에 출력
overlayTxt = OverlayText()
overlayTxt.left = 10
overlayTxt.top = 10
overlayTxt.width = 1200
overlayTxt.height = 1200
overlayTxt.fg_color.a = 1.0
overlayTxt.fg_color.r = 1.0
overlayTxt.fg_color.g = 1.0
overlayTxt.fg_color.b = 1.0
overlayTxt.text_size = 12
overlayTxt.text = "Frame_seq : {}".format(frame)
det_boxes = BoundingBoxArray() #3D Boxes with JSK
det_boxes.header = header
# Receive each objects info in this frame
for object_info in add_MarkerArray.markers:
#extract info [ frame,type(label),tx,ty,tz,h,w,l,ry ]
if object_info.ns == "/detection/lidar_detector/box_markers":
tx = object_info.pose.position.x
ty = object_info.pose.position.y
tz = object_info.pose.position.z
l = object_info.scale.x
w = object_info.scale.y
h = object_info.scale.z
quaternion_xyzw = [object_info.pose.orientation.x, object_info.pose.orientation.y, \
object_info.pose.orientation.z, object_info.pose.orientation.w]
rz = tf.transformations.euler_from_quaternion(
quaternion_xyzw)[2]
obj_box_info = np.append(
obj_box_info,
[[-ty, -tz, tx - 0.27, h, w, l, -rz + np.pi / 2]],
axis=0)
size_det = Vector3(l, w, h)
det_box = BoundingBox()
det_box.header = header
det_box.pose.position = Point(tx, ty, tz)
q_det_box = tf.transformations.quaternion_from_euler(
0.0, 0.0, rz) # 어쩔 수 없이 끝단에서만 90도 돌림
det_box.pose.orientation = Quaternion(*q_det_box)
det_box.dimensions = size_det
det_boxes.boxes.append(det_box)
elif object_info.ns == "/detection/lidar_detector/label_markers":
label = object_info.text.strip()
if label == '':
label = 'None'
obj_label_info = np.append(obj_label_info, [[frame, label]],
axis=0)
dets = np.concatenate((obj_label_info, obj_box_info), axis=1)
self.pub_det_markerarray.publish(det_boxes)
del current_id_list[:]
# All Detection Info in one Frame
bboxinfo = dets[dets[:, 0] == str(frame),
2:9] # [ tx, ty, tz, h, w, l, rz ]
additional_info = dets[dets[:, 0] == str(frame), 0:2] # frame, labe
reorder = [3, 4, 5, 0, 1, 2,
6] # [tx,ty,tz,h,w,l,ry] -> [h,w,l,tx,ty,tz,theta]
reorder_back = [3, 4, 5, 0, 1, 2,
6] # [h,w,l,tx,ty,tz,theta] -> [tx,ty,tz,h,w,l,ry]
reorder2velo = [2, 0, 1, 3, 4, 5, 6]
bboxinfo = bboxinfo[:,
reorder] # reorder bboxinfo parameter [h,w,l,x,y,z,theta]
bboxinfo = bboxinfo.astype(np.float64)
dets_all = {'dets': bboxinfo, 'info': additional_info}
# ObjectTracking from Detection
trackers = self.mot_tracker.update(dets_all) # h,w,l,x,y,z,theta
trackers_bbox = trackers[:, 0:7]
trackers_info = trackers[:, 7:10] # id, frame, label
trackers_bbox = trackers_bbox[:,
reorder_back] # reorder_back bboxinfo parameter [tx,ty,tz,h,w,l,ry]
trackers_bbox = trackers_bbox[:,
reorder2velo] # reorder coordinate system cam to velo
trackers_bbox = trackers_bbox.astype(np.float64)
trackers_bbox[:, 0] = trackers_bbox[:, 0]
trackers_bbox[:, 1] = trackers_bbox[:, 1] * -1
trackers_bbox[:, 2] = trackers_bbox[:, 2] * -1
trackers_bbox[:, 6] = trackers_bbox[:, 6] * -1
# for문을 통해 각 objects들의 정보를 추출하여 사용
for b, info in zip(trackers_bbox, trackers_info):
bbox = BoundingBox()
bbox.header = header
# parameter 뽑기 [tx,ty,tz,h,w,l,rz]
tx_trk, ty_trk, tz_trk = float(b[0]), float(b[1]), float(b[2])
rz_trk = float(b[6])
size_trk = Vector3(float(b[5]), float(b[4]), float(b[3]))
obj_id = info[0]
label_trk = info[2]
bbox_color = colorCategory20(int(obj_id))
odom_mat = get_odom(self.tf2, "velo_link", "map")
xyz = np.array(b[:3]).reshape(1, -1)
points = np.array((0, 3), float)
if odom_mat is not None:
points = get_transformation(odom_mat, xyz)
# 이전 x frame 까지 지나온 points들을 저장하여 반환하는 함수
# obj_id와 bbox.label은 단지 type차이만 날뿐 같은 데이터
# path_points_list = points_path(tx_trk, ty_trk, tz_trk, obj_id)
path_points_list = points_path(points[0, 0], points[0, 1],
points[0, 2], obj_id)
map_header = copy.deepcopy(header)
map_header.frame_id = "/map"
bbox_color = colorCategory20(int(obj_id))
path_marker = Marker(
type=Marker.LINE_STRIP,
id=int(obj_id),
lifetime=rospy.Duration(0.5),
# pose=Pose(Point(0,0,0), Quaternion(0, 0, 0, 1)), # origin point position
scale=Vector3(0.1, 0.0, 0.0), # line width
header=map_header,
color=bbox_color)
path_marker.points = path_points_list
obj_path_markers.markers.append(path_marker)
# Tracker들의 BoundingBoxArray 설정
bbox.pose.position = Point(tx_trk, ty_trk, tz_trk / 2.0)
q_box = tf.transformations.quaternion_from_euler(
0.0, 0.0, rz_trk + np.pi / 2) # 어쩔 수 없이 끝단에서만 90도 돌림
bbox.pose.orientation = Quaternion(*q_box)
bbox.dimensions = size_trk
bbox.label = int(obj_id)
boxes.boxes.append(bbox)
picto_text = Pictogram()
picto_text.header = header
picto_text.mode = Pictogram.STRING_MODE
picto_text.pose.position = Point(tx_trk, ty_trk, -tz_trk)
# q = tf.transformations.quaternion_from_euler(0.7, 0.0, -0.7)
picto_text.pose.orientation = Quaternion(0.0, -0.5, 0.0, 0.5)
picto_text.size = 4
picto_text.color = std_msgs.msg.ColorRGBA(1, 1, 1, 1)
picto_text.character = label_trk + ' ' + str(bbox.label)
texts.pictograms.append(picto_text)
# GPS sensor values
oxtLinear = TwistStamped.twist.linear
# oxtLinear = TwistStamped.twist.linear
# Tracker들의 속도 추정
obj_velo, dx_t, dy_t, dz_t = obj_velocity([tx_trk, ty_trk, tz_trk],
bbox.label, oxtLinear)
if obj_velo != None:
obj_velo = np.round_(obj_velo, 1) # m/s
obj_velo = obj_velo * 3.6 # km/h
obj_velo_scale = convert_velo2scale(obj_velo)
# # Tracker들의 Orientation
q_ori = tf.transformations.quaternion_from_euler(
0.0, 0.0, rz_trk + np.pi / 2) # 어쩔 수 없이 끝단에서만 90도 돌림
obj_ori_arrow = Marker(
type=Marker.ARROW,
id=bbox.label,
lifetime=rospy.Duration(0.2),
pose=Pose(Point(tx_trk, ty_trk, tz_trk / 2.0),
Quaternion(*q_ori)),
scale=Vector3(obj_velo_scale, 0.5, 0.5),
header=header,
# color=ColorRGBA(0.0, 1.0, 0.0, 0.8))
color=bbox_color)
obj_ori_arrows.markers.append(obj_ori_arrow)
obj_velo_marker = Marker(type=Marker.TEXT_VIEW_FACING,
id=bbox.label,
lifetime=rospy.Duration(0.5),
pose=Pose(Point(tx_trk, ty_trk, tz_trk),
Quaternion(0.0, -0.5, 0.0,
0.5)),
scale=Vector3(1.5, 1.5, 1.5),
header=header,
color=ColorRGBA(1.0, 1.0, 1.0, 1.0),
text="{}km/h".format(obj_velo))
velocity_markers.markers.append(obj_velo_marker)
current_id_list.append(bbox.label)
# Warning object line
warning_line = Marker(
type=Marker.LINE_LIST,
id=int(obj_id),
lifetime=rospy.Duration(0.2),
pose=Pose(Point(0, 0, 0),
Quaternion(0, 0, 0, 1)), # origin point position
scale=Vector3(0.2, 0.0, 0.0), # line width
header=header,
color=ColorRGBA(1.0, 0.0, 0.0, 1.0))
d = dist_from_objBbox(tx_trk, ty_trk, tz_trk, size_trk.x,
size_trk.y, size_trk.z)
if d < MIN_WARNING_DIST:
warning_line.points = Point(tx_trk, ty_trk,
tz_trk), Point(0.0, 0.0, 0.0)
warning_line_markers.markers.append(warning_line)
# Change Outer Circle Color
outer_circle_color = ColorRGBA(1.0 * 25 / 255, 1.0, 0.0, 1.0)
if len(warning_line_markers.markers) > 0:
outer_circle_color = ColorRGBA(1.0 * 255 / 255, 1.0 * 0 / 255,
1.0 * 0 / 255, 1.0)
# ego_vehicle's warning boundary
outer_circle = Marker(
type=Marker.CYLINDER,
id=int(obj_id),
lifetime=rospy.Duration(0.5),
pose=Pose(Point(0.0, 0.0, -2.0), Quaternion(0, 0, 0, 1)),
scale=Vector3(8.0, 8.0, 0.1), # line width
header=header,
color=outer_circle_color)
inner_circle = Marker(
type=Marker.CYLINDER,
id=int(obj_id),
lifetime=rospy.Duration(0.5),
pose=Pose(Point(0.0, 0.0, -1.8), Quaternion(0, 0, 0, 1)),
scale=Vector3(7.0, 7.0, 0.2), # line width
header=header,
color=ColorRGBA(0.22, 0.22, 0.22, 1.0))
# ego-vehicle velocity
selfvelo = np.sqrt(oxtLinear.x**2 + oxtLinear.y**2 + oxtLinear.z**2)
selfvelo = np.round_(selfvelo, 1) # m/s
selfvelo = selfvelo * 3.6 # km/h
oxtAngular = TwistStamped.twist.angular
q_gps = tf.transformations.quaternion_from_euler(
oxtAngular.x, oxtAngular.y, oxtAngular.z)
# # ego-vehicle 사진 출력
ego_car = Marker(type=Marker.MESH_RESOURCE,
id=0,
lifetime=rospy.Duration(0.5),
pose=Pose(Point(0.0, 0.0, -1.8),
Quaternion(0, 0, 0, 1)),
scale=Vector3(1.5, 1.5, 1.5),
header=header,
action=Marker.ADD,
mesh_resource=CAR_DAE_PATH,
color=ColorRGBA(1.0, 1.0, 1.0, 1.0))
# Self ego Velocity
text_marker = Marker(type=Marker.TEXT_VIEW_FACING,
id=0,
lifetime=rospy.Duration(0.5),
pose=Pose(Point(-7.0, 0.0, 0.0),
Quaternion(0, 0, 0, 1)),
scale=Vector3(1.5, 1.5, 1.5),
header=header,
color=ColorRGBA(1.0, 1.0, 1.0, 1.0),
text="{}km/h".format(selfvelo))
for i in prior_trk_xyz.keys():
if i not in current_id_list:
prior_trk_xyz.pop(i)
self.pub_frame_seq.publish(overlayTxt)
self.pub_boxes.publish(boxes)
self.pub_pictograms.publish(texts)
self.pub_selfvelo_text.publish(text_marker)
# self.pub_selfveloDirection.publish(arrow_marker)
self.pub_objs_ori.publish(obj_ori_arrows)
self.pub_objs_velo.publish(velocity_markers)
self.pub_path.publish(obj_path_markers)
self.pub_warning_lines.publish(warning_line_markers)
self.pub_ego_outCircle.publish(outer_circle)
self.pub_ego_innerCircle.publish(inner_circle)
self.pub_ego_car.publish(ego_car)
def disable(self): msg = OverlayText() msg.action = OverlayText.DELETE self.pub.publish(msg)
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 trajectory_callback(traj):
id_gen = id_generator(1)
# path visualizing
# waypoint position, connected with line
traj_viz = MarkerArray()
path_viz = Marker()
path_viz.header.frame_id = 'world'
path_viz.header.stamp = rospy.Time.now()
path_viz.id = next(id_gen)
path_viz.scale = Vector3(0.05, 0, 0)
path_viz.color = ColorRGBA(0, 1, 0, 1)
path_viz.type = Marker.LINE_STRIP
path_viz.action = Marker.ADD
for pt in traj.points:
path_viz.points.append(pt.transforms[0].translation)
traj_viz.markers.append(path_viz)
# waypoint direction. (linear velocity at waypoint)
# the direction of linear velocity is the same as the x direction of the drone, which is also the
# desired heading of the drone.
#
# to fully determine the orientation, we still need the direction of y or z axis of the drone
# after that we can construct a rotation matrix, and then convert it into quaternion
num_points = len(traj.points)
num_velocity_viz = 5
step = max(1, num_points / num_velocity_viz)
i = 0
while i < num_points:
pt = traj.points[i]
translation = pt.transforms[0].translation
linear = pt.velocities[0].linear
norm = math.sqrt(linear.x ** 2 + linear.y ** 2 + linear.z ** 2)
if norm < 1e-3:
i += step
continue
q = geo.vector_to_quat(linear)
velocity_viz = Marker()
velocity_viz.header.frame_id = "world"
velocity_viz.header.stamp = rospy.Time.now()
velocity_viz.id = next(id_gen)
velocity_viz.type = Marker.ARROW
velocity_viz.action = Marker.ADD
velocity_viz.pose.position = translation
velocity_viz.pose.orientation = Quaternion(*(q.tolist()))
velocity_viz.scale = Vector3(norm, 0.05, 0.05)
velocity_viz.color = ColorRGBA(1, 0, 0, 0.5)
traj_viz.markers.append(velocity_viz)
i += step
viz_pub.publish(traj_viz)
# visualize information of the next 3 waypoints
if num_points >= 3:
p1 = traj.points[0]
p2 = traj.points[1]
p3 = traj.points[2]
waypoints_info_text = OverlayText()
waypoints_info_text.action = OverlayText.ADD
waypoints_info_text.width = 128
waypoints_info_text.height = 64
waypoints_info_text.left = 0
waypoints_info_text.top = 0
waypoints_info_text.bg_color = ColorRGBA(0, 0, 0, 0.5)
waypoints_info_text.fg_color = ColorRGBA(1, 1, 1, 1)
waypoints_info_text.text_size = 8
waypoints_info_text.text = """Next waypoint @ {:.2f}s: {:.2f} {:.2f} {:.2f}. Speed: {:.2f}
Next waypoint @ {:.2f}s: {:.2f} {:.2f} {:.2f}. Speed: {:.2f}
Next waypoint @ {:.2f}s: {:.2f} {:.2f} {:.2f}. Speed: {:.2f}
""".format(p1.time_from_start.to_sec(), p1.transforms[0].translation.x, p1.transforms[0].translation.y, p1.transforms[0].translation.z, magnitude(p1.velocities[0].linear),
p2.time_from_start.to_sec(), p2.transforms[0].translation.x, p2.transforms[0].translation.y, p2.transforms[0].translation.z, magnitude(p2.velocities[0].linear),
p3.time_from_start.to_sec(), p3.transforms[0].translation.x, p3.transforms[0].translation.y, p3.transforms[0].translation.z, magnitude(p3.velocities[0].linear),)
waypoints_info_pub.publish(waypoints_info_text)
rospy.init_node("visualize_on_rviz")
json_file = rospy.get_param("~json")
bin_contents = dict(list(get_bin_contents(json_file)))
work_order = dict(list(get_work_order(json_file)))
pub_l = rospy.Publisher("~left", OverlayText, queue_size=1)
pub_r = rospy.Publisher("~right", OverlayText, queue_size=1)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
msg = {}
for arm in ["left", "right"]:
msg[arm] = OverlayText()
# robot state
state_text = rospy.get_param(arm + "_hand/state", "")
target_bin = rospy.get_param(arm + "_hand/target_bin", "")
target_text = target_bin
# contents and order
contents_text = ""
order_text = ""
if target_bin:
contents = bin_contents[target_bin]
contents_text = "objects in bin '{}': {}".format(target_bin.upper(), ", ".join(contents))
order = work_order[target_bin]
order_text = order
# recognition result
result_text = ""
if recognition_msg[arm] is not None:
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 publish_txt_info(self, txt):
msg = OverlayText()
msg.text = txt
self.text_publisher.publish(msg)
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
