From 682acd3e04f8faa9c5bbe8bd3fe4be44280e1b02 Mon Sep 17 00:00:00 2001
From: SzymonParapura <szymon.parapura@robotec.ai>
Date: Tue, 10 Dec 2024 23:57:59 +0100
Subject: [PATCH] fix(simple_sensor_simulator): Fix if condition by adding
 negation to ensure proper logic

---
 .../detection_sensor/detection_sensor.hpp                | 9 +++------
 .../detection_sensor/detection_sensor.cpp                | 2 +-
 2 files changed, 4 insertions(+), 7 deletions(-)

diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp
index 67fb5bd3025..0aff66be710 100644
--- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp
+++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp
@@ -40,10 +40,7 @@ class DetectionSensorBase
   explicit DetectionSensorBase(
     const double current_simulation_time,
     const simulation_api_schema::DetectionSensorConfiguration & configuration)
-  : previous_simulation_time_(current_simulation_time),
-    configuration_(configuration),
-    ego_plane_opt_(std::nullopt),
-    ego_plane_pose_opt_(std::nullopt)
+  : previous_simulation_time_(current_simulation_time), configuration_(configuration)
   {
   }
 
@@ -66,8 +63,8 @@ class DetectionSensorBase
     const std::vector<std::string> & lidar_detected_entities) = 0;
 
 private:
-  std::optional<math::geometry::Plane> ego_plane_opt_;
-  std::optional<geometry_msgs::msg::Pose> ego_plane_pose_opt_;
+  std::optional<math::geometry::Plane> ego_plane_opt_{std::nullopt};
+  std::optional<geometry_msgs::msg::Pose> ego_plane_pose_opt_{std::nullopt};
 };
 
 template <typename T, typename U = autoware_perception_msgs::msg::TrackedObjects>
diff --git a/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp b/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp
index c5f180d70a2..cf5925688ff 100644
--- a/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp
+++ b/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp
@@ -97,7 +97,7 @@ auto DetectionSensorBase::isOnOrAboveEgoPlane(
     // update ego plane if needed
     geometry_msgs::msg::Pose ego_pose_ros;
     simulation_interface::toMsg(ego_pose, ego_pose_ros);
-    if (!ego_plane_opt_ || ego_plane_pose_opt_ || hasEgoOrientationChanged(ego_pose_ros)) {
+    if (!ego_plane_opt_ || !ego_plane_pose_opt_ || hasEgoOrientationChanged(ego_pose_ros)) {
       ego_plane_opt_.emplace(
         ego_pose_ros.position, math::geometry::getNormalVector(ego_pose_ros.orientation));
       ego_plane_pose_opt_ = ego_pose_ros;