From 152984bb24e7ebdc87377b328295019484cbdea0 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Iv=C3=A1n=20del=20Pino?= <idelpino@iri.upc.edu>
Date: Tue, 12 Jul 2022 10:58:00 +0200
Subject: [PATCH] bug fixed in time supervision --> removed ifs that make the
object to be destroyed after the if sentence
---
src/kf_based_terrain_analysis.cpp | 23 +++++++++++------------
src/time_supervisor.cpp | 4 ++--
2 files changed, 13 insertions(+), 14 deletions(-)
diff --git a/src/kf_based_terrain_analysis.cpp b/src/kf_based_terrain_analysis.cpp
index 699be79..59bb8ea 100644
--- a/src/kf_based_terrain_analysis.cpp
+++ b/src/kf_based_terrain_analysis.cpp
@@ -21,8 +21,8 @@ void CKf_Based_Terrain_Analysis::extractPointsInROI(
const pcl::PointXYZRGBNormal reference, const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr elevation_cloud_ptr,
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr points_in_ROI)
{
- if (filtering_configuration.measure_performance)
- CFunctionMonitor performance_monitor("extractPointsInROI", performance_supervisor_ptr_);
+// if (filtering_configuration.measure_performance)
+// CFunctionMonitor performance_monitor("extractPointsInROI", performance_supervisor_ptr_);
//std::cout << "Extracting points in ROI: reference.x = " << reference.x << " reference.y = " << reference.y
// << " reference.z = " << reference.z << std::endl;
@@ -307,8 +307,8 @@ void CKf_Based_Terrain_Analysis::labelLocalGroundPoints(
std::vector<std::vector<kf_based_terrain_analysis_lib::Edge>> &discarded_edges)
{
//std::cout << "Starting labelLocalGroundPoints!!" << std::endl;
- if (filtering_configuration.measure_performance)
- CFunctionMonitor performance_monitor("labelLocalGroundPoints", performance_supervisor_ptr_);
+// if (filtering_configuration.measure_performance)
+// CFunctionMonitor performance_monitor("labelLocalGroundPoints", performance_supervisor_ptr_);
CObservationDownsampler new_references_creator(filtering_configuration.ground_reference_search_resolution,
filtering_configuration.propagation_additive_noise,
@@ -395,7 +395,7 @@ void CKf_Based_Terrain_Analysis::labelLocalGroundPoints(
// << " z = " << point_in_sensor_frame.z << std::endl << "mahalanobis_distance = " << mahalanobis_distance
// << " mahalanobis_threshold = " << filtering_configuration.mahalanobis_threshold << std::endl;
- if ((int)std::floor(point_in_sensor_frame.data_c[DATA_C_1_ID_CLASS]) == CLASS_UNKNOWN) // we only store de data if it has not been done
+ if ((int)std::floor(point_in_sensor_frame.data_c[DATA_C_1_ID_CLASS]) == CLASS_UNKNOWN) // we only store the data if it has not been done
// previously
{
// if it is an obstacle point we use the ground reference z prediction as z ground
@@ -559,7 +559,7 @@ void CKf_Based_Terrain_Analysis::labelGroundPoints(
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_ptr, std::vector<std::vector<int>> &edges,
std::vector<std::vector<kf_based_terrain_analysis_lib::Edge>> &discarded_edges)
{
- if (filtering_configuration.measure_performance)
+ //if (filtering_configuration.measure_performance)
CFunctionMonitor performance_monitor("labelGroundPoints", performance_supervisor_ptr_);
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr points_in_ROI(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
@@ -592,7 +592,7 @@ void CKf_Based_Terrain_Analysis::generateElevationCloud(
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pcl_cloud_ptr,
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr elevation_cloud, std::vector<std::vector<int>> &correspondence_indexes)
{
- if (filtering_configuration.measure_performance)
+ //if (filtering_configuration.measure_performance)
CFunctionMonitor performance_monitor("generateElevationCloud", performance_supervisor_ptr_);
pcl::PointCloud<pcl::PointXY>::Ptr pcl_pointcloud_2D_ptr(new pcl::PointCloud<pcl::PointXY>);
@@ -738,7 +738,7 @@ void CKf_Based_Terrain_Analysis::createDenseGroundCloud(
const kf_based_terrain_analysis_lib::SensorConfiguration lidar_configuration,
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pcl_dense_cloud_ptr)
{
- if (filtering_configuration.measure_performance)
+ //if (filtering_configuration.measure_performance)
CFunctionMonitor performance_monitor("createDenseGroundCloud", performance_supervisor_ptr_);
//std::cout << "setting input cloud" << std::endl;
@@ -838,7 +838,7 @@ void CKf_Based_Terrain_Analysis::labelPointcloudUsingGroundModel(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr ground_reference_cloud_ptr,
const std::vector<std::vector<int>> &edges, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pcl_cloud_ptr)
{
- if (filtering_configuration.measure_performance)
+ //if (filtering_configuration.measure_performance)
CFunctionMonitor performance_monitor("labelPointcloudUsingGroundModel", performance_supervisor_ptr_);
//std::cout << "Labeling points in the original pointcloud using the posterior and the ROI indexes vectors..."
// << std::endl;
@@ -856,7 +856,6 @@ void CKf_Based_Terrain_Analysis::labelPointcloudUsingGroundModel(
int num_of_references_found = kdtree.nearestKSearch(point_in_sensor_frame, K, pointIdxKNNSearch,
pointKNNSquaredDistance);
-
int best_reference_index = -1;
int best_reference_index_predicting_ground = -1;
int best_reference_index_predicting_obstacle = -1;
@@ -1172,8 +1171,8 @@ void CKf_Based_Terrain_Analysis::groundSegmentation(
std::vector<std::vector<kf_based_terrain_analysis_lib::Edge>> &discarded_edges,
pcl::PointCloud<pcl::PointXYZRGBNormal> &ground_dense_reconstruction_pcl_cloud)
{
- if (filtering_configuration.measure_performance)
- CFunctionMonitor performance_monitor("groundSegmentation", performance_supervisor_ptr_);
+// if (filtering_configuration.measure_performance)
+// CFunctionMonitor performance_monitor("groundSegmentation", performance_supervisor_ptr_);
//std::cout << "Starting ground segmentation!!" << std::endl;
//std::cout << "Initializing pointcloud: Number of points in pcl_cloud = " << pcl_cloud.points.size() << std::endl;
diff --git a/src/time_supervisor.cpp b/src/time_supervisor.cpp
index 8fba8e8..a07810a 100644
--- a/src/time_supervisor.cpp
+++ b/src/time_supervisor.cpp
@@ -121,14 +121,14 @@ CFunctionMonitor::CFunctionMonitor(const std::string function_name, const CTimeS
{
my_supervisor_ = time_supervisor_ptr;
function_name_ = function_name;
- initial_time_ = ros::Time::now().toSec();
+ initial_time_ = ros::WallTime::now().toSec();
final_time_ = 0.0;
time_expended_ = 0.0;
}
CFunctionMonitor::~CFunctionMonitor()
{
- final_time_ = ros::Time::now().toSec();
+ final_time_ = ros::WallTime::now().toSec();
time_expended_ = final_time_ - initial_time_;
//assert(time_expended_ >= 0.0 && "Error in CFunctionMonitor: time negative!!");
if(time_expended_ < 0.0)
--
GitLab