diff --git a/laser_analysis/config/parameters.yaml b/laser_analysis/config/parameters.yaml index eb566f6..46e6fd4 100644 --- a/laser_analysis/config/parameters.yaml +++ b/laser_analysis/config/parameters.yaml @@ -10,6 +10,7 @@ viz_topic: "~viz_req" dt: 25 human_speed: 5 distance: 4 +min_distance: 1 timewindow: 40 write_to_file: False file: '/home/rosturtle/Desktop/hprStat.csv' diff --git a/laser_analysis/config/parameters_URG-04LX-UG01.yaml b/laser_analysis/config/parameters_URG-04LX-UG01.yaml index 8789292..41b780a 100644 --- a/laser_analysis/config/parameters_URG-04LX-UG01.yaml +++ b/laser_analysis/config/parameters_URG-04LX-UG01.yaml @@ -10,6 +10,7 @@ viz_topic: "~viz_req" dt: 50 human_speed: 5 distance: 4 +min_distance: 1 timewindow: 20 write_to_file: True file: '/home/rosturtle/Desktop/URG04stats.csv' diff --git a/laser_analysis/src/analysis.py b/laser_analysis/src/analysis.py index 97b4ecf..b2028b6 100755 --- a/laser_analysis/src/analysis.py +++ b/laser_analysis/src/analysis.py @@ -31,6 +31,7 @@ timestamp = 0.0 dt_ratio = 1.0 pca_obj = PCA() +min_distance = 1 #list_of: #It contains information about the walk statistics (distance in meters, time for that distance) for each traced_cluster. @@ -45,6 +46,7 @@ def init(): global timewindow, distance global stat_file, writeToFile global pca_obj + global min_distance rospy.init_node('laser_analysis') @@ -58,6 +60,7 @@ def init(): speed_ = rospy.get_param('~human_speed', 5) timewindow = rospy.get_param('~timewindow', 40) distance = rospy.get_param('~distance', 4) + min_distance = rospy.get_param('~min_distance', 1) writeToFile = rospy.get_param('~write_to_file', False) stat_file = rospy.get_param('~file', '/home/hprStats.csv') pca_file = rospy.get_param('~pca_file','/home/myPCA.p') @@ -159,7 +162,7 @@ def analysis(clusters_msg): if sumV == array_sizes[k]: k = k + 1 sumV = 0 - + sumV += num_clusters[j] num_clusters = num_clusters[num_clusters != 0] @@ -224,7 +227,7 @@ def analysis(clusters_msg): prev_index_walk = array_sizes[i] - 1 - # it takes only the last part-cluster bc the previous clusters where computed before (slice-window mode). + # it takes only the last part-cluster bc the previous clusters where computed before (slice-window mode). if human_predict(xk,yk,zk) == 1: walk_analysis(xCl, yCl, i) else: @@ -301,7 +304,7 @@ def cluster_analysis(clusters_msg): if sumV == array_sizes[k]: k = k + 1 sumV = 0 - + sumV += num_clusters[j] num_clusters = num_clusters[num_clusters != 0] @@ -363,7 +366,7 @@ def cluster_analysis(clusters_msg): prev_index_walk = array_sizes[i] - 1 - # it takes only the last part-cluster bc the previous clusters where computed before (slice-window mode). + # it takes only the last part-cluster bc the previous clusters where computed before (slice-window mode). if human_predict(xCl,yCl,zCl) == 1: walk_analysis(xCl, yCl, i) @@ -450,6 +453,7 @@ def walk_analysis(x, y, pos): global timewindow, scan_time, distance, timestamp, cluster_parts global walkTrack, writeToFile, frame_id, results4meters_publisher global dt_ratio + global min_distance split = len(x)/cluster_parts split_count = 0 @@ -483,7 +487,7 @@ def walk_analysis(x, y, pos): else: human.set_timestamp(timestamp) - + human.set_prevMedian(xmed, ymed) human.set_time(time_increment) @@ -492,18 +496,19 @@ def walk_analysis(x, y, pos): split_count += split - if human.get_distance() >= distance: + if human.get_distance() >= distance or human.get_distance() > min_distance: print '\n*****\nHuman {} walked {} meters in {} seconds\n*****\n'.format(human.get_id(), human.get_distance(), human.get_time()) - analysis4meters_msg = Analysis4MetersMsg() + analysis4meters_msg = Analysis4MetersMsg() analysis4meters_msg.header.stamp = rospy.Time.now() analysis4meters_msg.header.frame_id = frame_id - analysis4meters_msg.human_id = human.get_id() - analysis4meters_msg.time_needed = human.get_time() - analysis4meters_msg.distance = human.get_distance() - results4meters_publisher.publish(analysis4meters_msg) + analysis4meters_msg.human_id = human.get_id() + analysis4meters_msg.time_needed = human.get_time() * distance / human.get_distance() + analysis4meters_msg.distance = human.get_distance() + results4meters_publisher.publish(analysis4meters_msg) if writeToFile: write_results(pos) - human.initialise() + if human.get_distance() >= distance: + human.initialise() def write_results(pos):