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 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 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 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 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 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 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 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 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 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):
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(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 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 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 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 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 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 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 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)
pass
#end state machine
#publication
self.state_machine_pub_.publish(self.state_name_[self.state_machine_])
if self.visualization_ == True:
if self.state_machine_ != self.FINISH_STATE_:
self.task_elapsed_time = (rospy.Time.now() -
self.task_start_time_).to_sec()
text_msg = OverlayText()
text_msg.width = 500
text_msg.height = 110
text_msg.left = 10
text_msg.top = 10
text_msg.text_size = 20
text_msg.line_width = 2
text_msg.font = "DejaVu Sans Mono"
text_msg.text = """Task Kind:%d
State:%d,%s
Time:%.1f """ % (
self.task_kind_, self.state_machine_,
self.state_name_[self.state_machine_], self.task_elapsed_time)
text_msg.fg_color = ColorRGBA(25 / 255.0, 1.0, 240.0 / 255.0, 1.0)
text_msg.bg_color = ColorRGBA(0.0, 0.0, 0.0, 0.2)
self.state_visualization_pub_.publish(text_msg)
target_pos_msg = Odometry()
target_pos_msg.header = self.odom_.header
if self.target_frame_ == self.GLOBAL_FRAME_:
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 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)
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:
rospy.logerr("ignore error")
finally:
r.sleep()
def isConvergent(self,
frame,
target_xy,
target_z,
target_yaw,
gps_mode,
pos_conv_thresh,
yaw_conv_thresh,
vel_conv_thresh,
att_conv_thresh=0.06):
current_xy = self.getBaselinkPos()[0:2]
current_z = self.getBaselinkPos()[2]
current_yaw = self.getBaselinkRPY()[2]
current_vel = self.getBaselinkLinearVel()
current_rp = self.getBaselinkRPY()[0:2]
if gps_mode:
delta_pos = gps2xy(self.gps_lat_lon_, target_xy)
else:
if frame == 'global':
delta_pos = np.array([
target_xy[0] - current_xy[0], target_xy[1] - current_xy[1],
target_z - current_z
])
elif frame == 'local':
delta_pos = np.array(
[target_xy[0], target_xy[1], target_z - current_z])
else:
rospy.logerr('invalid frame %s' % (frame))
return
delta_yaw = target_yaw - current_yaw
if delta_yaw > np.pi:
delta_yaw -= np.pi * 2
elif delta_yaw < -np.pi:
delta_yaw += np.pi * 2
if self.debug_view_:
text = OverlayText()
text.width = 400
text.height = 200
text.left = 10
text.top = 10
text.text_size = 12
text.line_width = 2
text.font = "DejaVu Sans Mono"
text.text = 'Diff\n pos: {:.4g}, yaw: {:.4g}, vel: {:.4g}\n roll: {:.4g}, pitch: {:.4g}\nSetPoint\n x: {:.11g} y: {:.11g} z: {:.4g} yaw: {:.4g}'.format(
np.linalg.norm(delta_pos), abs(delta_yaw),
np.linalg.norm(current_vel), abs(current_rp[0]),
abs(current_rp[1]), target_xy[0], target_xy[1], target_z,
target_yaw)
self.nav_debug_pub_.publish(text)
if np.linalg.norm(delta_pos) < pos_conv_thresh and abs(
delta_yaw) < yaw_conv_thresh and np.linalg.norm(
current_vel) < vel_conv_thresh and abs(
current_rp[0]) < att_conv_thresh and abs(
current_rp[0]) < att_conv_thresh:
return True
else:
return False
def callback(self, msg):
main_msg = np.reshape(msg.data, [3, 7])
#print(main_msg)
pos_x = main_msg[0, 0]
pos_y = main_msg[0, 1]
vel_x = main_msg[0, 2]
vel_y = main_msg[0, 3]
acc_x = main_msg[0, 4]
acc_y = main_msg[0, 5]
rad = main_msg[0, 6]
if PUBLISH:
self.x_1.publish(pos_x)
self.y_1.publish(pos_y)
self.x_vel_1.publish(vel_x)
self.y_vel_1.publish(vel_y)
self.x_acc_1.publish(acc_x)
self.y_acc_1.publish(acc_y)
self.rad_1.publish(rad)
text = OverlayText()
text.width = 400
text.height = 600
text.left = 10
text.top = 10
text.text_size = 12
text.line_width = 2
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)
text.font = "DejaVu Sans Mono"
arg = [x for x in main_msg[0, :]]
dist = math.sqrt((arg[0]**2 + arg[1]**2))
speed = math.sqrt((arg[2]**2 + arg[3]**2))
text.text = """
Runtime: %.2f ms
Object 1
Position = [%.2f, %.2f]
Distance = %.2f
Velocity = [%.2f, %.2f]
Speed = %.2f
Acceleration = [%.2f, %.2f]
True Radius = %.2f
""" % (self.runtime, arg[0], arg[1], dist, arg[2], arg[3], speed, arg[4],
arg[5], arg[6])
self.text_pub_1.publish(text)
arg = [x for x in main_msg[1, :]]
dist = math.sqrt((arg[0]**2 + arg[1]**2))
speed = math.sqrt((arg[2]**2 + arg[3]**2))
text.text = """
Object 2
Position = [%.2f, %.2f]
Distance = %.2f
Velocity = [%.2f, %.2f]
Speed = %.2f
Acceleration = [%.2f, %.2f]
True Radius = %.2f
""" % (arg[0], arg[1], dist, arg[2], arg[3], speed, arg[4], arg[5], arg[6])
self.text_pub_2.publish(text)
arg = [x for x in main_msg[2, :]]
dist = math.sqrt((arg[0]**2 + arg[1]**2))
speed = math.sqrt((arg[2]**2 + arg[3]**2))
text.text = """
Object 3
Position = [%.2f, %.2f]
Distance = %.2f
Velocity = [%.2f, %.2f]
Speed = %.2f
Acceleration = [%.2f, %.2f]
True Radius = %.2f
""" % (arg[0], arg[1], dist, arg[2], arg[3], speed, arg[4], arg[5], arg[6])
self.text_pub_3.publish(text)
for i, mu in enumerate(main_msg[:3]):
self.time_elapsed = rospy.get_time() - self.start_time
circles = self.obstacles[i]
circles_lidar = self.lidars[i]
self.print_mu[i].append(
np.append(mu,
(self.runtime, self.time_elapsed, circles.center.x,
circles.center.y, circles.true_radius,
circles_lidar.center.x, circles_lidar.center.y,
circles_lidar.true_radius)))
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:
rospy.logerr("ignore error")
finally:
r.sleep()
