Merge pull request #1 from RADIO-PROJECT-EU/develop

Publish estimations of 4m walk when the distance walked is bigger tha…
roboskel · Mar 8, 2017 · fd3f4ee · fd3f4ee
2 parents 626788c + 9a3a194
commit fd3f4ee
Show file tree

Hide file tree

Showing 3 changed files with 19 additions and 12 deletions.
diff --git 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
@@ -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
@@ -31,6 +31,7 @@
 timestamp = 0.0
 dt_ratio = 1.0
 pca_obj = PCA()
+min_distance = 1
 
 #list_of<WalkTrack>: 
 #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):