def readXML(file):
tree = ET.parse(file)
root = tree.getroot()
item = root.findall('./tracklets/item')
bboxs = {}
pictograms = {}
arrows = {}
paths = {}
for i, v in enumerate(item):
h = float(v.find('h').text)
w = float(v.find('w').text)
l = float(v.find('l').text)
frame = int(v.find('first_frame').text)
size = Vector3(l, w, h)
label = v.find('objectType').text
track_id = int(v.find('trackid').text)
pose = v.findall('./poses/item')
for j, p in enumerate(pose):
tx = float(p.find('tx').text)
ty = float(p.find('ty').text)
tz = float(p.find('tz').text) + 0.85
rz = float(p.find('rz').text)
q = tf.transformations.quaternion_from_euler(0.0, 0.0, rz)
bbox = BoundingBox()
picto = Pictogram()
arrow = Marker()
bbox = viz_bbox(Vector3(tx, ty, tz), q, size, track_id)
picto = viz_picto(Vector3(tx, ty, -tz / 2.0), q, label)
arrow = viz_arrow(Vector3(tx, ty, tz), q, track_id)
if bboxs.has_key(frame + j) == True:
bboxs[frame + j].append(bbox)
pictograms[frame + j].append(picto)
arrows[frame + j].append(arrow)
else:
bboxs[frame + j] = [bbox]
pictograms[frame + j] = [picto]
arrows[frame + j] = [arrow]
return bboxs, pictograms, arrows
def viz_picto(position, q, label):
picto = Pictogram()
picto.mode = Pictogram.STRING_MODE
picto.pose.position = position
picto.pose.orientation = Quaternion(0.0, -0.5, 0.0, 0.5)
picto.size = 3
picto.color = ColorRGBA(1, 1, 1, 1)
picto.character = label
return picto
def readXML(file):
tree = ET.parse(file)
root = tree.getroot()
item = root.findall('./tracklets/item')
d = {}
pictograms = {}
for i, v in enumerate(item):
h = float(v.find('h').text)
w = float(v.find('w').text)
l = float(v.find('l').text)
frame = int(v.find('first_frame').text)
size = Vector3(l, w, h)
label = v.find('objectType').text
pose = v.findall('./poses/item')
for j, p in enumerate(pose):
tx = float(p.find('tx').text)
ty = float(p.find('ty').text)
tz = float(p.find('tz').text)
rz = float(p.find('rz').text)
q = tf.transformations.quaternion_from_euler(0.0, 0.0, rz)
b = BoundingBox()
b.pose.position = Vector3(tx, ty, tz/2.0)
b.pose.orientation = Quaternion(*q)
b.dimensions = size
b.label = i
picto_text = Pictogram()
picto_text.mode = Pictogram.STRING_MODE
picto_text.pose.position = Vector3(tx, ty, -tz/2.0)
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 = 1
picto_text.color = std_msgs.msg.ColorRGBA(1, 1, 1, 1)
picto_text.character = label
if d.has_key(frame + j) == True:
d[frame + j].append(b)
pictograms[frame + j].append(picto_text)
else:
d[frame + j] = [b]
pictograms[frame + j] = [picto_text]
return d, pictograms
def make_pictogram(character,
position,
frame_id='/map',
duration=0.5,
color=[1.0, 1.0, 1.0]):
"""
Helper function for generating visualization markers
Args:
character (str): Character (icon) to be displayed.
position (list): List containing [x,y,z] positions
frame_id (str): ROS TF frame id
duration (rospy.Duration): How long the label will be displayed for
color (list): List of label color floats from 0 to 1 [r,g,b]
Returns:
Pictogram: A jsk_rviz_plugins/Pictogram message which can be published to RViz
"""
msg = Pictogram()
msg.action = Pictogram.ADD
msg.header.frame_id = frame_id
msg.header.stamp = rospy.Time.now()
msg.mode = Pictogram.PICTOGRAM_MODE
msg.character = character
msg.speed = 1.0
msg.ttl = duration
msg.size = 3
msg.color.r = color[0]
msg.color.g = color[1]
msg.color.b = color[2]
msg.color.a = 1.0
msg.pose.orientation.x = 0.0
msg.pose.orientation.y = -1.0
msg.pose.orientation.z = 0.0
msg.pose.orientation.w = 1.0
msg.pose.position.x = position[0]
msg.pose.position.y = position[1]
msg.pose.position.z = position[2]
return msg
def make_jsk_text_pub(self, name, x, y):
qua = Quaternion()
qua = self.euler_to_quaternion(Vector3(0.0, -1.57, 0.0))
print qua
jsk_text_msg = Pictogram()
jsk_text_msg.header.stamp = rospy.Time.now()
jsk_text_msg.header.frame_id = "map"
jsk_text_msg.pose.position.x = x
jsk_text_msg.pose.position.y = y
jsk_text_msg.pose.position.z = 1.5
jsk_text_msg.pose.orientation.x = qua.x
jsk_text_msg.pose.orientation.y = qua.y
jsk_text_msg.pose.orientation.z = qua.z
jsk_text_msg.pose.orientation.w = qua.w
jsk_text_msg.character = name
jsk_text_msg.mode = 1
jsk_text_msg.color.b = 1.0
jsk_text_msg.color.g = 1.0
jsk_text_msg.color.a = 1.0
jsk_text_msg.action = 4
jsk_text_msg.speed = 0.3
self.jsk_text_pub.publish(jsk_text_msg)
"fa-toggle-up", "fa-trash", "fa-trash-o", "fa-tree", "fa-trello",
"fa-trophy", "fa-truck", "fa-try", "fa-tty", "fa-tumblr",
"fa-tumblr-square", "fa-turkish-lira", "fa-twitch", "fa-twitter",
"fa-twitter-square", "fa-umbrella", "fa-underline", "fa-undo",
"fa-university", "fa-unlink", "fa-unlock", "fa-unlock-alt", "fa-unsorted",
"fa-upload", "fa-usd", "fa-user", "fa-user-md", "fa-users",
"fa-video-camera", "fa-vimeo-square", "fa-vine", "fa-vk", "fa-volume-down",
"fa-volume-off", "fa-volume-up", "fa-warning", "fa-wechat", "fa-weibo",
"fa-weixin", "fa-wheelchair", "fa-wifi", "fa-windows", "fa-won",
"fa-wordpress", "fa-wrench", "fa-xing", "fa-xing-square", "fa-yahoo",
"fa-yelp", "fa-yen", "fa-youtube", "fa-youtube-play", "fa-youtube-square"
]
counter = 0
while not rospy.is_shutdown():
msg = Pictogram()
msg.action = Pictogram.JUMP_ONCE
msg.header.frame_id = "/base_link"
msg.header.stamp = rospy.Time.now()
msg.pose.position.z = 1.6
msg.pose.orientation.w = 0.7
msg.pose.orientation.x = 0
msg.pose.orientation.y = -0.7
msg.pose.orientation.z = 0
msg.mode = Pictogram.PICTOGRAM_MODE
msg.speed = 1.0
# msg.ttl = 5.0
msg.size = 1
msg.color.r = 25 / 255.0
msg.color.g = 255 / 255.0
msg.color.b = 240 / 255.0
"fa-windows",
"fa-won",
"fa-wordpress",
"fa-wrench",
"fa-xing",
"fa-xing-square",
"fa-yahoo",
"fa-yelp",
"fa-yen",
"fa-youtube",
"fa-youtube-play",
"fa-youtube-square"]
counter = 0
while not rospy.is_shutdown():
msg = Pictogram()
msg.action = Pictogram.JUMP_ONCE
msg.header.frame_id = "/base_link"
msg.header.stamp = rospy.Time.now()
msg.pose.position.z = 1.6
msg.pose.orientation.w = 0.7
msg.pose.orientation.x = 0
msg.pose.orientation.y = -0.7
msg.pose.orientation.z = 0
msg.size = 0.5
msg.color.r = 25 / 255.0
msg.color.g = 255 / 255.0
msg.color.b = 240 / 255.0
msg.color.a = 1.0
msg.character = pictograms[counter]
p.publish(msg)
def callback(ref, act):
"msgs = ContactStatesStamped"
if len(ref_contact_states_queue) > buffer_size - 1:
arr = PictogramArray()
arr.header.frame_id = "/odom"
arr.header.stamp = rospy.Time.now()
for i, (ref_st, act_st) in enumerate(zip(ref.states, act.states)):
picto = Pictogram()
if ref_st.state.state == act_st.state.state:
continue
else:
if [ref_st.state.state, act_st.state.state
] == [ContactState.OFF, ContactState.ON]:
if [
x.states[i].state.state
for x in ref_contact_states_queue
] == [ContactState.OFF] * buffer_size and [
x.states[i].state.state
for x in act_contact_states_queue
] == [ContactState.OFF] * buffer_size:
picto.character = "fa-long-arrow-down"
picto.size = 1
picto.pose.orientation = Quaternion(0, -1, 0, 1)
picto.action = Pictogram.ADD
picto.color = ColorRGBA(1.0, 0.0, 0.0, 0.8)
rospy.loginfo("%s early landing %s [s]",
ref_st.header.frame_id,
str(ref_st.state.remaining_time))
elif [
x.states[i].state.state
for x in ref_contact_states_queue
] == [ContactState.ON] * buffer_size and [
x.states[i].state.state
for x in act_contact_states_queue
] == [ContactState.ON] * buffer_size:
picto.character = "up-bold"
picto.size = 1
picto.pose.orientation = Quaternion(0, -1, 0, 1)
picto.action = Pictogram.ADD
picto.color = ColorRGBA(0.0, 1.0, 0.0, 0.8)
rospy.loginfo("%s late taking off",
ref_st.header.frame_id)
print("oso hanare")
else:
continue
elif [ref_st.state.state, act_st.state.state
] == [ContactState.ON, ContactState.OFF]:
if [
x.states[i].state.state
for x in ref_contact_states_queue
] == [ContactState.OFF] * buffer_size and [
x.states[i].state.state
for x in act_contact_states_queue
] == [ContactState.OFF] * buffer_size:
picto.character = "fa-long-arrow-down"
picto.size = 1
picto.pose.orientation = Quaternion(0, -1, 0, 1)
picto.action = Pictogram.ADD
picto.color = ColorRGBA(0.0, 1.0, 0.0, 0.8)
rospy.loginfo("%s late landing",
ref_st.header.frame_id)
elif [
x.states[i].state.state
for x in ref_contact_states_queue
] == [ContactState.ON] * buffer_size and [
x.states[i].state.state
for x in act_contact_states_queue
] == [ContactState.ON] * buffer_size:
picto.character = "up-bold"
picto.size = 1
picto.pose.orientation = Quaternion(0, -1, 0, 1)
picto.action = Pictogram.ADD
picto.color = ColorRGBA(1.0, 0.0, 0.0, 0.8)
rospy.loginfo("%s early taking off %s [s]",
ref_st.header.frame_id,
str(ref_st.state.remaining_time))
else:
continue
picto.header.frame_id = ref_st.header.frame_id
picto.header.stamp = ref_st.header.stamp
arr.pictograms.append(picto)
if len(arr.pictograms) > 0:
pub.publish(arr)
ref_contact_states_queue.pop(0)
act_contact_states_queue.pop(0)
ref_contact_states_queue.append(ref)
act_contact_states_queue.append(act)
"fa-windows",
"fa-won",
"fa-wordpress",
"fa-wrench",
"fa-xing",
"fa-xing-square",
"fa-yahoo",
"fa-yelp",
"fa-yen",
"fa-youtube",
"fa-youtube-play",
"fa-youtube-square"]
counter = 0
while not rospy.is_shutdown():
msg = Pictogram()
msg.action = Pictogram.JUMP_ONCE
msg.header.frame_id = "/base_link"
msg.header.stamp = rospy.Time.now()
msg.pose.position.z = 1.6
msg.pose.orientation.w = 0.7
msg.pose.orientation.x = 0
msg.pose.orientation.y = -0.7
msg.pose.orientation.z = 0
msg.mode = Pictogram.PICTOGRAM_MODE
msg.speed = 1.0
# msg.ttl = 5.0
msg.size = 1
msg.color.r = 25 / 255.0
msg.color.g = 255 / 255.0
msg.color.b = 240 / 255.0
"fa-sort-numeric-down-alt", "fa-sort-numeric-up-alt", "fa-spell-check",
"fa-voicemail", "fa-cotton-bureau", "fa-buy-n-large", "fa-hat-cowboy",
"fa-hat-cowboy-side", "fa-mdb", "fa-mouse", "fa-orcid", "fa-record-vinyl",
"fa-swift", "fa-umbraco", "fa-caravan", "fa-firefox-browser", "fa-ideal",
"fa-microblog", "fa-pied-piper-square", "fa-trailer", "fa-unity"
]
counter = 0
while not rospy.is_shutdown():
initial_x = -int(math.sqrt(len(pictograms))) / 2
arr = PictogramArray()
arr.header.frame_id = "base_link"
arr.header.stamp = rospy.Time.now()
prev_xyz = [initial_x, -10, 0]
for character in pictograms:
msg = Pictogram()
msg.header.frame_id = "base_link"
msg.action = choice(actions)
msg.header.stamp = rospy.Time.now()
msg.pose.position.x = prev_xyz[0] + 1
msg.pose.position.y = prev_xyz[1]
msg.pose.position.z = 0
msg.pose.orientation.w = 0.7
msg.pose.orientation.x = 0
msg.pose.orientation.y = -0.7
msg.pose.orientation.z = 0
msg.size = 1
msg.color.r = 25 / 255.0
msg.color.g = 255 / 255.0
msg.color.b = 240 / 255.0
msg.color.a = 1.0
def callback(ref, act):
"msgs = ContactStatesStamped"
if len(ref_contact_states_queue) > buffer_size - 1:
arr = PictogramArray()
arr.header.frame_id = "/odom"
arr.header.stamp = rospy.Time.now()
for i, (ref_st, act_st) in enumerate(zip(ref.states, act.states)):
picto = Pictogram()
if ref_st.state.state == act_st.state.state:
continue
else:
if [ref_st.state.state, act_st.state.state] == [ContactState.OFF, ContactState.ON]:
if [x.states[i].state.state for x in ref_contact_states_queue] == [ContactState.OFF] * buffer_size and [x.states[i].state.state for x in act_contact_states_queue] == [ContactState.OFF] * buffer_size:
picto.character = "fa-long-arrow-down"
picto.size = 1
picto.pose.orientation = Quaternion(0, -1, 0, 1)
picto.action = Pictogram.ADD
picto.color = ColorRGBA(1.0, 0.0, 0.0, 0.8)
rospy.loginfo("%s early landing %s [s]", ref_st.header.frame_id, str(ref_st.state.remaining_time))
elif [x.states[i].state.state for x in ref_contact_states_queue] == [ContactState.ON] * buffer_size and [x.states[i].state.state for x in act_contact_states_queue] == [ContactState.ON] * buffer_size:
picto.character = "up-bold"
picto.size = 1
picto.pose.orientation = Quaternion(0, -1, 0, 1)
picto.action = Pictogram.ADD
picto.color = ColorRGBA(0.0, 1.0, 0.0, 0.8)
rospy.loginfo("%s late taking off", ref_st.header.frame_id)
print "oso hanare"
else:
continue
elif [ref_st.state.state, act_st.state.state] == [ContactState.ON, ContactState.OFF]:
if [x.states[i].state.state for x in ref_contact_states_queue] == [ContactState.OFF] * buffer_size and [x.states[i].state.state for x in act_contact_states_queue] == [ContactState.OFF] * buffer_size:
picto.character = "fa-long-arrow-down"
picto.size = 1
picto.pose.orientation = Quaternion(0, -1, 0, 1)
picto.action = Pictogram.ADD
picto.color = ColorRGBA(0.0, 1.0, 0.0, 0.8)
rospy.loginfo("%s late landing", ref_st.header.frame_id)
elif [x.states[i].state.state for x in ref_contact_states_queue] == [ContactState.ON] * buffer_size and [x.states[i].state.state for x in act_contact_states_queue] == [ContactState.ON] * buffer_size:
picto.character = "up-bold"
picto.size = 1
picto.pose.orientation = Quaternion(0, -1, 0, 1)
picto.action = Pictogram.ADD
picto.color = ColorRGBA(1.0, 0.0, 0.0, 0.8)
rospy.loginfo("%s early taking off %s [s]", ref_st.header.frame_id, str(ref_st.state.remaining_time))
else:
continue
picto.header.frame_id = ref_st.header.frame_id
picto.header.stamp = ref_st.header.stamp
arr.pictograms.append(picto)
if len(arr.pictograms) > 0:
pub.publish(arr)
ref_contact_states_queue.pop(0)
act_contact_states_queue.pop(0)
ref_contact_states_queue.append(ref)
act_contact_states_queue.append(act)
[0, 0, 0, ' ', False], # 30
[-0.850, -1.000, 0, 'P', True], # 31
[0, 0, 0, ' ', False], # 32
[1.130, -0.200, pi / 2, 'Q', True] # 33
]
p = 0
p_old = 1
posn = 0
move0 = {'theta': 0.0, 'phi': 0.0, 'r': 1.0}
move1 = {'x': 0.0, 'y': 0.0, 'z': 0.0}
old_adrs = 99
pp = Pose()
pict = Pictogram()
auto_cnt = 0
auto_p = 0
autof = resetf = False
auto_focus_point = Point(0, 0, 0)
adrsf = False
auto = True
adrs = True
rate_float = 2
def callback(message):
global move0, move1
"fa-yahoo",
"fa-yelp",
"fa-yen",
"fa-youtube",
"fa-youtube-play",
"fa-youtube-square"]
counter = 0
while not rospy.is_shutdown():
initial_x = -int(math.sqrt(len(pictograms)))/2
arr = PictogramArray()
arr.header.frame_id = "/base_link"
arr.header.stamp = rospy.Time.now()
prev_xyz = [initial_x, -10, 0]
for character in pictograms:
msg = Pictogram()
msg.header.frame_id = "/base_link"
msg.action = choice(actions)
msg.header.stamp = rospy.Time.now()
msg.pose.position.x = prev_xyz[0] + 1
msg.pose.position.y = prev_xyz[1]
msg.pose.position.z = 0
msg.pose.orientation.w = 0.7
msg.pose.orientation.x = 0
msg.pose.orientation.y = -0.7
msg.pose.orientation.z = 0
msg.size = 1
msg.color.r = 25 / 255.0
msg.color.g = 255 / 255.0
msg.color.b = 240 / 255.0
msg.color.a = 1.0
def callback(data):
PictArr.header = data.header
PictArr.pictograms = []
for box in data.boxes:
msg = Pictogram()
msg_string = Pictogram()
msg.action = Pictogram.JUMP
msg.speed = 0.3
msg.header.frame_id = box.header.frame_id
msg.pose.position = box.pose.position
msg.pose.position.y -= 0.1
msg.pose.position.z -= 0.1
msg.pose.orientation.w = 0
msg.pose.orientation.x = 0.7
msg.pose.orientation.y = -0.7
msg.pose.orientation.z = 0
msg.header.stamp = rospy.Time.now()
msg.mode = Pictogram.PICTOGRAM_MODE
msg.size = 0.05
msg.color.r = 25 / 255.0
msg.color.g = 255 / 255.0
msg.color.b = 240 / 255.0
msg.color.a = 1.0
msg.character = "chevron-thin-down"
PictArr.pictograms.append(msg)
msg_string = copy.deepcopy(msg)
msg_string.pose.position.y -= 0.05
msg_string.mode = Pictogram.STRING_MODE
msg_string.size = 0.05
msg_string.character = "box found"
PictArr.pictograms.append(msg_string)
p.publish(PictArr)
def readXML(file):
tree = ET.parse(file)
root = tree.getroot()
item = root.findall('./tracklets/item')
d = {}
boxes_2d = {}
pictograms = {}
for i, v in enumerate(item):
h = float(v.find('h').text)
w = float(v.find('w').text)
l = float(v.find('l').text)
frame = int(v.find('first_frame').text)
size = Vector3(l, w, h)
label = v.find('objectType').text
pose = v.findall('./poses/item')
for j, p in enumerate(pose):
tx = float(p.find('tx').text)
ty = float(p.find('ty').text)
tz = float(p.find('tz').text)
rz = float(p.find('rz').text)
occlusion = float(p.find('occlusion').text)
q = tf.transformations.quaternion_from_euler(0.0, 0.0, rz)
b = BoundingBox()
b.pose.position = Vector3(tx, ty, tz/2.0)
b.pose.orientation = Quaternion(*q)
b.dimensions = size
b.label = i
picto_text = Pictogram()
picto_text.mode = Pictogram.STRING_MODE
picto_text.pose.position = Vector3(tx, ty, -tz/2.0)
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 = 5
picto_text.color = std_msgs.msg.ColorRGBA(1, 1, 1, 1)
picto_text.character = label
# Bounding Box corners
corner_x = np.array([l/2, l/2, -l/2, -l/2, l/2, l/2, -l/2, -l/2])
corner_y = np.array([w/2, -w/2, -w/2, w/2, w/2, -w/2, -w/2, w/2])
corner_z = np.array([0, 0, 0, 0, h, h, h, h])
rz = wrapToPi(rz)
###################
#create box on origin, then translate and rotate according to pose. finally, project into 2D image
# Rotate and translate 3D bounding box in velodyne coordinate system
R = np.array([ [math.cos(rz), -math.sin(rz), 0],
[math.sin(rz), math.cos(rz), 0],
[0, 0, 1]])
corner_3d = np.dot(R,np.array([corner_x, corner_y, corner_z]))
#Translate
corner_3d[0,:] = corner_3d[0,:] + tx
corner_3d[1,:] = corner_3d[1,:] + ty
corner_3d[2,:] = corner_3d[2,:] + tz
#Project to 2D
low_row = np.vstack([corner_3d, np.ones(corner_3d.shape[1], dtype=np.float)])
corner_3d = np.dot(np.asarray(rt_matrix), low_row)
#################################
#Create an orientation vector
orientation_3d = np.dot( R, np.array([[0,0.7*l],[0,0],[0,0]]) )
#Translate
orientation_3d[0,:] = orientation_3d[0,:] + tx
orientation_3d[1,:] = orientation_3d[1,:] + ty
orientation_3d[2,:] = orientation_3d[2,:] + tz
#Project
low_row = np.vstack([orientation_3d, np.ones(orientation_3d.shape[1], dtype=np.float)])
orientation_3d = np.dot(rt_matrix, low_row)
K = np.asarray(cam_to_cam['P_rect_02']).reshape(3,4)
K = K[:3,:3]
corners_2d = projectToImage(corner_3d, K)
orientation_2d = projectToImage(orientation_3d, K)
x1 = min(corners_2d[0,:])
x2 = max(corners_2d[0,:])
y1 = min(corners_2d[1,:])
y2 = max(corners_2d[1,:])
bbox_2d = ImageRect()
bbox_2d.score = -10.0
if ( (label == 'Car' or label=='Truck' or label=='Van') and np.any(corner_3d[2,:]>=0.5)) and (np.any(orientation_3d[2,:]>=0.5) and x1>=0 and x2>=0 and y1>0 and y2>=0 and occlusion <2):
bbox_2d.x = x1
bbox_2d.y = y1
bbox_2d.width = x2-x1
bbox_2d.height = y2-y1
bbox_2d.score = 1.0
if d.has_key(frame + j) == True:
d[frame + j].append(b)
boxes_2d[frame + j].append(bbox_2d)
pictograms[frame + j].append(picto_text)
else:
d[frame + j] = [b]
boxes_2d[frame + j] = [bbox_2d]
pictograms[frame + j]= [picto_text]
return d, boxes_2d, pictograms
def readXML(file):
tree = ET.parse(file)
root = tree.getroot()
item = root.findall('./tracklets/item')
d = {}
boxes_2d = {}
pictograms = {}
for i, v in enumerate(item):
h = float(v.find('h').text)
w = float(v.find('w').text)
l = float(v.find('l').text)
frame = int(v.find('first_frame').text)
size = Vector3(l, w, h)
label = v.find('objectType').text
pose = v.findall('./poses/item')
for j, p in enumerate(pose):
tx = float(p.find('tx').text)
ty = float(p.find('ty').text)
tz = float(p.find('tz').text)
rz = float(p.find('rz').text)
occlusion = float(p.find('occlusion').text)
q = tf.transformations.quaternion_from_euler(0.0, 0.0, rz)
b = BoundingBox()
b.pose.position = Vector3(tx, ty, tz / 2.0)
b.pose.orientation = Quaternion(*q)
b.dimensions = size
b.label = i
picto_text = Pictogram()
picto_text.mode = Pictogram.STRING_MODE
picto_text.pose.position = Vector3(tx, ty, -tz / 2.0)
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 = 5
picto_text.color = std_msgs.msg.ColorRGBA(1, 1, 1, 1)
picto_text.character = label
# Bounding Box corners
corner_x = np.array(
[l / 2, l / 2, -l / 2, -l / 2, l / 2, l / 2, -l / 2, -l / 2])
corner_y = np.array(
[w / 2, -w / 2, -w / 2, w / 2, w / 2, -w / 2, -w / 2, w / 2])
corner_z = np.array([0, 0, 0, 0, h, h, h, h])
rz = wrapToPi(rz)
###################
#create box on origin, then translate and rotate according to pose. finally, project into 2D image
# Rotate and translate 3D bounding box in velodyne coordinate system
R = np.array([[math.cos(rz), -math.sin(rz), 0],
[math.sin(rz), math.cos(rz), 0], [0, 0, 1]])
corner_3d = np.dot(R, np.array([corner_x, corner_y, corner_z]))
#Translate
corner_3d[0, :] = corner_3d[0, :] + tx
corner_3d[1, :] = corner_3d[1, :] + ty
corner_3d[2, :] = corner_3d[2, :] + tz
#Project to 2D
low_row = np.vstack(
[corner_3d,
np.ones(corner_3d.shape[1], dtype=np.float)])
corner_3d = np.dot(np.asarray(rt_matrix), low_row)
#################################
#Create an orientation vector
orientation_3d = np.dot(R, np.array([[0, 0.7 * l], [0, 0], [0,
0]]))
#Translate
orientation_3d[0, :] = orientation_3d[0, :] + tx
orientation_3d[1, :] = orientation_3d[1, :] + ty
orientation_3d[2, :] = orientation_3d[2, :] + tz
#Project
low_row = np.vstack([
orientation_3d,
np.ones(orientation_3d.shape[1], dtype=np.float)
])
orientation_3d = np.dot(rt_matrix, low_row)
K = np.asarray(cam_to_cam['P_rect_02']).reshape(3, 4)
K = K[:3, :3]
corners_2d = projectToImage(corner_3d, K)
orientation_2d = projectToImage(orientation_3d, K)
x1 = min(corners_2d[0, :])
x2 = max(corners_2d[0, :])
y1 = min(corners_2d[1, :])
y2 = max(corners_2d[1, :])
bbox_2d = image_rect()
bbox_2d.score = -10.0
if ((label == 'Car' or label == 'Truck' or label == 'Van')
and np.any(corner_3d[2, :] >= 0.5)) and (
np.any(orientation_3d[2, :] >= 0.5) and x1 >= 0
and x2 >= 0 and y1 > 0 and y2 >= 0 and occlusion < 2):
bbox_2d.x = x1
bbox_2d.y = y1
bbox_2d.width = x2 - x1
bbox_2d.height = y2 - y1
bbox_2d.score = 1.0
if d.has_key(frame + j) == True:
d[frame + j].append(b)
boxes_2d[frame + j].append(bbox_2d)
pictograms[frame + j].append(picto_text)
else:
d[frame + j] = [b]
boxes_2d[frame + j] = [bbox_2d]
pictograms[frame + j] = [picto_text]
return d, boxes_2d, pictograms
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)
"fa-toggle-up", "fa-trash", "fa-trash-o", "fa-tree", "fa-trello",
"fa-trophy", "fa-truck", "fa-try", "fa-tty", "fa-tumblr",
"fa-tumblr-square", "fa-turkish-lira", "fa-twitch", "fa-twitter",
"fa-twitter-square", "fa-umbrella", "fa-underline", "fa-undo",
"fa-university", "fa-unlink", "fa-unlock", "fa-unlock-alt", "fa-unsorted",
"fa-upload", "fa-usd", "fa-user", "fa-user-md", "fa-users",
"fa-video-camera", "fa-vimeo-square", "fa-vine", "fa-vk", "fa-volume-down",
"fa-volume-off", "fa-volume-up", "fa-warning", "fa-wechat", "fa-weibo",
"fa-weixin", "fa-wheelchair", "fa-wifi", "fa-windows", "fa-won",
"fa-wordpress", "fa-wrench", "fa-xing", "fa-xing-square", "fa-yahoo",
"fa-yelp", "fa-yen", "fa-youtube", "fa-youtube-play", "fa-youtube-square"
]
counter = 0
while not rospy.is_shutdown():
msg = Pictogram()
msg.action = Pictogram.JUMP_ONCE
msg.header.frame_id = "/base_link"
msg.header.stamp = rospy.Time.now()
msg.pose.position.z = 1.6
msg.pose.orientation.w = 0.7
msg.pose.orientation.x = 0
msg.pose.orientation.y = -0.7
msg.pose.orientation.z = 0
msg.size = 0.5
msg.color.r = 25 / 255.0
msg.color.g = 255 / 255.0
msg.color.b = 240 / 255.0
msg.color.a = 1.0
msg.character = pictograms[counter]
p.publish(msg)
