Use single PC ptr

Signed-off-by: Daniel Sanchez <[email protected]>
Interplai · Apr 17, 2024 · b5091fc · b5091fc
1 parent 0be9601
commit b5091fc
Show file tree

Hide file tree

Showing 2 changed files with 5 additions and 7 deletions.
diff --git a/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp b/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp
@@ -165,7 +165,6 @@ class AEB : public rclcpp::Node
   void addCollisionMarker(const ObjectData & data, MarkerArray & debug_markers);
 
   PointCloud2::SharedPtr obstacle_ros_pointcloud_ptr_{nullptr};
-  PointCloud2::SharedPtr cropped_ros_pointcloud_ptr_{nullptr};
 
   VelocityReport::ConstSharedPtr current_velocity_ptr_{nullptr};
   Vector3::SharedPtr angular_velocity_ptr_{nullptr};

diff --git a/control/autonomous_emergency_braking/src/node.cpp b/control/autonomous_emergency_braking/src/node.cpp
@@ -242,7 +242,6 @@ void AEB::onPointCloud(const PointCloud2::ConstSharedPtr input_msg)
   filter.filter(*no_height_filtered_pointcloud_ptr);
 
   obstacle_ros_pointcloud_ptr_ = std::make_shared<PointCloud2>();
-  cropped_ros_pointcloud_ptr_ = std::make_shared<PointCloud2>();
 
   pcl::toROSMsg(*no_height_filtered_pointcloud_ptr, *obstacle_ros_pointcloud_ptr_);
   obstacle_ros_pointcloud_ptr_->header = input_msg->header;
@@ -386,8 +385,8 @@ bool AEB::checkCollision(MarkerArray & debug_markers)
     }
   }
 
-  if (cropped_ros_pointcloud_ptr_) {
-    pub_obstacle_pointcloud_->publish(*cropped_ros_pointcloud_ptr_);
+  if (obstacle_ros_pointcloud_ptr_) {
+    pub_obstacle_pointcloud_->publish(*obstacle_ros_pointcloud_ptr_);
   }
 
   return has_collision_ego || has_collision_predicted;
@@ -577,7 +576,7 @@ void AEB::cropPointCloudWithEgoFootprintPath(const std::vector<Polygon2d> & ego_
 
   // Store the results
   if (!objects->empty()) {
-    pcl::toROSMsg(*objects, *cropped_ros_pointcloud_ptr_);
+    pcl::toROSMsg(*objects, *obstacle_ros_pointcloud_ptr_);
   }
 }
 
@@ -586,12 +585,12 @@ void AEB::createObjectDataUsingPointCloudClusters(
   std::vector<ObjectData> & objects)
 {
   // check if the predicted path has valid number of points
-  if (ego_path.size() < 2 || ego_polys.empty() || cropped_ros_pointcloud_ptr_->data.empty()) {
+  if (ego_path.size() < 2 || ego_polys.empty() || obstacle_ros_pointcloud_ptr_->data.empty()) {
     return;
   }
 
   PointCloud::Ptr obstacle_points_ptr(new PointCloud);
-  pcl::fromROSMsg(*cropped_ros_pointcloud_ptr_, *obstacle_points_ptr);
+  pcl::fromROSMsg(*obstacle_ros_pointcloud_ptr_, *obstacle_points_ptr);
   if (obstacle_points_ptr->empty()) return;
 
   std::vector<pcl::PointIndices> cluster_indices;