minor changes

decision_making/config/parameters.yaml

@@ -5,6 +5,7 @@ events_topic   : "/events"
 csv_fields      : "Timestamp, Rect_Id, Annotation"
 min_depth       : 0
 max_depth       : 6000
+history_size    : 20
 create_directory: true
 write_csv       : true
 display         : false

decision_making/include/decision.hpp

@@ -80,6 +80,7 @@ class Decision_making
         bool write_csv;
 		struct tm gmtm;
         int frameCounter = -1;
+        int history_size;
         ros::Time standUp_time = ros::Time(0);
 
         vector<Box> boxes;

decision_making/src/decision.cpp

@@ -12,6 +12,7 @@ Decision_making::Decision_making()
 	local_nh.param("csv_fields"      , csv_fields      , string(""));
 	local_nh.param("max_depth"       , max_depth       , DEPTH_MAX);
 	local_nh.param("min_depth"       , min_depth       , DEPTH_MIN);
+	local_nh.param("history_size"    , history_size    , 7);
 	local_nh.param("create_directory", create_dir      , false);
 	local_nh.param("write_csv"       , write_csv       , false);
 
@@ -36,7 +37,7 @@ Decision_making::Decision_making()
 void Decision_making::callback(const fusion::FusionMsg::ConstPtr& msg)
 {
     decision_making::Event event_msg;
-    event_msg.header.stamp = ros::Time::now();
+    event_msg.header.stamp = msg->header.stamp;
     event_msg.header.frame_id = camera_frame;
     event_msg.four_meter = false;
     event_msg.stand_up = false;
@@ -69,15 +70,16 @@ void Decision_making::callback(const fusion::FusionMsg::ConstPtr& msg)
         else
         {
             boxes_hist.at(box.id).insert(boxes_hist.at(box.id).begin(), box);
-            if(boxes_hist.at(box.id).size() > 7)
+            if(boxes_hist.at(box.id).size() > history_size)
                 boxes_hist.at(box.id).pop_back();
             inserted.at(box.id) = true;
         }
     }
 
 
     //Calculates the median distance and publishes the approximate time when
-    //a person has walked 4 meters
+    //a person has walked 4 meters and the median_top and calculates when
+    //he stands up
     int i = 0;
     for(vector<vector<Decision_making::Box>>::iterator it = boxes_hist.begin(); it < boxes_hist.end(); it++, i++)
     {
@@ -106,6 +108,7 @@ void Decision_making::callback(const fusion::FusionMsg::ConstPtr& msg)
         }
         else
         {
+
             Decision_making::Position pos;
             pos = positions.at(i).front();
             float dist = sqrt(pow(x_median - pos.x, 2) + pow(y_median - pos.y, 2));
@@ -145,7 +148,7 @@ void Decision_making::callback(const fusion::FusionMsg::ConstPtr& msg)
             pos.top = top_median;
             positions.at(i).insert(positions.at(i).begin(), pos);
 
-            if(positions.at(i).size() > 7)
+            if(positions.at(i).size() > history_size)
                 positions.at(i).pop_back();
 
             if(distances.size() < i + 1)
@@ -168,7 +171,7 @@ void Decision_making::callback(const fusion::FusionMsg::ConstPtr& msg)
                     temp_boxes.front().pos.acc_distance = dist;
                     if(dist > 4000 && prev_dist < 4000 && write_csv)
                     {
-                        ros::Time time = msg->header.stamp;
+                        ros::Time time = event_msg.header.stamp;
                         ofstream storage(session_path + "/decision_making.csv" ,ios::out | ios::app );
                         storage
                             <<time<<"\t"
@@ -183,24 +186,15 @@ void Decision_making::callback(const fusion::FusionMsg::ConstPtr& msg)
                     temp_boxes.front().pos.acc_distance = dist;
                 *it = temp_boxes;
                 distances.at(i).insert(distances.at(i).begin(), dist_median);
-                if(distances.at(i).size() > 7)
+                if(distances.at(i).size() > history_size)
                     distances.at(i).pop_back();
             }
 
 
 
         }
     }
-
-
-
-
-
-
-
-
-
-
+
     vector<vector<float>>::iterator dist_it                    = distances.begin();
     vector<vector<Decision_making::Box>>::iterator boxes_it    = boxes_hist.begin();
     vector<vector<Decision_making::Position>>::iterator pos_it = positions.begin();
@@ -222,12 +216,7 @@ void Decision_making::callback(const fusion::FusionMsg::ConstPtr& msg)
 
         }
     }
-
-
-
-
-
-
+
     results_publisher.publish(event_msg);
 
     //~ for(vector<float> dist: distances)

fusion/config/parameters.yaml

@@ -10,4 +10,4 @@ max_depth       : 6000
 playback_topics : false
 create_directory: true
 write_csv       : true
-display         : true
+display         : false

fusion/include/fusion.hpp

@@ -41,7 +41,7 @@ class Fusion_processing
 		void callback(const sensor_msgs::Image::ConstPtr& chroma_msg, const sensor_msgs::Image::ConstPtr& chroma_dif_msg, const sensor_msgs::Image::ConstPtr& depth_msg);
 
 		void writeCSV(People& collection, string path, ros::Time time);
-		void publishResults(People& collection);
+		void publishResults(People& collection, ros::Time time);
 
 
 

fusion/src/fusion.cpp

@@ -186,14 +186,12 @@ void Fusion_processing::callback(const sensor_msgs::ImageConstPtr& chroma_msg, c
 		//~ cout<<"Distance:  "<<people.tracked_pos[index].distance<<endl;
 		//~ cout<<endl;
 
+
+		ros::Time time = cv_ptr->header.stamp;
 		if(write_csv)
-		{
-
-			ros::Time time = cv_ptr->header.stamp;
 			writeCSV(people, session_path, time);
-		}		
 
-		publishResults(people);
+		publishResults(people, time);
 
 
 	}
@@ -230,12 +228,12 @@ void Fusion_processing::writeCSV(People& collection, string path, ros::Time time
 	}
 }
 
-void Fusion_processing::publishResults(People& collection){
+void Fusion_processing::publishResults(People& collection, ros::Time time){
 	if (!collection.tracked_boxes.empty())
 	{
 		fusion::FusionMsg fmsg;
 
-		fmsg.header.stamp = ros::Time::now();
+		fmsg.header.stamp = time;
 		fmsg.header.frame_id = camera_frame;
 
 		for(int i = 0; i < collection.tracked_boxes.size() ; i++)