Skip to content

Commit

Permalink
Start Data Logger
Browse files Browse the repository at this point in the history 
Signed-off-by: Ken Haagh <[email protected]>
  • Loading branch information
@KenH2
KenH2 committed Dec 3, 2023
1 parent 347977b commit d3ff7d7
Show file tree
Hide file tree
Showing 2 changed files with 113 additions and 34 deletions.
91 changes: 81 additions & 10 deletions people_tracking/src/people_tracker.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,8 @@
from cv_bridge import CvBridge
import copy

import csv

from typing import List
from collections import namedtuple
from UKFclass import *
Expand All @@ -31,19 +33,20 @@

class PeopleTracker:
def __init__(self) -> None:

csv_file = open(csv_file_path, 'w', newline='')
self.csv_writer = csv.writer(csv_file, delimiter=',', quotechar='"', quoting=csv.QUOTE_MINIMAL)
# ROS Initialize
rospy.init_node(NODE_NAME, anonymous=True)
self.subscriber_hoc = rospy.Subscriber(TOPIC_PREFIX + 'HoC', ColourTarget, self.callback_hoc,
queue_size=5)
queue_size=2)
self.subscriber_face = rospy.Subscriber(TOPIC_PREFIX + 'face_detections', ColourCheckedTarget,
self.callback_face, queue_size=5)
self.callback_face, queue_size=2)
self.subscriber_persons = rospy.Subscriber(TOPIC_PREFIX + 'person_detections', DetectedPerson,
self.callback_persons, queue_size=5)
self.callback_persons, queue_size=2)
self.subscriber_image_raw = rospy.Subscriber(name_subscriber_RGB, Image, self.get_latest_image, queue_size=1)

self.publisher_debug = rospy.Publisher(TOPIC_PREFIX + 'debug/people_tracker', Image, queue_size=10)
self.rate = rospy.Rate(20) # 20hz
self.publisher_debug = rospy.Publisher(TOPIC_PREFIX + 'debug/people_tracker', Image, queue_size=2)
self.rate = rospy.Rate(50) # 20hz

# Create a ROS Service Proxys for the reset services
rospy.wait_for_service(TOPIC_PREFIX + 'HoC/reset')
Expand All @@ -68,6 +71,16 @@ def __init__(self) -> None:
self.ukf_from_data = UKF()
self.ukf_past_data = UKF()

self.last_timestamp_hoc = None
self.message_count_hoc = 0
self.rate_estimate_hoc = 0.0
self.last_timestamp_face = None
self.message_count_face = 0
self.rate_estimate_face = 0.0
self.last_timestamp_da = None
self.message_count_da = 0
self.rate_estimate_da = 0.0

def reset(self):
""" Reset all stored variables in Class to their default values."""
self.approved_targets = [Target(0, 0, 0, 320, 240, 0)]
Expand Down Expand Up @@ -154,6 +167,20 @@ def callback_hoc(self, data: ColourCheckedTarget) -> None:
self.detections[idx] = Persons(nr_batch, time, nr_persons, x_positions, y_positions, z_positions,
colour_vectors, face_detected)
self.new_detections = True

self.csv_writer.writerow([nr_batch, time, nr_persons,
x_positions, y_positions, z_positions,
colour_vectors, face_detected])
rospy.loginfo(f"hoc: {nr_batch}")


current_timestamp = rospy.get_time()
if self.last_timestamp_hoc is not None:
time_difference = current_timestamp - self.last_timestamp_hoc
self.rate_estimate_hoc = 1.0 / time_difference if time_difference > 0 else 0.0

self.last_timestamp_hoc = current_timestamp
self.message_count_hoc += 1
else:
rospy.loginfo("HoC detection not used")

Expand All @@ -167,6 +194,18 @@ def callback_face(self, data: ColourCheckedTarget) -> None:
self.detections[idx] = Persons(nr_batch, time, nr_persons, x_positions, y_positions, z_positions,
colour_vectors, face_detections)
self.new_detections = True
self.csv_writer.writerow([nr_batch, time, nr_persons,
x_positions, y_positions, z_positions,
colour_vectors, face_detections])
rospy.loginfo(f"face: {nr_batch}")

current_timestamp = rospy.get_time()
if self.last_timestamp_face is not None:
time_difference = current_timestamp - self.last_timestamp_face
self.rate_estimate_face = 1.0 / time_difference if time_difference > 0 else 0.0

self.last_timestamp_face = current_timestamp
self.message_count_face += 1
else:
rospy.loginfo("Face detection not used")

Expand All @@ -186,6 +225,17 @@ def callback_persons(self, data: DetectedPerson) -> None:
self.detections.append(
Persons(nr_batch, time, nr_persons, x_positions, y_positions, z_positions, colour_vectors, face_detected))
self.new_detections = True
self.csv_writer.writerow([nr_batch, time, nr_persons,
x_positions, y_positions, z_positions,
colour_vectors, face_detected])
rospy.loginfo(f"pos: {nr_batch}")
current_timestamp = rospy.get_time()
if self.last_timestamp_da is not None:
time_difference = current_timestamp - self.last_timestamp_da
self.rate_estimate_da = 1.0 / time_difference if time_difference > 0 else 0.0

self.last_timestamp_da = current_timestamp
self.message_count_da += 1
# rospy.loginfo([person.nr_batch for person in self.detections])

def plot_tracker(self):
Expand Down Expand Up @@ -349,10 +399,10 @@ def track_person(self):
combined = [weight_face * faces[person] +
weight_hoc * norm_hoc_distance[person] +
weight_da * norm_distance_da[person] for person in range(measurement.nr_persons)]
rospy.loginfo(f"face: {faces}, hoc: {distance_hoc}, da: {distance_da}, combined: {combined}")
# rospy.loginfo(f"face: {faces}, hoc: {distance_hoc}, da: {distance_da}, combined: {combined}")
# rospy.loginfo(f"face: {faces}, norm_hoc: {norm_hoc_distance}, norm_da: {norm_distance_da}")
idx_target = combined.index(min(combined))
rospy.loginfo(f"target: {idx_target}")
# rospy.loginfo(f"target: {idx_target}")

if any([flag_face, flag_hoc, flag_da]):
x = measurement.x_positions[idx_target]
Expand All @@ -373,6 +423,7 @@ def track_person(self):
def loop(self):
""" Loop that repeats itself at self.rate. Can be used to execute methods at given rate. """
time_old = rospy.get_time()

while not rospy.is_shutdown():
# if self.new_detections: # Do data association with most recent detection.

Expand All @@ -383,12 +434,18 @@ def loop(self):
while len(self.detections) > 500:
self.detections.pop(0)

#lOG detection rate
rospy.loginfo( f"da: {self.rate_estimate_da:.2f} Hz, face: {self.rate_estimate_face:.2f} Hz, hoc: {self.rate_estimate_hoc:.2f} Hz")

# ToDo Move the picture plotter to different node -> might help with visible lag spikes in tracker
current_time = rospy.get_time()
if self.new_detections and current_time - time_old > 0.2:
time_old = current_time
self.track_person()
# self.track_person()
self.new_detections = False
rospy.loginfo(self.detections[-1])



# Add target lost check (based on time)
if self.time_since_identifiers is None:
Expand All @@ -404,11 +461,25 @@ def remove_outside_batches(lst: List, start_batch: int = 0, end_batch: int = flo
:return: list with all the batches removed.
"""
result = [entry for entry in lst if start_batch <= entry.nr_batch <= end_batch]
# rospy.loginfo(result)
return result

import os
import rospkg
import time

if __name__ == '__main__':
try:
rospack = rospkg.RosPack()
package_path = rospack.get_path("people_tracking")
full_path = os.path.join(package_path, 'data/')

# Make sure the directory exists
os.makedirs(full_path, exist_ok=True)
time = time.ctime(time.time())
csv_file_path = os.path.join(full_path, f'{time}_test.csv')
except:
pass

try:
node_pt = PeopleTracker()
node_pt.loop()
Expand Down
56 changes: 32 additions & 24 deletions people_tracking/src/person_detection.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,6 +40,8 @@ def __init__(self) -> None:
self.latest_image = None # To store the most recent image
self.latest_image_time = None

self.bridge = CvBridge()


# depth
rospy.wait_for_service(TOPIC_PREFIX + 'depth/depth_data')
Expand Down Expand Up @@ -103,22 +105,24 @@ def process_latest_image(self):
latest_image = self.latest_image
latest_image_time = self.latest_image_time
batch_nr = self.batch_nr
bridge = CvBridge()
cv_image = bridge.imgmsg_to_cv2(latest_image, desired_encoding='passthrough')
cv_image = cv2.GaussianBlur(cv_image, (5, 5), 0)

depth_image = self.request_depth_image(self.latest_image_time).image
# rospy.loginfo(type(depth_image))
cv_depth_image = bridge.imgmsg_to_cv2(depth_image, desired_encoding='passthrough')
cv_depth_image = cv2.GaussianBlur(cv_depth_image, (5, 5), 0)
# Import RGB Image
cv_image = self.bridge.imgmsg_to_cv2(latest_image, desired_encoding='passthrough')
cv_image = cv2.GaussianBlur(cv_image, (5, 5), 0)

# People detection
classes, segmentations, bounding_box_corners = self.detect(self.model, cv_image)

if classes is None or segmentations is None:
self.latest_image = None # Clear the latest image after processing
self.latest_image_time = None
return

# Import Depth Image
if depth_camera:
depth_image = self.request_depth_image(self.latest_image_time).image
cv_depth_image = self.bridge.imgmsg_to_cv2(depth_image, desired_encoding='passthrough')
cv_depth_image = cv2.GaussianBlur(cv_depth_image, (5, 5), 0)

detected_persons = []
depth_detected = []
x_positions = []
Expand All @@ -135,24 +139,28 @@ def process_latest_image(self):
cv2.fillPoly(mask, [seg], (255, 255, 255))
cv_image[mask == 0] = 0
cropped_image = cv_image[y1:y2, x1:x2]
image_message = bridge.cv2_to_imgmsg(cropped_image, encoding="passthrough")
image_message = self.bridge.cv2_to_imgmsg(cropped_image, encoding="passthrough")

if depth_camera:
mask_depth = np.zeros_like(cv_depth_image, dtype=np.uint8)
cv2.fillPoly(mask_depth, [seg], (255, 255, 255))

mask_depth = np.zeros_like(cv_depth_image, dtype=np.uint8)
cv2.fillPoly(mask_depth, [seg], (255, 255, 255))
# Extract the values based on the mask
masked_pixels = cv_depth_image[mask_depth]
median_color = np.median(masked_pixels)
# print("Median color:", median_color)

# Extract the values based on the mask
masked_pixels = cv_depth_image[mask_depth]
median_color = np.median(masked_pixels)
# print("Median color:", median_color)

# average_color = cv2.mean(cv_depth_image, mask=mask_depth)
cv_depth_image[mask_depth == 0] = 0
depth_cropped = cv_depth_image[y1:y2, x1:x2]
image_message_depth = bridge.cv2_to_imgmsg(depth_cropped, encoding="passthrough")
depth_detected.append(image_message_depth)
cv_depth_image[mask_depth == 0] = 0
depth_cropped = cv_depth_image[y1:y2, x1:x2]
image_message_depth = self.bridge.cv2_to_imgmsg(depth_cropped, encoding="passthrough")
depth_detected.append(image_message_depth)

# rospy.loginfo(f"color {int(average_color[0])}")
# z_positions.append(int(average_color[0]))
# average_color = cv2.mean(cv_depth_image, mask=mask_depth)
# rospy.loginfo(f"color {int(average_color[0])}")
# z_positions.append(int(average_color[0]))
else:
median_color = 0
z_positions.append(int(median_color))
detected_persons.append(image_message)
x_positions.append(int(x1 + ((x2 - x1) / 2)))
Expand All @@ -179,8 +187,8 @@ def process_latest_image(self):
self.latest_image = None # Clear the latest image after processing
self.latest_image_time = None

for image_message in depth_detected:
self.publisher_debug.publish(image_message)
# for image_message in depth_detected:
# self.publisher_debug.publish(image_message)
# for image_message in detected_persons:
# self.publisher_debug.publish(image_message)

Expand Down

0 comments on commit d3ff7d7

Please sign in to comment.