15 #ifndef SIMPLE_SENSOR_SIMULATOR__SENSOR_SIMULATION__DETECTION_SENSOR__DETECTION_SENSOR_HPP_ 
   16 #define SIMPLE_SENSOR_SIMULATOR__SENSOR_SIMULATION__DETECTION_SENSOR__DETECTION_SENSOR_HPP_ 
   18 #include <simulation_interface/simulation_api_schema.pb.h> 
   20 #include <autoware_perception_msgs/msg/tracked_objects.hpp> 
   22 #include <geometry_msgs/msg/pose.hpp> 
   23 #include <get_parameter/get_parameter.hpp> 
   28 #include <rclcpp/rclcpp.hpp> 
   31 #include <unordered_map> 
   45     const double current_simulation_time,
 
   46     const simulation_api_schema::DetectionSensorConfiguration & configuration)
 
   55     const std::vector<traffic_simulator_msgs::EntityStatus> &) 
const 
   56     -> std::vector<traffic_simulator_msgs::EntityStatus>::const_iterator;
 
   65     const double current_simulation_time, 
const std::vector<traffic_simulator_msgs::EntityStatus> &,
 
   66     const rclcpp::Time & current_ros_time,
 
   67     const std::vector<std::string> & lidar_detected_entities) = 0;
 
   70   std::optional<math::geometry::Plane> ego_plane_opt_{std::nullopt};
 
   71   std::optional<geometry_msgs::msg::Pose> ego_plane_pose_opt_{std::nullopt};
 
   74 template <
typename T, 
typename U = autoware_perception_msgs::msg::TrackedObjects>
 
   77   const typename rclcpp::Publisher<T>::SharedPtr detected_objects_publisher;
 
   79   const typename rclcpp::Publisher<U>::SharedPtr ground_truth_objects_publisher;
 
   81   int noise_model_version;
 
   83   std::default_random_engine random_engine_;
 
   85   std::queue<std::pair<std::vector<traffic_simulator_msgs::EntityStatus>, 
double>>
 
   86     unpublished_detected_entities, unpublished_ground_truth_entities;
 
   90     double simulation_time;
 
   93     double distance_noise, yaw_noise;
 
   94     bool true_positive, flip;
 
   97     double v4_position_x_noise, v4_position_y_noise;
 
  101     explicit NoiseOutput(
double simulation_time = 0.0)
 
  102     : simulation_time(simulation_time),
 
  107       v4_position_x_noise(0.0),
 
  108       v4_position_y_noise(0.0),
 
  114   std::unordered_map<std::string, NoiseOutput> noise_outputs;
 
  116   template <
typename Message, 
typename std::enable_if_t<std::is_same_v<Message, T>, 
int> = 0>
 
  119     static const auto override_legacy_configuration = common::getParameter<bool>(
 
  120       detected_objects_publisher->get_topic_name() + 
std::string(
".override_legacy_configuration"));
 
  121     if (override_legacy_configuration) {
 
  122       static const auto delay = common::getParameter<double>(
 
  123         detected_objects_publisher->get_topic_name() + 
std::string(
".delay"));
 
  130   template <
typename Message, 
typename std::enable_if_t<std::is_same_v<Message, U>, 
int> = 0>
 
  133     static const auto override_legacy_configuration = common::getParameter<bool>(
 
  134       ground_truth_objects_publisher->get_topic_name() +
 
  136     if (override_legacy_configuration) {
 
  137       static const auto delay = common::getParameter<double>(
 
  138         ground_truth_objects_publisher->get_topic_name() + 
std::string(
".delay"));
 
  147     static const auto override_legacy_configuration = common::getParameter<bool>(
 
  148       detected_objects_publisher->get_topic_name() + 
std::string(
".override_legacy_configuration"));
 
  149     if (override_legacy_configuration) {
 
  150       static const auto range = common::getParameter<double>(
 
  151         detected_objects_publisher->get_topic_name() + 
std::string(
".range"), 300.0);
 
  158   auto detect_all_objects_in_range()
 const 
  161     static const auto override_legacy_configuration = common::getParameter<bool>(
 
  162       detected_objects_publisher->get_topic_name() + 
std::string(
".override_legacy_configuration"));
 
  163     if (override_legacy_configuration) {
 
  164       static const auto occlusionless = common::getParameter<bool>(
 
  165         detected_objects_publisher->get_topic_name() + 
std::string(
".occlusionless"));
 
  166       return occlusionless;
 
  174     const double current_simulation_time,
 
  175     const simulation_api_schema::DetectionSensorConfiguration & configuration,
 
  176     const typename rclcpp::Publisher<T>::SharedPtr & publisher,
 
  177     const typename rclcpp::Publisher<U>::SharedPtr & ground_truth_publisher = 
nullptr)
 
  179     detected_objects_publisher(publisher),
 
  180     ground_truth_objects_publisher(ground_truth_publisher),
 
  181     noise_model_version(
common::getParameter<int>(
 
  182       detected_objects_publisher->get_topic_name() + 
std::
string(
".noise.model.version"))),
 
  183     random_engine_([this]() {
 
  184       if (
const auto seed = [
this]() -> std::random_device::result_type {
 
  185             switch (noise_model_version) {
 
  193                 return common::getParameter<int>(
 
  194                   detected_objects_publisher->get_topic_name() + 
std::string(
".seed"));
 
  198         if (std::random_device::min() <= seed and seed <= std::random_device::max()) {
 
  202             "The value of parameter ",
 
  203             std::quoted(detected_objects_publisher->get_topic_name() + 
std::string(
".seed")),
 
  204             " must be greater than or equal to ", std::random_device::min(),
 
  205             " and less than or equal to ", std::random_device::max());
 
  208         return std::random_device()();
 
  217     const double, 
const std::vector<traffic_simulator_msgs::EntityStatus> &, 
const rclcpp::Time &,
 
  218     const std::vector<std::string> & lidar_detected_entities) -> 
void override;
 
Definition: detection_sensor.hpp:38
auto isOnOrAboveEgoPlane(const geometry_msgs::Pose &npc_pose, const geometry_msgs::Pose &ego_pose) -> bool
Definition: detection_sensor.cpp:75
DetectionSensorBase(const double current_simulation_time, const simulation_api_schema::DetectionSensorConfiguration &configuration)
Definition: detection_sensor.hpp:44
virtual ~DetectionSensorBase()=default
simulation_api_schema::DetectionSensorConfiguration configuration_
Definition: detection_sensor.hpp:42
auto isEgoEntityStatusToWhichThisSensorIsAttached(const traffic_simulator_msgs::EntityStatus &) const -> bool
Definition: detection_sensor.cpp:52
double previous_simulation_time_
Definition: detection_sensor.hpp:40
virtual void update(const double current_simulation_time, const std::vector< traffic_simulator_msgs::EntityStatus > &, const rclcpp::Time ¤t_ros_time, const std::vector< std::string > &lidar_detected_entities)=0
auto findEgoEntityStatusToWhichThisSensorIsAttached(const std::vector< traffic_simulator_msgs::EntityStatus > &) const -> std::vector< traffic_simulator_msgs::EntityStatus >::const_iterator
Definition: detection_sensor.cpp:59
Definition: detection_sensor.hpp:76
DetectionSensor(const double current_simulation_time, const simulation_api_schema::DetectionSensorConfiguration &configuration, const typename rclcpp::Publisher< T >::SharedPtr &publisher, const typename rclcpp::Publisher< U >::SharedPtr &ground_truth_publisher=nullptr)
Definition: detection_sensor.hpp:173
~DetectionSensor() override=default
auto update(const double, const std::vector< traffic_simulator_msgs::EntityStatus > &, const rclcpp::Time &, const std::vector< std::string > &lidar_detected_entities) -> void override
Definition: concatenate.hpp:24
Definition: constants.hpp:19
Definition: lanelet_wrapper.hpp:40
geometry_msgs::msg::Pose Pose
Definition: lanelet_wrapper.hpp:66
std::string string
Definition: junit5.hpp:26
traffic_simulator_msgs::EntityStatus EntityStatus
Definition: helper_functions.hpp:32
Definition: exception.hpp:27