scenario_simulator_v2 C++ API
detection_sensor.hpp
Go to the documentation of this file.
1 // Copyright 2015 TIER IV, Inc. All rights reserved.
2 //
3 // Licensed under the Apache License, Version 2.0 (the "License");
4 // you may not use this file except in compliance with the License.
5 // You may obtain a copy of the License at
6 //
7 // http://www.apache.org/licenses/LICENSE-2.0
8 //
9 // Unless required by applicable law or agreed to in writing, software
10 // distributed under the License is distributed on an "AS IS" BASIS,
11 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 // See the License for the specific language governing permissions and
13 // limitations under the License.
14 
15 #ifndef SIMPLE_SENSOR_SIMULATOR__SENSOR_SIMULATION__DETECTION_SENSOR__DETECTION_SENSOR_HPP_
16 #define SIMPLE_SENSOR_SIMULATOR__SENSOR_SIMULATION__DETECTION_SENSOR__DETECTION_SENSOR_HPP_
17 
18 #include <simulation_interface/simulation_api_schema.pb.h>
19 
20 #include <autoware_perception_msgs/msg/tracked_objects.hpp>
21 #include <geometry/plane.hpp>
22 #include <geometry_msgs/msg/pose.hpp>
23 #include <get_parameter/get_parameter.hpp>
24 #include <memory>
25 #include <optional>
26 #include <queue>
27 #include <random>
28 #include <rclcpp/rclcpp.hpp>
30 #include <string>
31 #include <unordered_map>
32 #include <utility>
33 #include <vector>
34 
36 {
38 {
39 protected:
41 
42  simulation_api_schema::DetectionSensorConfiguration configuration_;
43 
45  const double current_simulation_time,
46  const simulation_api_schema::DetectionSensorConfiguration & configuration)
47  : previous_simulation_time_(current_simulation_time), configuration_(configuration)
48  {
49  }
50 
52  const traffic_simulator_msgs::EntityStatus &) const -> bool;
53 
55  const std::vector<traffic_simulator_msgs::EntityStatus> &) const
56  -> std::vector<traffic_simulator_msgs::EntityStatus>::const_iterator;
57 
59  const geometry_msgs::Pose & npc_pose, const geometry_msgs::Pose & ego_pose) -> bool;
60 
61 public:
62  virtual ~DetectionSensorBase() = default;
63 
64  virtual void update(
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;
68 
69 private:
70  std::optional<math::geometry::Plane> ego_plane_opt_{std::nullopt};
71  std::optional<geometry_msgs::msg::Pose> ego_plane_pose_opt_{std::nullopt};
72 };
73 
74 template <typename T, typename U = autoware_perception_msgs::msg::TrackedObjects>
76 {
77  const typename rclcpp::Publisher<T>::SharedPtr detected_objects_publisher;
78 
79  const typename rclcpp::Publisher<U>::SharedPtr ground_truth_objects_publisher;
80 
81  int noise_model_version;
82 
83  std::default_random_engine random_engine_;
84 
85  std::queue<std::pair<std::vector<traffic_simulator_msgs::EntityStatus>, double>>
86  unpublished_detected_entities, unpublished_ground_truth_entities;
87 
88  struct NoiseOutput
89  {
90  double simulation_time;
91 
92  // for noise v2/v3
93  double distance_noise, yaw_noise;
94  bool true_positive, flip;
95 
96  // for noise v4
97  double v4_position_x_noise, v4_position_y_noise;
98 
99  std::string config_name;
100 
101  explicit NoiseOutput(double simulation_time = 0.0)
102  : simulation_time(simulation_time),
103  distance_noise(0.0),
104  yaw_noise(0.0),
105  true_positive(true),
106  flip(false),
107  v4_position_x_noise(0.0),
108  v4_position_y_noise(0.0),
109  config_name("")
110  {
111  }
112  };
113 
114  std::unordered_map<std::string, NoiseOutput> noise_outputs;
115 
116  template <typename Message, typename std::enable_if_t<std::is_same_v<Message, T>, int> = 0>
117  auto delay() const
118  {
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"));
124  return delay;
125  } else {
126  return configuration_.object_recognition_delay();
127  }
128  }
129 
130  template <typename Message, typename std::enable_if_t<std::is_same_v<Message, U>, int> = 0>
131  auto delay() const
132  {
133  static const auto override_legacy_configuration = common::getParameter<bool>(
134  ground_truth_objects_publisher->get_topic_name() +
135  std::string(".override_legacy_configuration"));
136  if (override_legacy_configuration) {
137  static const auto delay = common::getParameter<double>(
138  ground_truth_objects_publisher->get_topic_name() + std::string(".delay"));
139  return delay;
140  } else {
141  return configuration_.object_recognition_ground_truth_delay();
142  }
143  }
144 
145  auto range() const
146  {
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);
152  return range;
153  } else {
154  return configuration_.range();
155  }
156  }
157 
158  auto detect_all_objects_in_range() const
159  {
160  // cspell: ignore occlusionless
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;
167  } else {
168  return configuration_.detect_all_objects_in_range();
169  }
170  }
171 
172 public:
173  explicit DetectionSensor(
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)
178  : DetectionSensorBase(current_simulation_time, configuration),
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) {
186  default:
187  [[fallthrough]];
188  case 1:
189  return configuration_.random_seed();
190  case 2:
191  [[fallthrough]];
192  case 3:
193  return common::getParameter<int>(
194  detected_objects_publisher->get_topic_name() + std::string(".seed"));
195  }
196  }();
197  seed) {
198  if (std::random_device::min() <= seed and seed <= std::random_device::max()) {
199  return seed;
200  } else {
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());
206  }
207  } else {
208  return std::random_device()();
209  }
210  }())
211  {
212  }
213 
214  ~DetectionSensor() override = default;
215 
216  auto update(
217  const double, const std::vector<traffic_simulator_msgs::EntityStatus> &, const rclcpp::Time &,
218  const std::vector<std::string> & lidar_detected_entities) -> void override;
219 };
220 } // namespace simple_sensor_simulator
221 
222 #endif // SIMPLE_SENSOR_SIMULATOR__SENSOR_SIMULATION__DETECTION_SENSOR__DETECTION_SENSOR_HPP_
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
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 &current_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
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