15 #ifndef SIMPLE_SENSOR_SIMULATOR__SENSOR_SIMULATION__SENSOR_SIMULATION_HPP_
16 #define SIMPLE_SENSOR_SIMULATOR__SENSOR_SIMULATION__SENSOR_SIMULATION_HPP_
18 #include <simulation_api_schema.pb.h>
20 #include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
21 #include <autoware_auto_perception_msgs/msg/tracked_objects.hpp>
22 #include <autoware_auto_perception_msgs/msg/traffic_signal_array.hpp>
23 #include <autoware_perception_msgs/msg/traffic_signal_array.hpp>
26 #include <rclcpp/rclcpp.hpp>
39 const double current_simulation_time,
40 const simulation_api_schema::LidarConfiguration & configuration, rclcpp::Node & node) ->
void
42 if (configuration.architecture_type().find(
"awf/universe") != std::string::npos) {
44 current_simulation_time, configuration,
45 node.create_publisher<sensor_msgs::msg::PointCloud2>(
46 "/perception/obstacle_segmentation/pointcloud", 1)));
49 ss <<
"Unexpected architecture_type " << std::quoted(configuration.architecture_type())
51 throw std::runtime_error(ss.str());
56 const double current_simulation_time,
57 const simulation_api_schema::DetectionSensorConfiguration & configuration, rclcpp::Node & node)
60 if (configuration.architecture_type().find(
"awf/universe") != std::string::npos) {
61 using Message = autoware_auto_perception_msgs::msg::DetectedObjects;
62 using GroundTruthMessage = autoware_auto_perception_msgs::msg::TrackedObjects;
64 current_simulation_time, configuration,
65 node.create_publisher<Message>(
"/perception/object_recognition/detection/objects", 1),
66 node.create_publisher<GroundTruthMessage>(
67 "/perception/object_recognition/ground_truth/objects", 1)));
70 ss <<
"Unexpected architecture_type " << std::quoted(configuration.architecture_type())
72 throw std::runtime_error(ss.str());
77 const double current_simulation_time,
78 const simulation_api_schema::OccupancyGridSensorConfiguration & configuration,
79 rclcpp::Node & node) ->
void
81 if (configuration.architecture_type().find(
"awf/universe") != std::string::npos) {
82 using Message = nav_msgs::msg::OccupancyGrid;
84 current_simulation_time, configuration,
85 node.create_publisher<Message>(
"/perception/occupancy_grid_map/map", 1)));
88 ss <<
"Unexpected architecture_type " << std::quoted(configuration.architecture_type())
90 throw std::runtime_error(ss.str());
96 const simulation_api_schema::PseudoTrafficLightDetectorConfiguration & configuration,
97 rclcpp::Node & node, std::shared_ptr<hdmap_utils::HdMapUtils>
hdmap_utils) ->
void
99 if (configuration.architecture_type() ==
"awf/universe") {
100 using Message = autoware_auto_perception_msgs::msg::TrafficSignalArray;
101 traffic_lights_detectors_.push_back(std::make_unique<traffic_lights::TrafficLightsDetector>(
103 "/perception/traffic_light_recognition/traffic_signals", &node,
hdmap_utils)));
104 }
else if (configuration.architecture_type() >=
"awf/universe/20230906") {
105 using Message = autoware_perception_msgs::msg::TrafficSignalArray;
106 traffic_lights_detectors_.push_back(std::make_unique<traffic_lights::TrafficLightsDetector>(
108 "/perception/traffic_light_recognition/internal/traffic_signals", &node,
hdmap_utils)));
110 std::stringstream ss;
111 ss <<
"Unexpected architecture_type " << std::quoted(configuration.architecture_type())
113 throw std::runtime_error(ss.str());
118 double current_simulation_time,
const rclcpp::Time & current_ros_time,
119 const std::vector<traffic_simulator_msgs::EntityStatus> &,
120 const simulation_api_schema::UpdateTrafficLightsRequest &) -> void;
123 std::vector<std::unique_ptr<LidarSensorBase>> lidar_sensors_;
124 std::vector<std::unique_ptr<DetectionSensorBase>> detection_sensors_;
125 std::vector<std::unique_ptr<OccupancyGridSensorBase>> occupancy_grid_sensors_;
126 std::vector<std::unique_ptr<traffic_lights::TrafficLightsDetector>> traffic_lights_detectors_;
Definition: detection_sensor.hpp:62
Definition: lidar_sensor.hpp:60
occupancy grid sensor implementation
Definition: occupancy_grid_sensor.hpp:83
Definition: sensor_simulation.hpp:36
auto attachDetectionSensor(const double current_simulation_time, const simulation_api_schema::DetectionSensorConfiguration &configuration, rclcpp::Node &node) -> void
Definition: sensor_simulation.hpp:55
auto attachOccupancyGridSensor(const double current_simulation_time, const simulation_api_schema::OccupancyGridSensorConfiguration &configuration, rclcpp::Node &node) -> void
Definition: sensor_simulation.hpp:76
auto attachLidarSensor(const double current_simulation_time, const simulation_api_schema::LidarConfiguration &configuration, rclcpp::Node &node) -> void
Definition: sensor_simulation.hpp:38
auto updateSensorFrame(double current_simulation_time, const rclcpp::Time ¤t_ros_time, const std::vector< traffic_simulator_msgs::EntityStatus > &, const simulation_api_schema::UpdateTrafficLightsRequest &) -> void
Definition: sensor_simulation.cpp:22
auto attachPseudoTrafficLightsDetector(const double, const simulation_api_schema::PseudoTrafficLightDetectorConfiguration &configuration, rclcpp::Node &node, std::shared_ptr< hdmap_utils::HdMapUtils > hdmap_utils) -> void
Definition: sensor_simulation.hpp:94
Definition: traffic_light_publisher.hpp:36
Definition: constants.hpp:19