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 3917feae62a..eb477a9d661 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 @@ -41,22 +41,12 @@ class DetectionSensorBase { } - auto isWithinRange( - const geometry_msgs::Point & point1, const geometry_msgs::Point & point2, - const double range) const -> bool; + auto isEgoEntityStatusToWhichThisSensorIsAttached( + const traffic_simulator_msgs::EntityStatus &) const -> bool; - auto filterObjectsBySensorRange( - const std::vector &, const std::vector &, - const double) const -> std::vector; - - auto getEntityPose(const std::vector &, const std::string &) - const -> geometry_msgs::Pose; - - auto getDetectedObjects(const std::vector &) const - -> std::vector; - - auto getSensorPose(const std::vector &) const - -> geometry_msgs::Pose; + auto findEgoEntityStatusToWhichThisSensorIsAttached( + const std::vector &) const + -> std::vector::const_iterator; public: virtual ~DetectionSensorBase() = default; @@ -67,27 +57,30 @@ class DetectionSensorBase const std::vector & lidar_detected_entities) = 0; }; -template +template class DetectionSensor : public DetectionSensorBase { - const typename rclcpp::Publisher::SharedPtr publisher_ptr_; + const typename rclcpp::Publisher::SharedPtr detected_objects_publisher; + + const typename rclcpp::Publisher::SharedPtr ground_truth_objects_publisher; - typename rclcpp::PublisherBase::SharedPtr ground_truth_publisher_base_ptr_; + std::default_random_engine random_engine_; - std::mt19937 random_engine_; + std::queue> + detected_objects_queue; - auto applyPositionNoise(typename T::_objects_type::value_type) -> - typename T::_objects_type::value_type; + std::queue> + ground_truth_objects_queue; public: explicit DetectionSensor( const double current_simulation_time, const simulation_api_schema::DetectionSensorConfiguration & configuration, const typename rclcpp::Publisher::SharedPtr & publisher, - const typename rclcpp::PublisherBase::SharedPtr & ground_truth_publisher = nullptr) + const typename rclcpp::Publisher::SharedPtr & ground_truth_publisher = nullptr) : DetectionSensorBase(current_simulation_time, configuration), - publisher_ptr_(publisher), - ground_truth_publisher_base_ptr_(ground_truth_publisher), + detected_objects_publisher(publisher), + ground_truth_objects_publisher(ground_truth_publisher), random_engine_(configuration.random_seed()) { } 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 de0152ff850..b7f32b6b57b 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 @@ -23,6 +23,7 @@ #include #include #include +#include #include #include #include @@ -31,108 +32,272 @@ namespace simple_sensor_simulator { -auto DetectionSensorBase::isWithinRange( - const geometry_msgs::Point & point1, const geometry_msgs::Point & point2, - const double range) const -> bool +auto distance(const geometry_msgs::Pose & pose1, const geometry_msgs::Pose & pose2) { - auto distanceX = point1.x() - point2.x(); - auto distanceY = point1.y() - point2.y(); - auto distanceZ = point1.z() - point2.z(); + return std::hypot( + pose1.position().x() - pose2.position().x(), // + pose1.position().y() - pose2.position().y(), // + pose1.position().z() - pose2.position().z()); +} - double distance = std::hypot(distanceX, distanceY, distanceZ); - return distance <= range; +auto DetectionSensorBase::isEgoEntityStatusToWhichThisSensorIsAttached( + const traffic_simulator_msgs::EntityStatus & status) const -> bool +{ + return status.name() == configuration_.entity() and + status.type().type() == traffic_simulator_msgs::EntityType::EGO; } -auto DetectionSensorBase::getSensorPose( - const std::vector & statuses) const -> geometry_msgs::Pose +auto DetectionSensorBase::findEgoEntityStatusToWhichThisSensorIsAttached( + const std::vector & statuses) const + -> std::vector::const_iterator { - for (const auto & status : statuses) { - if ( - status.type().type() == traffic_simulator_msgs::EntityType::EGO && - status.name() == configuration_.entity()) { - return status.pose(); - } + if (auto iter = std::find_if( + statuses.begin(), statuses.end(), + [this](const auto & status) { + return isEgoEntityStatusToWhichThisSensorIsAttached(status); + }); + iter != statuses.end()) { + return iter; + } else { + throw SimulationRuntimeError("Detection sensor can be attached only ego entity."); } - throw SimulationRuntimeError("Detection sensor can be attached only ego entity."); } -auto DetectionSensorBase::getEntityPose( - const std::vector & entity_statuses, - const std::string & entity_string) const -> geometry_msgs::Pose +template +auto make(From &&...) -> To; + +template <> +auto make(const traffic_simulator_msgs::EntityStatus & status) -> unique_identifier_msgs::msg::UUID +{ + static auto generate_uuid = boost::uuids::name_generator(boost::uuids::random_generator()()); + const auto uuid = generate_uuid(status.name()); + unique_identifier_msgs::msg::UUID message; + std::copy(uuid.begin(), uuid.end(), message.uuid.begin()); + return message; +} + +template <> +auto make(const traffic_simulator_msgs::EntityStatus & status) + -> autoware_auto_perception_msgs::msg::ObjectClassification { - for (const auto & entity_status : entity_statuses) { - if (entity_status.name() == entity_string) { - return entity_status.pose(); + auto object_classification = autoware_auto_perception_msgs::msg::ObjectClassification(); + + object_classification.label = [&]() { + switch (status.subtype().value()) { + case traffic_simulator_msgs::EntitySubtype::CAR: + return autoware_auto_perception_msgs::msg::ObjectClassification::CAR; + case traffic_simulator_msgs::EntitySubtype::TRUCK: + return autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK; + case traffic_simulator_msgs::EntitySubtype::BUS: + return autoware_auto_perception_msgs::msg::ObjectClassification::BUS; + case traffic_simulator_msgs::EntitySubtype::TRAILER: + return autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER; + case traffic_simulator_msgs::EntitySubtype::MOTORCYCLE: + return autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE; + case traffic_simulator_msgs::EntitySubtype::BICYCLE: + return autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE; + case traffic_simulator_msgs::EntitySubtype::PEDESTRIAN: + return autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN; + default: + return autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN; } - } + }(); - throw SimulationRuntimeError( - configuration_.detect_all_objects_in_range() - ? "Filtered object is not includes in entity statuses" - : "Detected object by lidar sensor is not included in lidar detected entity"); + object_classification.probability = 1; + + return object_classification; } -auto DetectionSensorBase::getDetectedObjects( - const std::vector & statuses) const - -> std::vector +template <> +auto make(const traffic_simulator_msgs::EntityStatus & status) -> geometry_msgs::msg::Pose { - std::vector detected_objects; - const auto pose = getSensorPose(statuses); - - for (const auto & status : statuses) { - if ( - status.name() != configuration_.entity() && - isWithinRange(status.pose().position(), pose.position(), 300.0)) { - detected_objects.emplace_back(status.name()); - } - } + auto pose = geometry_msgs::msg::Pose(); + simulation_interface::toMsg(status.pose(), pose); + + auto center_point = geometry_msgs::msg::Point(); + simulation_interface::toMsg(status.bounding_box().center(), center_point); - return detected_objects; + Eigen::Vector3d center = quaternion_operation::getRotationMatrix(pose.orientation) * + Eigen::Vector3d(center_point.x, center_point.y, center_point.z); + + pose.position.x = pose.position.x + center.x(); + pose.position.y = pose.position.y + center.y(); + pose.position.z = pose.position.z + center.z(); + + return pose; } -auto DetectionSensorBase::filterObjectsBySensorRange( - const std::vector & entity_statuses, - const std::vector & selected_entity_strings, - const double detection_sensor_range) const -> std::vector +template <> +auto make(const traffic_simulator_msgs::EntityStatus & status) -> geometry_msgs::msg::Twist { - std::vector detected_objects; - const auto sensor_pose = getSensorPose(entity_statuses); - - for (const auto & selected_entity_status : selected_entity_strings) { - const auto selected_entity_pose = getEntityPose(entity_statuses, selected_entity_status); - if ( - selected_entity_status != configuration_.entity() && - isWithinRange( - selected_entity_pose.position(), sensor_pose.position(), detection_sensor_range)) { - detected_objects.emplace_back(selected_entity_status); + auto twist = geometry_msgs::msg::Twist(); + simulation_interface::toMsg(status.action_status().twist(), twist); + return twist; +} + +template <> +auto make(const traffic_simulator_msgs::EntityStatus & status) + -> autoware_auto_perception_msgs::msg::DetectedObjectKinematics +{ + auto kinematics = autoware_auto_perception_msgs::msg::DetectedObjectKinematics(); + + kinematics.pose_with_covariance.pose = make(status); + + // clang-format off + kinematics.pose_with_covariance.covariance = { + /* + Row-major representation of the 6x6 covariance matrix. The orientation + parameters use a fixed-axis representation. In order, the parameters + are: (x, y, z, rotation about X axis, rotation about Y axis, rotation + about Z axis) + */ + 1, 0, 0, 0, 0, 0, + 0, 1, 0, 0, 0, 0, + 0, 0, 1, 0, 0, 0, + 0, 0, 0, 1, 0, 0, + 0, 0, 0, 0, 1, 0, + 0, 0, 0, 0, 0, 1, + }; + // clang-format on + + kinematics.twist_with_covariance.twist = make(status); + + kinematics.orientation_availability = [&]() { + switch (status.subtype().value()) { + case traffic_simulator_msgs::EntitySubtype::BICYCLE: + case traffic_simulator_msgs::EntitySubtype::MOTORCYCLE: + return autoware_auto_perception_msgs::msg::DetectedObjectKinematics::SIGN_UNKNOWN; + default: + return autoware_auto_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; } - } - return detected_objects; + }(); + + return kinematics; +} + +template <> +auto make(const traffic_simulator_msgs::EntityStatus & status) + -> autoware_auto_perception_msgs::msg::Shape +{ + auto shape = autoware_auto_perception_msgs::msg::Shape(); + simulation_interface::toMsg(status.bounding_box().dimensions(), shape.dimensions); + shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + return shape; } template <> -auto DetectionSensor::applyPositionNoise( - autoware_auto_perception_msgs::msg::DetectedObject detected_object) +auto make(const traffic_simulator_msgs::EntityStatus & status) -> autoware_auto_perception_msgs::msg::DetectedObject { - auto position_noise_distribution = - std::normal_distribution<>(0.0, configuration_.pos_noise_stddev()); - detected_object.kinematics.pose_with_covariance.pose.position.x += - position_noise_distribution(random_engine_); - detected_object.kinematics.pose_with_covariance.pose.position.y += - position_noise_distribution(random_engine_); + auto detected_object = autoware_auto_perception_msgs::msg::DetectedObject(); + detected_object.classification.push_back( + make(status)); + detected_object.kinematics = + make(status); + detected_object.shape = make(status); return detected_object; } -unique_identifier_msgs::msg::UUID generateUUIDMsg(const std::string & input) +template <> +auto make( + const traffic_simulator_msgs::EntityStatus & status, + const autoware_auto_perception_msgs::msg::DetectedObject & detected_object) + -> autoware_auto_perception_msgs::msg::TrackedObject { - static auto generate_uuid = boost::uuids::name_generator(boost::uuids::random_generator()()); - const auto uuid = generate_uuid(input); + // ref: https://github.com/autowarefoundation/autoware.universe/blob/main/common/perception_utils/src/conversion.cpp + auto tracked_object = autoware_auto_perception_msgs::msg::TrackedObject(); + // clang-format off + tracked_object.object_id = make(status); + tracked_object.existence_probability = detected_object.existence_probability; + tracked_object.classification = detected_object.classification; + tracked_object.kinematics.orientation_availability = detected_object.kinematics.orientation_availability; + tracked_object.kinematics.pose_with_covariance = detected_object.kinematics.pose_with_covariance; + tracked_object.kinematics.twist_with_covariance = detected_object.kinematics.twist_with_covariance; + tracked_object.shape = detected_object.shape; + // clang-format on + return tracked_object; +}; + +struct DefaultNoiseApplicator +{ + const double current_simulation_time; - unique_identifier_msgs::msg::UUID uuid_msg; - std::copy(uuid.begin(), uuid.end(), uuid_msg.uuid.begin()); - return uuid_msg; -} + const rclcpp::Time & current_ros_time; + + const traffic_simulator_msgs::EntityStatus & ego_entity_status; + + std::default_random_engine & random_engine; + + const simulation_api_schema::DetectionSensorConfiguration & detection_sensor_configuration; + + explicit DefaultNoiseApplicator( + double current_simulation_time, const rclcpp::Time & current_ros_time, + const traffic_simulator_msgs::EntityStatus & ego_entity_status, + std::default_random_engine & random_engine, + const simulation_api_schema::DetectionSensorConfiguration & detection_sensor_configuration) + : current_simulation_time(current_simulation_time), + current_ros_time(current_ros_time), + ego_entity_status(ego_entity_status), + random_engine(random_engine), + detection_sensor_configuration(detection_sensor_configuration) + { + } + + DefaultNoiseApplicator(const DefaultNoiseApplicator &) = delete; + + DefaultNoiseApplicator(DefaultNoiseApplicator &&) = delete; + + auto operator=(const DefaultNoiseApplicator &) = delete; + + auto operator=(DefaultNoiseApplicator &&) = delete; + + auto operator()(autoware_auto_perception_msgs::msg::DetectedObjects detected_objects) + -> decltype(auto) + { + auto position_noise_distribution = + std::normal_distribution<>(0.0, detection_sensor_configuration.pos_noise_stddev()); + + for (auto && detected_object : detected_objects.objects) { + detected_object.kinematics.pose_with_covariance.pose.position.x += + position_noise_distribution(random_engine); + detected_object.kinematics.pose_with_covariance.pose.position.y += + position_noise_distribution(random_engine); + } + + detected_objects.objects.erase( + std::remove_if( + detected_objects.objects.begin(), detected_objects.objects.end(), + [this](auto &&) { + return std::uniform_real_distribution()(random_engine) < + detection_sensor_configuration.probability_of_lost(); + }), + detected_objects.objects.end()); + + return detected_objects; + } +}; + +struct CustomNoiseApplicator : public DefaultNoiseApplicator +{ + using DefaultNoiseApplicator::DefaultNoiseApplicator; + + /* + NOTE: for Autoware developers + + If you need to apply experimental noise to the DetectedObjects that the + simulator publishes, comment out the following member functions and + implement them. + + See class DefaultNoiseApplicator for the default noise implementation. + This class inherits from DefaultNoiseApplicator, so you can use its data + members, or you can explicitly call DefaultNoiseApplicator::operator(). + */ + // auto operator()(autoware_auto_perception_msgs::msg::DetectedObjects detected_objects) + // -> decltype(auto) + // { + // return detected_objects; + // } +}; template <> auto DetectionSensor::update( @@ -141,173 +306,55 @@ auto DetectionSensor::updat const rclcpp::Time & current_ros_time, const std::vector & lidar_detected_entities) -> void { - auto makeObjectClassification = [](const auto & label) { - autoware_auto_perception_msgs::msg::ObjectClassification object_classification; - object_classification.label = label; - object_classification.probability = 1; - return object_classification; - }; - if ( current_simulation_time - previous_simulation_time_ - configuration_.update_duration() >= -0.002) { - const std::vector detected_objects = filterObjectsBySensorRange( - statuses, - configuration_.detect_all_objects_in_range() ? getDetectedObjects(statuses) - : lidar_detected_entities, - configuration_.range()); - - autoware_auto_perception_msgs::msg::DetectedObjects msg; - msg.header.stamp = current_ros_time; - msg.header.frame_id = "map"; - - autoware_auto_perception_msgs::msg::TrackedObjects ground_truth_msg; - ground_truth_msg.header = msg.header; - previous_simulation_time_ = current_simulation_time; - for (const auto & status : statuses) { - if ( - std::find(detected_objects.begin(), detected_objects.end(), status.name()) != - detected_objects.end() and - status.type().type() != traffic_simulator_msgs::EntityType_Enum::EntityType_Enum_EGO) { - autoware_auto_perception_msgs::msg::DetectedObject object; - switch (status.subtype().value()) { - case traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_UNKNOWN: - object.classification.push_back(makeObjectClassification( - autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN)); - break; - case traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_CAR: - object.classification.push_back(makeObjectClassification( - autoware_auto_perception_msgs::msg::ObjectClassification::CAR)); - break; - case traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_TRUCK: - object.classification.push_back(makeObjectClassification( - autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK)); - break; - case traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_BUS: - object.classification.push_back(makeObjectClassification( - autoware_auto_perception_msgs::msg::ObjectClassification::BUS)); - break; - case traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_TRAILER: - object.classification.push_back(makeObjectClassification( - autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER)); - break; - case traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_MOTORCYCLE: - object.classification.push_back(makeObjectClassification( - autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE)); - object.kinematics.orientation_availability = - autoware_auto_perception_msgs::msg::DetectedObjectKinematics::SIGN_UNKNOWN; - break; - case traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_BICYCLE: - object.classification.push_back(makeObjectClassification( - autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE)); - object.kinematics.orientation_availability = - autoware_auto_perception_msgs::msg::DetectedObjectKinematics::SIGN_UNKNOWN; - break; - case traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_PEDESTRIAN: - object.classification.push_back(makeObjectClassification( - autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN)); - break; - default: - object.classification.push_back(makeObjectClassification( - autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN)); - break; - } - - simulation_interface::toMsg(status.bounding_box().dimensions(), object.shape.dimensions); - geometry_msgs::msg::Pose pose; - simulation_interface::toMsg(status.pose(), pose); - auto rotation = quaternion_operation::getRotationMatrix(pose.orientation); - geometry_msgs::msg::Point center_point; - simulation_interface::toMsg(status.bounding_box().center(), center_point); - Eigen::Vector3d center(center_point.x, center_point.y, center_point.z); - center = rotation * center; - pose.position.x = pose.position.x + center.x(); - pose.position.y = pose.position.y + center.y(); - pose.position.z = pose.position.z + center.z(); - object.kinematics.pose_with_covariance.pose = pose; - object.kinematics.pose_with_covariance.covariance = {1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, - 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, - 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1}; - simulation_interface::toMsg( - status.action_status().twist(), object.kinematics.twist_with_covariance.twist); - object.shape.type = object.shape.BOUNDING_BOX; - - msg.objects.push_back(object); - - // ref: https://github.com/autowarefoundation/autoware.universe/blob/main/common/perception_utils/src/conversion.cpp - static auto toTrackedObject = - [&]( - const std::string & name, - const autoware_auto_perception_msgs::msg::DetectedObject & detected_object) - -> autoware_auto_perception_msgs::msg::TrackedObject { - autoware_auto_perception_msgs::msg::TrackedObject tracked_object; - tracked_object.existence_probability = detected_object.existence_probability; - - tracked_object.classification = detected_object.classification; - - tracked_object.kinematics.pose_with_covariance = - detected_object.kinematics.pose_with_covariance; - tracked_object.kinematics.twist_with_covariance = - detected_object.kinematics.twist_with_covariance; - tracked_object.kinematics.orientation_availability = - detected_object.kinematics.orientation_availability; - - tracked_object.shape = detected_object.shape; - tracked_object.object_id = generateUUIDMsg(name); - - return tracked_object; - }; - - ground_truth_msg.objects.push_back(toTrackedObject(status.name(), object)); - } - } + autoware_auto_perception_msgs::msg::DetectedObjects detected_objects; + detected_objects.header.stamp = current_ros_time; + detected_objects.header.frame_id = "map"; - static std::queue> - queue_objects; - static std::queue> - queue_ground_truth_objects; + autoware_auto_perception_msgs::msg::TrackedObjects ground_truth_objects; + ground_truth_objects.header = detected_objects.header; - queue_objects.push(std::make_pair(msg, current_simulation_time)); - queue_ground_truth_objects.push(std::make_pair(ground_truth_msg, current_simulation_time)); + const auto ego_entity_status = findEgoEntityStatusToWhichThisSensorIsAttached(statuses); - static rclcpp::Publisher::SharedPtr - ground_truth_publisher = std::dynamic_pointer_cast< - rclcpp::Publisher>( - ground_truth_publisher_base_ptr_); + auto is_in_range = [&](const auto & status) { + return not isEgoEntityStatusToWhichThisSensorIsAttached(status) and + distance(status.pose(), ego_entity_status->pose()) <= configuration_.range() and + (configuration_.detect_all_objects_in_range() or + std::find( + lidar_detected_entities.begin(), lidar_detected_entities.end(), status.name()) != + lidar_detected_entities.end()); + }; - autoware_auto_perception_msgs::msg::DetectedObjects delayed_msg; - autoware_auto_perception_msgs::msg::TrackedObjects delayed_ground_truth_msg; - - if ( - current_simulation_time - queue_objects.front().second >= - configuration_.object_recognition_delay()) { - delayed_msg = queue_objects.front().first; - delayed_ground_truth_msg = queue_ground_truth_objects.front().first; - queue_objects.pop(); + for (const auto & status : statuses) { + if (is_in_range(status)) { + const auto detected_object = + make(status); + detected_objects.objects.push_back(detected_object); + ground_truth_objects.objects.push_back( + make(status, detected_object)); + } } - if ( - current_simulation_time - queue_ground_truth_objects.front().second >= - configuration_.object_recognition_ground_truth_delay()) { - delayed_ground_truth_msg = queue_ground_truth_objects.front().first; - queue_ground_truth_objects.pop(); + if (detected_objects_queue.emplace(detected_objects, current_simulation_time); + current_simulation_time - detected_objects_queue.front().second >= + configuration_.object_recognition_delay()) { + auto apply_noise = CustomNoiseApplicator( + current_simulation_time, current_ros_time, *ego_entity_status, random_engine_, + configuration_); + detected_objects_publisher->publish(apply_noise(detected_objects_queue.front().first)); + detected_objects_queue.pop(); } - autoware_auto_perception_msgs::msg::DetectedObjects noised_msg; - noised_msg.header = delayed_msg.header; - noised_msg.objects.reserve(delayed_msg.objects.size()); - for (const auto & object : delayed_msg.objects) { - if (auto probability_of_lost = std::uniform_real_distribution(); - probability_of_lost(random_engine_) > configuration_.probability_of_lost()) { - noised_msg.objects.push_back(applyPositionNoise(object)); - } + if (ground_truth_objects_queue.emplace(ground_truth_objects, current_simulation_time); + current_simulation_time - ground_truth_objects_queue.front().second >= + configuration_.object_recognition_ground_truth_delay()) { + ground_truth_objects_publisher->publish(ground_truth_objects_queue.front().first); + ground_truth_objects_queue.pop(); } - - publisher_ptr_->publish(noised_msg); - - ground_truth_publisher->publish(delayed_ground_truth_msg); } } } // namespace simple_sensor_simulator