scenario_simulator_v2 C++ API
simple_sensor_simulator.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__SIMPLE_SENSOR_SIMULATOR_HPP_
16 #define SIMPLE_SENSOR_SIMULATOR__SIMPLE_SENSOR_SIMULATOR_HPP_
17 
18 #include <tf2/LinearMath/Quaternion.h>
19 #include <tf2_ros/transform_broadcaster.h>
20 
21 #include <geographic_msgs/msg/geo_point.hpp>
22 #include <geometry_msgs/msg/pose_stamped.hpp>
23 #include <geometry_msgs/msg/transform_stamped.hpp>
24 #include <map>
25 #include <memory>
26 #include <rclcpp/rclcpp.hpp>
32 #include <string>
33 #include <thread>
35 #include <vector>
36 #include <visualization_msgs/msg/marker_array.hpp>
37 
38 #if __cplusplus
39 extern "C" {
40 #endif
41 
42 // The below macros are taken from https://gcc.gnu.org/wiki/Visibility and from
43 // demos/composition/include/composition/visibility_control.h at https://github.com/ros2/demos
44 #if defined _WIN32 || defined __CYGWIN__
45 #ifdef __GNUC__
46 #define SIMPLE_SENSOR_SIMULATOR_SIMPLE_SENSOR_SIMULATOR_COMPONENT_EXPORT __attribute__((dllexport))
47 #define SIMPLE_SENSOR_SIMULATOR_SIMPLE_SENSOR_SIMULATOR_COMPONENT_IMPORT __attribute__((dllimport))
48 #else
49 #define SIMPLE_SENSOR_SIMULATOR_SIMPLE_SENSOR_SIMULATOR_COMPONENT_EXPORT __declspec(dllexport)
50 #define SIMPLE_SENSOR_SIMULATOR_SIMPLE_SENSOR_SIMULATOR_COMPONENT_IMPORT __declspec(dllimport)
51 #endif
52 #ifdef SIMPLE_SENSOR_SIMULATOR_SIMPLE_SENSOR_SIMULATOR_COMPONENT_BUILDING_DLL
53 #define SIMPLE_SENSOR_SIMULATOR_SIMPLE_SENSOR_SIMULATOR_COMPONENT_PUBLIC \
54  SIMPLE_SENSOR_SIMULATOR_SIMPLE_SENSOR_SIMULATOR_COMPONENT_EXPORT
55 #else
56 #define SIMPLE_SENSOR_SIMULATOR_SIMPLE_SENSOR_SIMULATOR_COMPONENT_PUBLIC \
57  SIMPLE_SENSOR_SIMULATOR_SIMPLE_SENSOR_SIMULATOR_COMPONENT_IMPORT
58 #endif
59 #define SIMPLE_SENSOR_SIMULATOR_SIMPLE_SENSOR_SIMULATOR_COMPONENT_PUBLIC_TYPE \
60  SIMPLE_SENSOR_SIMULATOR_SIMPLE_SENSOR_SIMULATOR_COMPONENT_PUBLIC
61 #define SIMPLE_SENSOR_SIMULATOR_SIMPLE_SENSOR_SIMULATOR_COMPONENT_LOCAL
62 #else
63 #define SIMPLE_SENSOR_SIMULATOR_SIMPLE_SENSOR_SIMULATOR_COMPONENT_EXPORT \
64  __attribute__((visibility("default")))
65 #define SIMPLE_SENSOR_SIMULATOR_SIMPLE_SENSOR_SIMULATOR_COMPONENT_IMPORT
66 #if __GNUC__ >= 4
67 #define SIMPLE_SENSOR_SIMULATOR_SIMPLE_SENSOR_SIMULATOR_COMPONENT_PUBLIC \
68  __attribute__((visibility("default")))
69 #define SIMPLE_SENSOR_SIMULATOR_SIMPLE_SENSOR_SIMULATOR_COMPONENT_LOCAL \
70  __attribute__((visibility("hidden")))
71 #else
72 #define SIMPLE_SENSOR_SIMULATOR_SIMPLE_SENSOR_SIMULATOR_COMPONENT_PUBLIC
73 #define SIMPLE_SENSOR_SIMULATOR_SIMPLE_SENSOR_SIMULATOR_COMPONENT_LOCAL
74 #endif
75 #define SIMPLE_SENSOR_SIMULATOR_SIMPLE_SENSOR_SIMULATOR_COMPONENT_PUBLIC_TYPE
76 #endif
77 #if __cplusplus
78 } // extern "C"
79 #endif
80 
82 {
83 class ScenarioSimulator : public rclcpp::Node
84 {
85 public:
87  explicit ScenarioSimulator(const rclcpp::NodeOptions & options);
89 
90 private:
91  SensorSimulation sensor_sim_;
92 
93  auto initialize(const simulation_api_schema::InitializeRequest &)
94  -> simulation_api_schema::InitializeResponse;
95 
96  auto updateFrame(const simulation_api_schema::UpdateFrameRequest &)
97  -> simulation_api_schema::UpdateFrameResponse;
98 
99  auto updateStepTime(const simulation_api_schema::UpdateStepTimeRequest &)
100  -> simulation_api_schema::UpdateStepTimeResponse;
101 
102  auto updateEntityStatus(const simulation_api_schema::UpdateEntityStatusRequest &)
103  -> simulation_api_schema::UpdateEntityStatusResponse;
104 
105  auto spawnVehicleEntity(const simulation_api_schema::SpawnVehicleEntityRequest &)
106  -> simulation_api_schema::SpawnVehicleEntityResponse;
107 
108  template <typename SpawnRequestType>
109  auto insertEntitySpawnedStatus(
110  const SpawnRequestType & spawn_request, const traffic_simulator_msgs::EntityType::Enum & type,
111  const traffic_simulator_msgs::EntitySubtype::Enum & subtype) -> void;
112 
113  auto spawnPedestrianEntity(const simulation_api_schema::SpawnPedestrianEntityRequest &)
114  -> simulation_api_schema::SpawnPedestrianEntityResponse;
115 
116  auto spawnMiscObjectEntity(const simulation_api_schema::SpawnMiscObjectEntityRequest &)
117  -> simulation_api_schema::SpawnMiscObjectEntityResponse;
118 
119  auto despawnEntity(const simulation_api_schema::DespawnEntityRequest &)
120  -> simulation_api_schema::DespawnEntityResponse;
121 
122  auto attachImuSensor(const simulation_api_schema::AttachImuSensorRequest &)
123  -> simulation_api_schema::AttachImuSensorResponse;
124 
125  auto attachDetectionSensor(const simulation_api_schema::AttachDetectionSensorRequest &)
126  -> simulation_api_schema::AttachDetectionSensorResponse;
127 
128  auto attachLidarSensor(const simulation_api_schema::AttachLidarSensorRequest &)
129  -> simulation_api_schema::AttachLidarSensorResponse;
130 
131  auto attachOccupancyGridSensor(const simulation_api_schema::AttachOccupancyGridSensorRequest &)
132  -> simulation_api_schema::AttachOccupancyGridSensorResponse;
133 
134  auto updateTrafficLights(const simulation_api_schema::UpdateTrafficLightsRequest &)
135  -> simulation_api_schema::UpdateTrafficLightsResponse;
136 
137  auto attachPseudoTrafficLightDetector(
138  const simulation_api_schema::AttachPseudoTrafficLightDetectorRequest &)
139  -> simulation_api_schema::AttachPseudoTrafficLightDetectorResponse;
140 
141  int getSocketPort();
142 
143  std::vector<traffic_simulator_msgs::VehicleParameters> ego_vehicles_;
144  std::vector<traffic_simulator_msgs::VehicleParameters> vehicles_;
145  std::vector<traffic_simulator_msgs::PedestrianParameters> pedestrians_;
146  std::vector<traffic_simulator_msgs::MiscObjectParameters> misc_objects_;
147  double realtime_factor_;
148  double step_time_;
149  double current_simulation_time_;
150  double current_scenario_time_;
151  rclcpp::Time current_ros_time_;
152  bool initialized_;
153  std::map<std::string, simulation_api_schema::EntityStatus> entity_status_;
154  simulation_api_schema::UpdateTrafficLightsRequest traffic_signals_states_;
155  traffic_simulator_msgs::BoundingBox getBoundingBox(const std::string & name);
156  zeromq::MultiServer server_;
157  geographic_msgs::msg::GeoPoint getOrigin();
158  std::shared_ptr<hdmap_utils::HdMapUtils> hdmap_utils_;
159  std::shared_ptr<vehicle_simulation::EgoEntitySimulation> ego_entity_simulation_;
160 
161  bool isEgo(const std::string & name);
162  bool isEntityExists(const std::string & name);
163 };
164 } // namespace simple_sensor_simulator
165 
166 #endif // SIMPLE_SENSOR_SIMULATOR__SIMPLE_SENSOR_SIMULATOR_HPP_
Definition: simple_sensor_simulator.hpp:84
~ScenarioSimulator()
Definition: simple_sensor_simulator.cpp:70
SIMPLE_SENSOR_SIMULATOR_SIMPLE_SENSOR_SIMULATOR_COMPONENT_PUBLIC ScenarioSimulator(const rclcpp::NodeOptions &options)
Definition: simple_sensor_simulator.cpp:30
Definition: sensor_simulation.hpp:44
Definition: zmq_multi_server.hpp:32
Definition: constants.hpp:19
std::string string
Definition: junit5.hpp:26
#define SIMPLE_SENSOR_SIMULATOR_SIMPLE_SENSOR_SIMULATOR_COMPONENT_PUBLIC
Definition: simple_sensor_simulator.hpp:72