From 083109ba89b19f9a5fe38b3ac74b5ef1143f66f0 Mon Sep 17 00:00:00 2001
From: dimitris <Dimitris Sgouropoulos>
Date: Wed, 22 Jun 2016 15:49:59 +0300
Subject: [PATCH] added duration to events, modified standup duration

---
 decision_making/config/parameters.yaml |  8 ++---
 decision_making/msg/Event.msg          |  6 ++--
 decision_making/src/decision.cpp       | 45 +++++++++++++++-----------
 fusion/config/parameters.yaml          |  4 +--
 4 files changed, 37 insertions(+), 26 deletions(-)

diff --git a/decision_making/config/parameters.yaml b/decision_making/config/parameters.yaml
index 374d03e..6f0c27a 100644
--- a/decision_making/config/parameters.yaml
+++ b/decision_making/config/parameters.yaml
@@ -2,10 +2,10 @@ camera_frame    : "camera_link"
 project_path    : find(ros_visual)
 results_topic   : "/fusion/results"
 events_topic   : "/events"
-csv_fields      : "Timestamp, Rect_Id, Annotation"
+csv_fields      : "Timestamp, Rect_Id, Annotation, Duration"
 min_depth       : 0
 max_depth       : 6000
-history_size    : 7
-create_directory: false
-write_csv       : false
+history_size    : 10
+create_directory: true
+write_csv       : true
 display         : false
diff --git a/decision_making/msg/Event.msg b/decision_making/msg/Event.msg
index 7b45845..b948ad6 100644
--- a/decision_making/msg/Event.msg
+++ b/decision_making/msg/Event.msg
@@ -1,2 +1,4 @@
-Header header
-int32 event
+Header 	 header
+int32 	 type
+string   description
+duration duration
diff --git a/decision_making/src/decision.cpp b/decision_making/src/decision.cpp
index 21bf0d4..e15b18f 100644
--- a/decision_making/src/decision.cpp
+++ b/decision_making/src/decision.cpp
@@ -36,10 +36,11 @@ Decision_making::Decision_making()
 
 void Decision_making::callback(const fusion::FusionMsg::ConstPtr& msg)
 {
-    decision_making::Event event_msg;
-    event_msg.header.stamp = msg->header.stamp;
+    decision_making::Event      event_msg;
+    event_msg.header.stamp    = msg->header.stamp;
     event_msg.header.frame_id = camera_frame;
-    event_msg.event = -1;
+    event_msg.type            = -1;
+    event_msg.description     = "";
     
     for(vector<bool>::iterator it = inserted.begin(); it < inserted.end(); it++)
         *it = false;
@@ -125,20 +126,20 @@ void Decision_making::callback(const fusion::FusionMsg::ConstPtr& msg)
                     float duration = event_msg.header.stamp.toSec() - standUp_time.toSec();
                     if((duration > 0.4) && (duration < 2.0))
                     {
-                        ros::Time time = event_msg.header.stamp;
+                        ros::Time time = event_msg.header.stamp + ros::Duration(duration/2);
+                        ros::Duration ros_duration = ros::Duration(duration) + ros::Duration(duration/2);
+                        event_msg.type = 3;
+                        event_msg.description = "stand up";
+                        event_msg.duration = ros_duration;
+                        results_publisher.publish(event_msg);
+                        
                         ofstream storage(session_path + "/decision_making.csv" ,ios::out | ios::app );
                         storage
-                            <<standUp_time<<"\t"
-                            <<i<<"\t"
-                            <<"2\t"<<
-                        endl;
-                        storage
-                            <<event_msg.header.stamp<<"\t"
+                            <<time<<"\t"
                             <<i<<"\t"
-                            <<"3\t"<<
+                            <<"3\t"
+                            <<ros_duration<<
                         endl;
-                        event_msg.event = 3;
-                        results_publisher.publish(event_msg);
                     }
                 }
                 standUp_time = ros::Time(0);
@@ -169,17 +170,25 @@ void Decision_making::callback(const fusion::FusionMsg::ConstPtr& msg)
                     float prev_dist = temp_boxes.at(1).pos.acc_distance;
                     dist += prev_dist;
                     temp_boxes.front().pos.acc_distance = dist;
-                    if(dist > 4000 && prev_dist < 4000 && write_csv)
+                    if(int(dist)%4000 < 500 && int(prev_dist)%4000 > 3500 && write_csv)
                     {
-                        ros::Time time = event_msg.header.stamp;
+                        ros::Time time         = event_msg.header.stamp + ros::Duration(0.25);
+                        event_msg.header.stamp = time;
+                        event_msg.type         = 4;
+                        event_msg.description  = "walked 4 meters";
+                        event_msg.duration     = ros::Duration(0);
+                        results_publisher.publish(event_msg);
+                        
+                        event_msg.header.stamp = msg->header.stamp;
+                        
                         ofstream storage(session_path + "/decision_making.csv" ,ios::out | ios::app );
                         storage
                             <<time<<"\t"
                             <<i<<"\t"
-                            <<"4\t"<<
+                            <<"4\t"
+                            <<ros::Duration(0)<<
                         endl;
-                        event_msg.event = 4;
-                        results_publisher.publish(event_msg);
+                        
                     }
                 }
                 else
diff --git a/fusion/config/parameters.yaml b/fusion/config/parameters.yaml
index 8d636b9..c32fd0a 100644
--- a/fusion/config/parameters.yaml
+++ b/fusion/config/parameters.yaml
@@ -8,6 +8,6 @@ csv_fields      : "Timestamp, Rect_id, Rect_x, Rect_y, Rect_W, Rect_H, Meter_X,
 min_depth       : 0
 max_depth       : 6000
 playback_topics : false
-create_directory: false
-write_csv       : false
+create_directory: true
+write_csv       : true
 display         : true