CARLA
 
载入中...
搜索中...
未找到
ROS2.cpp
浏览该文件的文档.
1// Copyright (c) 2023 Computer Vision Center (CVC) at the Universitat Autonoma
2// de Barcelona (UAB).
3//
4// This work is licensed under the terms of the MIT license.
5// For a copy, see <https://opensource.org/licenses/MIT>.
6
7#include "carla/Logging.h" // 引入日志模块
8#include "carla/ros2/ROS2.h" // 引入ROS2模块
9#include "carla/geom/GeoLocation.h" // 引入地理位置模块
10#include "carla/geom/Vector3D.h" // 引入三维向量模块
11#include "carla/sensor/data/DVSEvent.h" // 引入DVS事件数据模块
12#include "carla/sensor/data/LidarData.h" // 引入激光雷达数据模块
13#include "carla/sensor/data/SemanticLidarData.h" // 引入语义激光雷达数据模块
14#include "carla/sensor/data/RadarData.h" // 引入雷达数据模块
15#include "carla/sensor/data/Image.h" // 引入图像数据模块
16#include "carla/sensor/s11n/ImageSerializer.h" // 引入图像序列化模块
17#include "carla/sensor/s11n/SensorHeaderSerializer.h" // 引入传感器头序列化模块
18
19#include "publishers/CarlaPublisher.h" // 引入Carla发布者模块
20#include "publishers/CarlaClockPublisher.h" // 引入时钟发布者模块
21#include "publishers/CarlaRGBCameraPublisher.h" // 引入RGB相机发布者模块
22#include "publishers/CarlaDepthCameraPublisher.h" // 引入深度相机发布者模块
23#include "publishers/CarlaNormalsCameraPublisher.h" // 引入法线相机发布者模块
24#include "publishers/CarlaOpticalFlowCameraPublisher.h" // 引入光流相机发布者模块
25#include "publishers/CarlaSSCameraPublisher.h" // 引入超分辨率相机发布者模块
26#include "publishers/CarlaISCameraPublisher.h" // 引入实例分割相机发布者模块
27#include "publishers/CarlaDVSCameraPublisher.h" // 引入DVS相机发布者模块
28#include "publishers/CarlaLidarPublisher.h" // 引入激光雷达发布者模块
29#include "publishers/CarlaSemanticLidarPublisher.h" // 引入语义激光雷达发布者模块
30#include "publishers/CarlaRadarPublisher.h" // 引入雷达发布者模块
31#include "publishers/CarlaIMUPublisher.h" // 引入IMU发布者模块
32#include "publishers/CarlaGNSSPublisher.h" // 引入GNSS发布者模块
33#include "publishers/CarlaMapSensorPublisher.h" // 引入地图传感器发布者模块
34#include "publishers/CarlaSpeedometerSensor.h" // 引入速度计传感器模块
35#include "publishers/CarlaTransformPublisher.h" // 引入变换发布者模块
36#include "publishers/CarlaCollisionPublisher.h" // 引入碰撞发布者模块
37#include "publishers/CarlaLineInvasionPublisher.h" // 引入线路侵入发布者模块
38
39#include "subscribers/CarlaSubscriber.h" // 引入Carla订阅者模块
40#include "subscribers/CarlaEgoVehicleControlSubscriber.h" // 引入自我车辆控制订阅者模块
41
42#include <vector> // 引入向量库
43
44namespace carla {
45namespace ros2 {
46
47// 静态字段
48std::shared_ptr<ROS2> ROS2::_instance; // ROS2实例的共享指针
49
50// 传感器列表(应该等同于SensorsRegistry列表)
51enum ESensors { // 定义传感器枚举
52 CollisionSensor, // 碰撞传感器
53 DepthCamera, // 深度相机
54 NormalsCamera, // 法线相机
55 DVSCamera, // DVS相机
56 GnssSensor, // GNSS传感器
57 InertialMeasurementUnit, // 惯性测量单元
58 LaneInvasionSensor, // 车道入侵传感器
59 ObstacleDetectionSensor, // 障碍物检测传感器
60 OpticalFlowCamera, // 光流相机
61 Radar, // 雷达
62 RayCastSemanticLidar, // 光线投射语义激光雷达
63 RayCastLidar, // 光线投射激光雷达
64 RssSensor, // RSS传感器
65 SceneCaptureCamera, // 场景捕获相机
66 SemanticSegmentationCamera, // 语义分割相机
67 InstanceSegmentationCamera, // 实例分割相机
68 WorldObserver, // 世界观察者
69 CameraGBufferUint8, // 相机G缓冲区(8位无符号整数)
70 CameraGBufferFloat // 相机G缓冲区(浮点数)
71};
72// 启动或禁用ROS2系统
73// enabled是一个布尔值,表示是否启用ROS2
74// 设置_enabled 成员变量为传入的enable值
75// 记录启用状态到日志中
76// 创建一个名为clock的时钟发布者,并初始化它
77void ROS2::Enable(bool enable) { // 启用或禁用ROS2
78 _enabled = enable; // 设置启用状态
79 log_info("ROS2 enabled: ", _enabled); // 记录启用状态
80 _clock_publisher = std::make_shared<CarlaClockPublisher>("clock", ""); // 创建时钟发布者
81 _clock_publisher->Init(); // 初始化时钟发布者
82}
83// 设置当前帧,调用相应的回调函数
84// Frame是一个无符号64位整数,表示新的帧号
85// 更新_frame成员变量为传入的frame值
86// 如果控制器存在且仍然存活,并且有新消息
87// 获取当前操作者
88// 查找对应的回调函数
89// 获取控制信息并调用回调函数
90void ROS2::SetFrame(uint64_t frame) { // 设置帧
91 _frame = frame; // 更新帧
92 //log_info("ROS2 new frame: ", _frame); // 记录新帧信息
93 if (_controller) { // 如果控制器存在
94 void* actor = _controller->GetVehicle(); // 获取操作者
95 if (_controller->IsAlive()) { // 如果控制器仍然存活
96 if (_controller->HasNewMessage()) { // 如果有新消息
97 auto it = _actor_callbacks.find(actor); // 查找操作者回调
98 if (it != _actor_callbacks.end()) { // 如果找到操作者回调
99 VehicleControl control = _controller->GetMessage(); // 获取控制消息
100 it->second(actor, control); // 调用回调函数
101 }
102 }
103 } else { // 如果控制器不再存活
104 RemoveActorCallback(actor); // 移除操作者回调
105 }
106 }
107}
108
109void ROS2::SetTimestamp(double timestamp) { // 设置时间戳
110 double integral; // 整数部分
111 const double fractional = modf(timestamp, &integral); // 分数部分
112 const double multiplier = 1000000000.0; // 毫微秒乘数
113 _seconds = static_cast<int32_t>(integral); // 更新秒数
114 _nanoseconds = static_cast<uint32_t>(fractional * multiplier); // 更新纳秒数
115 _clock_publisher->SetData(_seconds, _nanoseconds); // 设置时钟数据
116 _clock_publisher->Publish(); // 发布时钟数据
117 //log_info("ROS2 new timestamp: ", _timestamp); // 记录新时间戳
118}
119
120void ROS2::AddActorRosName(void *actor, std::string ros_name) { // 添加操作者的ROS名称
121 _actor_ros_name.insert({actor, ros_name}); // 插入操作者和ROS名称
122}
123
124void ROS2::AddActorParentRosName(void *actor, void* parent) { // 添加操作者的父ROS名称
125 auto it = _actor_parent_ros_name.find(actor); // 查找操作者
126 if (it != _actor_parent_ros_name.end()) { // 如果找到
127 it->second.push_back(parent); // 添加父名称
128 } else { // 如果没找到
129 _actor_parent_ros_name.insert({actor, {parent}}); // 插入操作者和父名称
130 }
131}
132
133void ROS2::RemoveActorRosName(void *actor) { // 移除操作者的ROS名称
134 _actor_ros_name.erase(actor); // 移除ROS名称
135 _actor_parent_ros_name.erase(actor); // 移除父ROS名称
136
137 _publishers.erase(actor); // 移除发布者
138 _transforms.erase(actor); // 移除变换数据
139}
140
141void ROS2::UpdateActorRosName(void *actor, std::string ros_name) { // 更新操作者的ROS名称
142 auto it = _actor_ros_name.find(actor); // 查找操作者
143 if (it != _actor_ros_name.end()) { // 如果找到
144 it->second = ros_name; // 更新ROS名称
145 }
146}
147
148std::string ROS2::GetActorRosName(void *actor) { // 获取操作者的ROS名称
149 auto it = _actor_ros_name.find(actor); // 查找操作者
150 if (it != _actor_ros_name.end()) { // 如果找到
151 return it->second; // 返回ROS名称
152 } else { // 如果没找到
153 return std::string(""); // 返回空字符串
154 }
155}
156// 获取操作者父节点名称
157std::string ROS2::GetActorParentRosName(void *actor) {
158 auto it = _actor_parent_ros_name.find(actor);// 查找操作者的父节点ROS名称
159 if (it != _actor_parent_ros_name.end())
160 {
161 const std::string current_actor_name = GetActorRosName(actor);// 获取当前操作者的ROS名称
162 std::string parent_name;// 存储父节点名称
163 for (auto parent_it = it->second.cbegin(); parent_it != it->second.cend(); ++parent_it)
164 {
165 const std::string name = GetActorRosName(*parent_it);// 获取父节点的ROS名称
166 if (name == current_actor_name) // 如果是当前操作者,跳过
167 {
168 continue;
169 }
170 if (name.empty())// 如果名称为空,跳过
171 {
172 continue;
173 }
174 parent_name = name + '/' + parent_name;// 构建父节点名称
175 }
176 if (parent_name.back() == '/')// 如果最后一个字符是斜杠,去掉
177 parent_name.pop_back();
178 return parent_name;// 返回父节点名称
179 }
180 else
181 return std::string("");// 如果没有找到,返回空字符串
182}
183
184void ROS2::AddActorCallback(void* actor, std::string ros_name, ActorCallback callback) {
185 _actor_callbacks.insert({actor, std::move(callback)});// 添加操作者的回调函数
186
187 _controller.reset();// 重置控制器
188 _controller = std::make_shared<CarlaEgoVehicleControlSubscriber>(actor, ros_name.c_str());// 创建新的控制器
189 _controller->Init(); // 初始化控制器
190}
191
192void ROS2::RemoveActorCallback(void* actor) {
193 _controller.reset();// 重置控制器
194 _actor_callbacks.erase(actor);// 移除操作者的回调
195}
196
197std::pair<std::shared_ptr<CarlaPublisher>, std::shared_ptr<CarlaTransformPublisher>> ROS2::GetOrCreateSensor(int type, carla::streaming::detail::stream_id_type id, void* actor) {
198 auto it_publishers = _publishers.find(actor);// 查找操作者的发布者
199 auto it_transforms = _transforms.find(actor);// 查找操作者的变换发布者
200 std::shared_ptr<CarlaPublisher> publisher {};// 声明发布者
201 std::shared_ptr<CarlaTransformPublisher> transform {};// 声明变换发布者
202 if (it_publishers != _publishers.end()) {
203 publisher = it_publishers->second;// 如果找到,获取发布者
204 if (it_transforms != _transforms.end()) {
205 transform = it_transforms->second;// 如果找到,获取变换发布者
206 }
207 } else {
208 // 没找到传感器,创建一个给定类型
209 const std::string string_id = std::to_string(id);// 将ID转换为字符串
210 std::string ros_name = GetActorRosName(actor);// 获取操作者的ROS名称
211 std::string parent_ros_name = GetActorParentRosName(actor);// 获取操作者的父节点ROS名称
212 switch(type) {
213 case ESensors::CollisionSensor: {// 碰撞传感器
214 if (ros_name == "collision__") {
215 ros_name.pop_back();// 去掉最后一个字符
216 ros_name.pop_back();// 去掉倒数第二个字符
217 ros_name += string_id;// 添加ID
218 UpdateActorRosName(actor, ros_name);// 更新操作者的ROS名称
219 }
220 std::shared_ptr<CarlaCollisionPublisher> new_publisher = std::make_shared<CarlaCollisionPublisher>(ros_name.c_str(), parent_ros_name.c_str());// 创建新的碰撞发布者
221 if (new_publisher->Init()) {// 初始化发布者
222 _publishers.insert({actor, new_publisher});// 插入到发布者列表
223 publisher = new_publisher; // 设置当前发布者
224 }
225 std::shared_ptr<CarlaTransformPublisher> new_transform = std::make_shared<CarlaTransformPublisher>(ros_name.c_str(), parent_ros_name.c_str());// 创建新的变换发布者
226 if (new_transform->Init()) {// 初始化变换发布者
227 _transforms.insert({actor, new_transform});// 插入到变换发布者列表
228 transform = new_transform;// 设置当前变换发布者
229 }
230 } break;
231 case ESensors::DepthCamera: {// 深度相机
232 if (ros_name == "depth__") {
233 ros_name.pop_back(); // 去掉最后一个字符
234 ros_name.pop_back();// 去掉倒数第二个字符
235 ros_name += string_id;// 添加ID
236 UpdateActorRosName(actor, ros_name); // 更新操作者的ROS名称
237 }
238 std::shared_ptr<CarlaDepthCameraPublisher> new_publisher = std::make_shared<CarlaDepthCameraPublisher>(ros_name.c_str(),
239parent_ros_name.c_str());// 创建新的深度相机发布者
240 if (new_publisher->Init()) {// 初始化发布者
241 _publishers.insert({actor, new_publisher});// 插入到发布者列表
242 publisher = new_publisher;// 设置当前发布者
243 }
244 std::shared_ptr<CarlaTransformPublisher> new_transform = std::make_shared<CarlaTransformPublisher>(ros_name.c_str(), parent_ros_name.c_str());// 创建新的变换发布者
245 if (new_transform->Init()) {// 初始化变换发布者
246 _transforms.insert({actor, new_transform});// 插入到变换发布者列表
247 transform = new_transform; // 设置当前变换发布者
248 }
249 } break;
250 case ESensors::NormalsCamera: { // 法线相机
251 if (ros_name == "normals__") {
252 ros_name.pop_back();// 去掉最后一个字符
253 ros_name.pop_back();// 去掉倒数第二个字符
254 ros_name += string_id;// 添加ID
255 UpdateActorRosName(actor, ros_name);// 更新操作者的ROS名称
256 }
257 std::shared_ptr<CarlaNormalsCameraPublisher> new_publisher = std::make_shared<CarlaNormalsCameraPublisher>(ros_name.c_str(), parent_ros_name.c_str());// 创建一个新的法线相机发布者
258 if (new_publisher->Init()) {// 初始化发布者
259 _publishers.insert({actor, new_publisher});// 将发布者插入到发布者集合中
260 publisher = new_publisher;// 更新当前发布者
261 }
262 std::shared_ptr<CarlaTransformPublisher> new_transform = std::make_shared<CarlaTransformPublisher>(ros_name.c_str(), parent_ros_name.c_str()); // 创建一个新的变换发布者
263 if (new_transform->Init()) {// 初始化变换发布者
264 _transforms.insert({actor, new_transform});// 将变换发布者插入到变换集合中
265 transform = new_transform;// 更新当前变换
266 }
267 } break;
268 case ESensors::DVSCamera: {// DVS相机的处理
269 if (ros_name == "dvs__") {// 检查ROS名称是否为"dvs__"
270 ros_name.pop_back();// 去掉最后一个字符
271 ros_name.pop_back();// 再去掉一个字符
272 ros_name += string_id;// 添加字符串ID
273 UpdateActorRosName(actor, ros_name);// 更新操作者的ROS名称
274 }
275 std::shared_ptr<CarlaDVSCameraPublisher> new_publisher = std::make_shared<CarlaDVSCameraPublisher>(ros_name.c_str(), parent_ros_name.c_str());// 创建新的DVS相机发布者
276 if (new_publisher->Init()) {// 初始化DVS发布者
277 _publishers.insert({actor, new_publisher});// 将DVS发布者插入到发布者集合中
278 publisher = new_publisher;// 更新当前发布者
279 }
280 std::shared_ptr<CarlaTransformPublisher> new_transform = std::make_shared<CarlaTransformPublisher>(ros_name.c_str(), parent_ros_name.c_str());// 创建新的变换发布者
281 if (new_transform->Init()) {// 初始化变换发布者
282 _transforms.insert({actor, new_transform});// 将变换发布者插入到变换集合中
283 transform = new_transform;// 更新当前变换
284 }
285 } break;
286 case ESensors::GnssSensor: {// GNSS传感器的处理
287 if (ros_name == "gnss__") {// 检查ROS名称是否为"gnss__"
288 ros_name.pop_back();// 去掉最后一个字符
289 ros_name.pop_back();// 再去掉一个字符
290 ros_name += string_id;// 添加字符串ID
291 UpdateActorRosName(actor, ros_name);// 更新操作者的ROS名称
292 }
293 std::shared_ptr<CarlaGNSSPublisher> new_publisher = std::make_shared<CarlaGNSSPublisher>(ros_name.c_str(), parent_ros_name.c_str()); // 创建新的GNSS发布者
294 if (new_publisher->Init()) {// 初始化GNSS发布者
295 _publishers.insert({actor, new_publisher});// 将GNSS发布者插入到发布者集合中
296 publisher = new_publisher;// 更新当前发布者
297 }
298 std::shared_ptr<CarlaTransformPublisher> new_transform = std::make_shared<CarlaTransformPublisher>(ros_name.c_str(), parent_ros_name.c_str());// 创建新的变换发布者
299 if (new_transform->Init()) {// 初始化变换发布者
300 _transforms.insert({actor, new_transform});// 将变换发布者插入到变换集合中
301 transform = new_transform;// 更新当前变换
302 }
303 } break;
304 case ESensors::InertialMeasurementUnit: {// 惯性测量单元的处理
305 if (ros_name == "imu__") {// 检查ROS名称是否为"imu__"
306 ros_name.pop_back();// 去掉最后一个字符
307 ros_name.pop_back();// 再去掉一个字符
308 ros_name += string_id;// 添加字符串ID
309 UpdateActorRosName(actor, ros_name); // 更新操作者的ROS名称
310 }
311 std::shared_ptr<CarlaIMUPublisher> new_publisher = std::make_shared<CarlaIMUPublisher>(ros_name.c_str(), parent_ros_name.c_str());// 创建新的IMU发布者
312 if (new_publisher->Init()) {// 初始化IMU发布者
313 _publishers.insert({actor, new_publisher});// 将IMU发布者插入到发布者集合中
314 publisher = new_publisher;// 更新当前发布者
315 }
316 std::shared_ptr<CarlaTransformPublisher> new_transform = std::make_shared<CarlaTransformPublisher>(ros_name.c_str(), parent_ros_name.c_str());// 创建新的变换发布者
317 if (new_transform->Init()) {// 初始化变换发布者
318 _transforms.insert({actor, new_transform});// 将变换发布者插入到变换集合中
319 transform = new_transform;// 更新当前变换
320 }
321 } break;
322 case ESensors::LaneInvasionSensor: {// 车道侵入传感器的处理
323 if (ros_name == "lane_invasion__") {// 检查ROS名称是否为"lane_invasion__"
324 ros_name.pop_back();// 去掉最后一个字符
325 ros_name.pop_back();// 再去掉一个字符
326 ros_name += string_id;// 添加字符串ID
327 UpdateActorRosName(actor, ros_name);// 更新操作者的ROS名称
328 }
329 std::shared_ptr<CarlaLineInvasionPublisher> new_publisher = std::make_shared<CarlaLineInvasionPublisher>(ros_name.c_str(), parent_ros_name.c_str());// 创建新的压线发布者
330 if (new_publisher->Init()) {// 初始化压线发布者
331 _publishers.insert({actor, new_publisher});// 将压线发布者插入到发布者集合中
332 publisher = new_publisher;// 更新当前发布者
333 }
334 std::shared_ptr<CarlaTransformPublisher> new_transform = std::make_shared<CarlaTransformPublisher>(ros_name.c_str(), parent_ros_name.c_str());// 创建新的变换发布者
335 if (new_transform->Init()) {// 初始化变换发布者
336 _transforms.insert({actor, new_transform});// 将变换发布者插入到变换集合中
337 transform = new_transform;// 更新当前变换
338 }
339 } break;
340 case ESensors::ObstacleDetectionSensor: {// 遇到障碍物检测传感器
341 std::cerr << "Obstacle detection sensor does not have an available publisher" << std::endl;// 遇到障碍物检测传感器没有可用的发布者
342 } break;
343 case ESensors::OpticalFlowCamera: {// 光流相机
344 if (ros_name == "optical_flow__") {// 如果ros_name是光流相机
345 ros_name.pop_back(); // 移除最后一个字符
346 ros_name.pop_back();// 再次移除最后一个字符
347 ros_name += string_id;// 添加字符串标识
348 UpdateActorRosName(actor, ros_name);// 更新操作者的ros名称
349 }
350 std::shared_ptr<CarlaOpticalFlowCameraPublisher> new_publisher = std::make_shared<CarlaOpticalFlowCameraPublisher>(ros_name.c_str(), parent_ros_name.c_str());// 创建新的光流相机发布者
351 if (new_publisher->Init()) {// 初始化发布者
352 _publishers.insert({actor, new_publisher});// 将新发布者插入到发布者集合中
353 publisher = new_publisher;// 更新当前发布者
354 }
355 std::shared_ptr<CarlaTransformPublisher> new_transform = std::make_shared<CarlaTransformPublisher>(ros_name.c_str(), parent_ros_name.c_str());// 创建新的变换发布者
356 if (new_transform->Init()) {// 初始化变换发布者
357 _transforms.insert({actor, new_transform});// 将新变换发布者插入到变换集合中
358 transform = new_transform;// 更新当前变换发布者
359 }
360 } break;
361 case ESensors::Radar: {// 雷达传感器
362 if (ros_name == "radar__") {// 如果ros_name是雷达
363 ros_name.pop_back(); // 移除最后一个字符
364 ros_name.pop_back();// 再次移除最后一个字符
365 ros_name += string_id;// 添加字符串标识
366 UpdateActorRosName(actor, ros_name);// 更新操作者的ros名称
367 }
368 std::shared_ptr<CarlaRadarPublisher> new_publisher = std::make_shared<CarlaRadarPublisher>(ros_name.c_str(), parent_ros_name.c_str());// 创建新的雷达发布者
369 if (new_publisher->Init()) {// 初始化发布者
370 _publishers.insert({actor, new_publisher});// 将新发布者插入到发布者集合中
371 publisher = new_publisher;// 更新当前发布者
372 }
373 std::shared_ptr<CarlaTransformPublisher> new_transform = std::make_shared<CarlaTransformPublisher>(ros_name.c_str(), parent_ros_name.c_str());// 创建新的变换发布者
374 if (new_transform->Init()) {// 初始化变换发布者
375 _transforms.insert({actor, new_transform});// 将新变换发布者插入到变换集合中
376 transform = new_transform;// 更新当前变换发布者
377 }
378 } break;
379 case ESensors::RayCastSemanticLidar: {// 射线投射语义激光雷达
380 if (ros_name == "ray_cast_semantic__") { // 如果ros_name是射线投射语义
381 ros_name.pop_back();// 移除最后一个字符
382 ros_name.pop_back();// 再次移除最后一个字符
383 ros_name += string_id;// 添加字符串标识
384 UpdateActorRosName(actor, ros_name);// 更新操作者的ros名称
385 }
386 std::shared_ptr<CarlaSemanticLidarPublisher> new_publisher = std::make_shared<CarlaSemanticLidarPublisher>(ros_name.c_str(), parent_ros_name.c_str());// 创建新的语义激光雷达发布者
387 if (new_publisher->Init()) {// 初始化发布者
388 _publishers.insert({actor, new_publisher});// 将新发布者插入到发布者集合中
389 publisher = new_publisher;// 更新当前发布者
390 }
391 std::shared_ptr<CarlaTransformPublisher> new_transform = std::make_shared<CarlaTransformPublisher>(ros_name.c_str(), parent_ros_name.c_str());// 创建新的变换发布者
392 if (new_transform->Init()) {// 初始化变换发布者
393 _transforms.insert({actor, new_transform});// 将新变换发布者插入到变换集合中
394 transform = new_transform;// 更新当前变换发布者
395 }
396 } break;
397 case ESensors::RayCastLidar: {// 射线投射激光雷达
398 if (ros_name == "ray_cast__") {// 如果ros_name是射线投射
399 ros_name.pop_back();// 移除最后一个字符
400 ros_name.pop_back();// 再次移除最后一个字符
401 ros_name += string_id;// 添加字符串标识
402 UpdateActorRosName(actor, ros_name);// 更新操作者的ros名称
403 }
404 std::shared_ptr<CarlaLidarPublisher> new_publisher = std::make_shared<CarlaLidarPublisher>(ros_name.c_str(), parent_ros_name.c_str());// 创建新的激光雷达发布者
405 if (new_publisher->Init()) {// 初始化发布者
406 _publishers.insert({actor, new_publisher});// 将新发布者插入到发布者集合中
407 publisher = new_publisher;// 更新当前发布者
408 }
409 std::shared_ptr<CarlaTransformPublisher> new_transform = std::make_shared<CarlaTransformPublisher>(ros_name.c_str(), parent_ros_name.c_str());// 创建新的变换发布者
410 if (new_transform->Init()) {// 初始化变换发布者
411 _transforms.insert({actor, new_transform});// 将新变换发布者插入到变换集合中
412 transform = new_transform;// 更新当前变换发布者
413 }
414 } break;
415 case ESensors::RssSensor: {// RSS传感器
416 std::cerr << "RSS sensor does not have an available publisher" << std::endl;// RSS传感器没有可用的发布者
417 } break;
418 case ESensors::SceneCaptureCamera: {// 场景捕捉相机
419 if (ros_name == "rgb__") {// 如果ros_name是RGB
420 ros_name.pop_back();// 移除最后一个字符
421 ros_name.pop_back();// 再次移除最后一个字符
422 ros_name += string_id;// 添加字符串标识
423 UpdateActorRosName(actor, ros_name);// 更新操作者的ros名称
424 }
425 std::shared_ptr<CarlaRGBCameraPublisher> new_publisher = std::make_shared<CarlaRGBCameraPublisher>(ros_name.c_str(), parent_ros_name.c_str());// 创建新的RGB相机发布者
426 if (new_publisher->Init()) { // 初始化发布者
427 _publishers.insert({actor, new_publisher});// 将新发布者插入到发布者集合中
428 publisher = new_publisher; // 更新当前发布者
429 }
430 std::shared_ptr<CarlaTransformPublisher> new_transform = std::make_shared<CarlaTransformPublisher>(ros_name.c_str(), parent_ros_name.c_str());// 创建新的变换发布者
431 if (new_transform->Init()) { // 初始化变换发布者
432 _transforms.insert({actor, new_transform});// 将新变换发布者插入到变换集合中
433 transform = new_transform; // 更新当前变换发布者
434 }
435 } break;
436 case ESensors::SemanticSegmentationCamera: {// 如果传感器是语义分割相机
437 if (ros_name == "semantic_segmentation__") {// 检查ROS名称是否为语义分割相机
438 ros_name.pop_back();// 移除最后一个字符
439 ros_name.pop_back();// 移除倒数第二个字符
440 ros_name += string_id;// 添加字符串ID
441 UpdateActorRosName(actor, ros_name);// 更新操作者的ROS名称
442 }
443 std::shared_ptr<CarlaSSCameraPublisher> new_publisher = std::make_shared<CarlaSSCameraPublisher>(ros_name.c_str(), parent_ros_name.c_str()); // 创建新的语义分割相机发布者
444 if (new_publisher->Init()) {// 初始化发布者
445 _publishers.insert({actor, new_publisher});// 将新发布者插入到发布者集合中
446 publisher = new_publisher; // 更新当前发布者
447 }
448 std::shared_ptr<CarlaTransformPublisher> new_transform = std::make_shared<CarlaTransformPublisher>(ros_name.c_str(), parent_ros_name.c_str());// 创建新的变换发布者
449 if (new_transform->Init()) {// 初始化变换发布者
450 _transforms.insert({actor, new_transform});// 将新变换发布者插入到变换集合中
451 transform = new_transform;// 更新当前变换发布者
452 }
453 } break;
454 case ESensors::InstanceSegmentationCamera: {// 如果传感器是实例分割相机
455 if (ros_name == "instance_segmentation__") {// 检查ROS名称是否为实例分割相机
456 ros_name.pop_back(); // 移除最后一个字符
457 ros_name.pop_back();// 移除倒数第二个字符
458 ros_name += string_id;// 添加字符串ID
459 UpdateActorRosName(actor, ros_name); // 更新操作者的ROS名称
460 }
461 std::shared_ptr<CarlaISCameraPublisher> new_publisher = std::make_shared<CarlaISCameraPublisher>(ros_name.c_str(), parent_ros_name.c_str());// 创建新的实例分割相机发布者
462 if (new_publisher->Init()) {// 初始化发布者
463 _publishers.insert({actor, new_publisher});// 将新发布者插入到发布者集合中
464 publisher = new_publisher;// 更新当前发布者
465 }
466 std::shared_ptr<CarlaTransformPublisher> new_transform = std::make_shared<CarlaTransformPublisher>(ros_name.c_str(), parent_ros_name.c_str());// 创建新的变换发布者
467 if (new_transform->Init()) {// 初始化变换发布者
468 _transforms.insert({actor, new_transform});// 将新变换发布者插入到变换集合中
469 transform = new_transform; // 更新当前变换发布者
470 }
471 } break;
472 case ESensors::WorldObserver: {// 如果传感器是世界观察者
473 std::cerr << "World obserser does not have an available publisher" << std::endl;// 输出错误信息:世界观察者没有可用的发布者
474 } break;
475 case ESensors::CameraGBufferUint8: {// 如果传感器是GBuffer uint8相机
476 std::cerr << "Camera GBuffer uint8 does not have an available publisher" << std::endl;// 输出错误信息:GBuffer uint8相机没有可用的发布者
477 } break;
478 case ESensors::CameraGBufferFloat: {// 如果传感器是GBuffer float相机
479 std::cerr << "Camera GBuffer float does not have an available publisher" << std::endl;// 输出错误信息:GBuffer float相机没有可用的发布者
480 } break;
481 default: {// 默认情况
482 std::cerr << "Unknown sensor type" << std::endl;// 输出错误信息:未知的传感器类型
483 }
484 }
485 }
486 return { publisher, transform };// 返回当前发布者和变换发布者
487}
488
490 uint64_t sensor_type,// 传感器类型
492 const carla::geom::Transform sensor_transform,// 传感器变换
493 int W, int H, float Fov, // 宽度、高度、视场角
494 const carla::SharedBufferView buffer,// 数据缓冲区
495 void *actor) { // 操作者
496
497 switch (sensor_type) { // 根据传感器类型进行处理
498 case ESensors::CollisionSensor:// 碰撞传感器
499 log_info("Sensor Collision to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "buffer.", buffer->size());// 记录碰撞传感器数据
500 break;
501 case ESensors::DepthCamera:// 深度相机
502 {
503 log_info("Sensor DepthCamera to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "buffer.", buffer->size());// 记录深度相机数据
504 auto sensors = GetOrCreateSensor(ESensors::DepthCamera, stream_id, actor);
505 if (sensors.first) {// 如果存在第一个传感器
506 std::shared_ptr<CarlaDepthCameraPublisher> publisher = std::dynamic_pointer_cast<CarlaDepthCameraPublisher>(sensors.first); // 转换为深度相机发布者
507 const carla::sensor::s11n::ImageSerializer::ImageHeader *header =// 获取图像头信息
508 reinterpret_cast<const carla::sensor::s11n::ImageSerializer::ImageHeader *>(buffer->data());
509 if (!header) // 如果头信息为空
510 return;// 返回
511 if (!publisher->HasBeenInitialized())// 如果发布者未初始化
512 publisher->InitInfoData(0, 0, H, W, Fov, true); // 初始化信息数据
513 publisher->SetImageData(_seconds, _nanoseconds, header->height, header->width, (const uint8_t*) (buffer->data() + carla::sensor::s11n::ImageSerializer::header_offset));// 设置图像数据
514 publisher->SetCameraInfoData(_seconds, _nanoseconds);// 设置相机信息数据
515 publisher->Publish();// 发布数据
516 }
517 if (sensors.second) {// 如果存在第二个传感器
518 std::shared_ptr<CarlaTransformPublisher> publisher = std::dynamic_pointer_cast<CarlaTransformPublisher>(sensors.second); // 转换为变换发布者
519 publisher->SetData(_seconds, _nanoseconds, (const float*)&sensor_transform.location, (const float*)&sensor_transform.rotation);// 设置位置信息和旋转信息
520 publisher->Publish();// 发布数据
521 }
522 }
523 break;
524 case ESensors::NormalsCamera: // 法线相机
525 log_info("Sensor NormalsCamera to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "buffer.", buffer->size());// 记录法线相机数据
526 {
527 auto sensors = GetOrCreateSensor(ESensors::NormalsCamera, stream_id, actor); // 获取或创建传感器
528 if (sensors.first) { // 如果存在第一个传感器
529 std::shared_ptr<CarlaNormalsCameraPublisher> publisher = std::dynamic_pointer_cast<CarlaNormalsCameraPublisher>(sensors.first);// 转换为法线相机发布者
530 const carla::sensor::s11n::ImageSerializer::ImageHeader *header =// 获取图像头信息
531 reinterpret_cast<const carla::sensor::s11n::ImageSerializer::ImageHeader *>(buffer->data());
532 if (!header)// 如果头信息为空
533 return;// 返回
534 if (!publisher->HasBeenInitialized())// 如果发布者未初始化
535 publisher->InitInfoData(0, 0, H, W, Fov, true);// 初始化信息数据
536 publisher->SetImageData(_seconds, _nanoseconds, header->height, header->width, (const uint8_t*) (buffer->data() + carla::sensor::s11n::ImageSerializer::header_offset));// 设置图像数据
537 publisher->SetCameraInfoData(_seconds, _nanoseconds);// 设置相机信息数据
538 publisher->Publish();// 发布数据
539 }
540 if (sensors.second) {// 如果存在第二个传感器
541 std::shared_ptr<CarlaTransformPublisher> publisher = std::dynamic_pointer_cast<CarlaTransformPublisher>(sensors.second); // 转换为变换发布者
542 publisher->SetData(_seconds, _nanoseconds, (const float*)&sensor_transform.location, (const float*)&sensor_transform.rotation);// 设置位置信息和旋转信息
543 publisher->Publish(); // 发布数据
544 }
545 }
546 break;
547 case ESensors::LaneInvasionSensor:// 压线传感器
548 log_info("Sensor LaneInvasionSensor to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "buffer.", buffer->size());// 记录压线传感器的数据到ROS,输出帧、传感器类型、流ID和缓冲区大小
549 {
550 auto sensors = GetOrCreateSensor(ESensors::LaneInvasionSensor, stream_id, actor);// 获取或创建压线传感器
551 if (sensors.first) {// 如果第一个传感器存在
552 std::shared_ptr<CarlaLineInvasionPublisher> publisher = std::dynamic_pointer_cast<CarlaLineInvasionPublisher>(sensors.first); // 转换为压线发布者
553 publisher->SetData(_seconds, _nanoseconds, (const int32_t*) buffer->data());// 设置数据
554 publisher->Publish();// 发布数据
555 }
556 if (sensors.second) {// 如果第二个传感器存在
557 std::shared_ptr<CarlaTransformPublisher> publisher = std::dynamic_pointer_cast<CarlaTransformPublisher>(sensors.second);// 转换为变换发布者
558 publisher->SetData(_seconds, _nanoseconds, (const float*)&sensor_transform.location, (const float*)&sensor_transform.rotation);// 设置位置信息和旋转信息
559 publisher->Publish();// 发布数据
560 }
561 }
562 break;
563 case ESensors::OpticalFlowCamera:// 光流相机传感器
564 log_info("Sensor OpticalFlowCamera to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "buffer.", buffer->size());// 记录光流相机的数据到ROS,输出帧、传感器类型、流ID和缓冲区大小
565 {
566 auto sensors = GetOrCreateSensor(ESensors::OpticalFlowCamera, stream_id, actor);// 获取或创建光流相机传感器
567 if (sensors.first) { // 如果第一个传感器存在
568 std::shared_ptr<CarlaOpticalFlowCameraPublisher> publisher = std::dynamic_pointer_cast<CarlaOpticalFlowCameraPublisher>(sensors.first);// 转换为光流相机发布者
569 const carla::sensor::s11n::OpticalFlowImageSerializer::ImageHeader *header =// 获取图像头信息
570 reinterpret_cast<const carla::sensor::s11n::OpticalFlowImageSerializer::ImageHeader *>(buffer->data());
571 if (!header) // 如果没有图像头,返回
572 return;
573 if (!publisher->HasBeenInitialized())// 如果发布者尚未初始化
574 publisher->InitInfoData(0, 0, H, W, Fov, true);// 初始化信息数据
575 publisher->SetImageData(_seconds, _nanoseconds, header->height, header->width, (const float*) (buffer->data() + carla::sensor::s11n::OpticalFlowImageSerializer::header_offset));// 设置图像数据
576 publisher->SetCameraInfoData(_seconds, _nanoseconds);
577 publisher->Publish();// 发布数据
578 }
579 if (sensors.second) {// 如果第二个传感器存在
580 std::shared_ptr<CarlaTransformPublisher> publisher = std::dynamic_pointer_cast<CarlaTransformPublisher>(sensors.second);// 转换为变换发布者
581 publisher->SetData(_seconds, _nanoseconds, (const float*)&sensor_transform.location, (const float*)&sensor_transform.rotation);// 设置位置信息和旋转信息
582 publisher->Publish();// 发布数据
583 }
584 }
585 break;
586 case ESensors::RssSensor:// RSS传感器
587 log_info("Sensor RssSensor to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "buffer.", buffer->size()); // 记录RSS传感器的数据到ROS,输出帧、传感器类型、流ID和缓冲区大小
588 break;
589 case ESensors::SceneCaptureCamera:// 场景捕捉相机传感器
590 {
591 log_info("Sensor SceneCaptureCamera to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "buffer.", buffer->size());// 记录场景捕捉相机的数据到ROS,输出帧、传感器类型、流ID和缓冲区大小
592 {
593 auto sensors = GetOrCreateSensor(ESensors::SceneCaptureCamera, stream_id, actor);// 获取或创建场景捕捉相机传感器
594 if (sensors.first) {// 如果第一个传感器存在
595 std::shared_ptr<CarlaRGBCameraPublisher> publisher = std::dynamic_pointer_cast<CarlaRGBCameraPublisher>(sensors.first);// 设置图像数据
596 const carla::sensor::s11n::ImageSerializer::ImageHeader *header =// 获取图像头信息
597 reinterpret_cast<const carla::sensor::s11n::ImageSerializer::ImageHeader *>(buffer->data());
598 if (!header)// 如果没有图像头,返回
599 return;
600 if (!publisher->HasBeenInitialized())// 如果发布者尚未初始化
601 publisher->InitInfoData(0, 0, H, W, Fov, true);// 初始化信息数据
602 publisher->SetImageData(_seconds, _nanoseconds, header->height, header->width, (const uint8_t*) (buffer->data() + carla::sensor::s11n::ImageSerializer::header_offset));// 设置图像数据
603 publisher->SetCameraInfoData(_seconds, _nanoseconds);
604 publisher->Publish();// 发布数据
605 }
606 if (sensors.second) {// 如果第二个传感器存在
607 std::shared_ptr<CarlaTransformPublisher> publisher = std::dynamic_pointer_cast<CarlaTransformPublisher>(sensors.second);// 转换为变换发布者
608 publisher->SetData(_seconds, _nanoseconds, (const float*)&sensor_transform.location, (const float*)&sensor_transform.rotation); // 设置位置信息和旋转信息
609 publisher->Publish();// 发布数据
610 }
611 }
612 break;
613 }
614 case ESensors::SemanticSegmentationCamera:// 语义分割相机
615 log_info("Sensor SemanticSegmentationCamera to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "buffer.", buffer->size());// 记录信息:语义分割相机到ROS数据
616 {
617 auto sensors = GetOrCreateSensor(ESensors::SemanticSegmentationCamera, stream_id, actor);// 获取或创建传感器
618 if (sensors.first) {// 如果第一个传感器存在
619 std::shared_ptr<CarlaSSCameraPublisher> publisher = std::dynamic_pointer_cast<CarlaSSCameraPublisher>(sensors.first);// 转换为语义分割相机发布者
620 const carla::sensor::s11n::ImageSerializer::ImageHeader *header =// 获取图像头信息
621 reinterpret_cast<const carla::sensor::s11n::ImageSerializer::ImageHeader *>(buffer->data());// 从缓冲区中获取数据
622 if (!header) // 如果图像头不存在
623 return;// 返回
624 if (!publisher->HasBeenInitialized())// 如果发布者尚未初始化
625 publisher->InitInfoData(0, 0, H, W, Fov, true);// 初始化信息数据
626 publisher->SetImageData(_seconds, _nanoseconds, header->height, header->width, (const uint8_t*) (buffer->data() + carla::sensor::s11n::ImageSerializer::header_offset));// 设置图像数据
627 publisher->SetCameraInfoData(_seconds, _nanoseconds); // 设置相机信息数据
628 publisher->Publish();// 发布数据
629 }
630 if (sensors.second) {// 如果第二个传感器存在
631 std::shared_ptr<CarlaTransformPublisher> publisher = std::dynamic_pointer_cast<CarlaTransformPublisher>(sensors.second);// 转换为变换发布者
632 publisher->SetData(_seconds, _nanoseconds, (const float*)&sensor_transform.location, (const float*)&sensor_transform.rotation);// 设置位置信息和旋转信息
633 publisher->Publish();// 发布数据
634 }
635 }
636 break;// 结束该case
637 case ESensors::InstanceSegmentationCamera:// 实例分割相机
638 log_info("Sensor InstanceSegmentationCamera to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "buffer.", buffer->size());// 记录信息:实例分割相机到ROS数据
639 {
640 auto sensors = GetOrCreateSensor(ESensors::InstanceSegmentationCamera, stream_id, actor);// 获取或创建传感器
641 if (sensors.first) { // 如果第一个传感器存在
642 std::shared_ptr<CarlaISCameraPublisher> publisher = std::dynamic_pointer_cast<CarlaISCameraPublisher>(sensors.first);// 转换为实例分割相机发布者
643 const carla::sensor::s11n::ImageSerializer::ImageHeader *header =// 获取图像头信息
644 reinterpret_cast<const carla::sensor::s11n::ImageSerializer::ImageHeader *>(buffer->data());// 从缓冲区中获取数据
645 if (!header)// 如果图像头不存在
646 return;// 返回
647 if (!publisher->HasBeenInitialized())// 如果发布者尚未初始化
648 publisher->InitInfoData(0, 0, H, W, Fov, true);// 初始化信息数据
649 publisher->SetImageData(_seconds, _nanoseconds, header->height, header->width, (const uint8_t*) (buffer->data() + carla::sensor::s11n::ImageSerializer::header_offset));// 设置图像数据
650 publisher->SetCameraInfoData(_seconds, _nanoseconds);// 设置相机信息数据
651 publisher->Publish();// 发布数据
652 }
653 if (sensors.second) { // 如果第二个传感器存在
654 std::shared_ptr<CarlaTransformPublisher> publisher = std::dynamic_pointer_cast<CarlaTransformPublisher>(sensors.second);// 转换为变换发布者
655 publisher->SetData(_seconds, _nanoseconds, (const float*)&sensor_transform.location, (const float*)&sensor_transform.rotation);// 设置位置信息和旋转信息
656 publisher->Publish();// 发布数据
657 }
658 }
659 break;// 结束该case
660 case ESensors::WorldObserver:// 世界观察者
661 log_info("Sensor WorldObserver to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "buffer.", buffer->size());// 记录信息:世界观察者到ROS数据
662 break;// 结束该case
663 case ESensors::CameraGBufferUint8:// 相机G缓冲区(无符号8位)
664 log_info("Sensor CameraGBufferUint8 to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "buffer.", buffer->size()); // 记录信息:相机G缓冲区(无符号8位)到ROS数据
665 break;// 结束该case
666 case ESensors::CameraGBufferFloat:// 相机G缓冲区(浮点型)
667 log_info("Sensor CameraGBufferFloat to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "buffer.", buffer->size());// 记录信息:相机G缓冲区(浮点型)到ROS数据
668 break;// 结束该case
669 default:// 默认情况
670 log_info("Sensor to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "buffer.", buffer->size());// 记录信息:传感器到ROS数据
671 }
672}
673
675 uint64_t sensor_type,// 传感器类型
676 carla::streaming::detail::stream_id_type stream_id,// 数据流ID
677 const carla::geom::Transform sensor_transform,// 传感器变换
678 const carla::geom::GeoLocation &data, // 地理位置数据
679 void *actor) {// 操作者
680 log_info("Sensor GnssSensor to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "geo.", data.latitude, data.longitude, data.altitude);// 记录GNSS传感器数据
681 auto sensors = GetOrCreateSensor(ESensors::GnssSensor, stream_id, actor);// 获取或创建传感器
682 if (sensors.first) { // 如果存在第一个传感器
683 std::shared_ptr<CarlaGNSSPublisher> publisher = std::dynamic_pointer_cast<CarlaGNSSPublisher>(sensors.first); // 将传感器转换为GNSS发布者
684 publisher->SetData(_seconds, _nanoseconds, reinterpret_cast<const double*>(&data)); // 设置数据
685 publisher->Publish(); // 发布数据
686 }
687 if (sensors.second) { // 如果存在第二个传感器
688 std::shared_ptr<CarlaTransformPublisher> publisher = std::dynamic_pointer_cast<CarlaTransformPublisher>(sensors.second);// 将传感器转换为变换发布者
689 publisher->SetData(_seconds, _nanoseconds, (const float*)&sensor_transform.location, (const float*)&sensor_transform.rotation);// 设置变换数据
690 publisher->Publish();// 发布变换数据
691 }
692}
693
695 uint64_t sensor_type,// 传感器类型
696 carla::streaming::detail::stream_id_type stream_id,// 数据流ID
697 const carla::geom::Transform sensor_transform,// 传感器变换
698 carla::geom::Vector3D accelerometer, // 加速度计数据
699 carla::geom::Vector3D gyroscope,// 陀螺仪数据
700 float compass, // 指南针数据
701 void *actor) { // 操作者
702 log_info("Sensor InertialMeasurementUnit to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "imu.", accelerometer.x, gyroscope.x, compass);// 记录IMU传感器数据
703 auto sensors = GetOrCreateSensor(ESensors::InertialMeasurementUnit, stream_id, actor);// 获取或创建传感器
704 if (sensors.first) {// 如果存在第一个传感器
705 std::shared_ptr<CarlaIMUPublisher> publisher = std::dynamic_pointer_cast<CarlaIMUPublisher>(sensors.first);// 将传感器转换为IMU发布者
706 publisher->SetData(_seconds, _nanoseconds, reinterpret_cast<float*>(&accelerometer), reinterpret_cast<float*>(&gyroscope), compass);// 设置数据
707 publisher->Publish(); // 发布数据
708 }
709 if (sensors.second) {// 如果存在第二个传感器
710 std::shared_ptr<CarlaTransformPublisher> publisher = std::dynamic_pointer_cast<CarlaTransformPublisher>(sensors.second);// 将传感器转换为变换发布者
711 publisher->SetData(_seconds, _nanoseconds, (const float*)&sensor_transform.location, (const float*)&sensor_transform.rotation);// 设置变换数据
712 publisher->Publish();// 发布变换数据
713 }
714}
715
717 uint64_t sensor_type, // 传感器类型
718 carla::streaming::detail::stream_id_type stream_id,// 数据流ID
719 const carla::geom::Transform sensor_transform, // 传感器变换
720 const carla::SharedBufferView buffer,// 缓冲区视图
721 int W, int H, float Fov, // 宽度、高度、视场角
722 void *actor) { // 操作者
723 log_info("Sensor DVS to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id);// 记录DVS传感器数据
724 auto sensors = GetOrCreateSensor(ESensors::DVSCamera, stream_id, actor);// 获取或创建传感器
725 if (sensors.first) { // 如果存在第一个传感器
726 std::shared_ptr<CarlaDVSCameraPublisher> publisher = std::dynamic_pointer_cast<CarlaDVSCameraPublisher>(sensors.first);// 将传感器转换为DVS相机发布者
727 const carla::sensor::s11n::ImageSerializer::ImageHeader *header =// 图像头信息
728 reinterpret_cast<const carla::sensor::s11n::ImageSerializer::ImageHeader *>(buffer->data());// 从缓冲区获取头部
729 if (!header)// 如果头部为空
730 return; // 退出
731 if (!publisher->HasBeenInitialized()) // 如果发布者尚未初始化
732 publisher->InitInfoData(0, 0, H, W, Fov, true);// 初始化信息数据
733 size_t elements = (buffer->size() - carla::sensor::s11n::ImageSerializer::header_offset) / sizeof(carla::sensor::data::DVSEvent);// 计算元素数量
734 publisher->SetImageData(_seconds, _nanoseconds, elements, header->height, header->width, (const uint8_t*) (buffer->data() + carla::sensor::s11n::ImageSerializer::header_offset));// 设置图像数据
735 publisher->SetCameraInfoData(_seconds, _nanoseconds);// 设置相机信息数据
736 publisher->SetPointCloudData(1, elements * sizeof(carla::sensor::data::DVSEvent), elements, (const uint8_t*) (buffer->data() + carla::sensor::s11n::ImageSerializer::header_offset));// 设置点云数据
737 publisher->Publish();// 发布数据
738 }
739 if (sensors.second) { // 如果存在第二个传感器
740 std::shared_ptr<CarlaTransformPublisher> publisher = std::dynamic_pointer_cast<CarlaTransformPublisher>(sensors.second);// 将传感器转换为变换发布者
741 publisher->SetData(_seconds, _nanoseconds, (const float*)&sensor_transform.location, (const float*)&sensor_transform.rotation);// 设置变换数据
742 publisher->Publish();// 发布变换数据
743 }
744}
745
747 uint64_t sensor_type,// 传感器类型
748 carla::streaming::detail::stream_id_type stream_id,// 数据流ID
749 const carla::geom::Transform sensor_transform, // 传感器变换
750 carla::sensor::data::LidarData &data, // 激光雷达数据
751 void *actor) {// 操作者
752 log_info("Sensor Lidar to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "points.", data._points.size());// 记录激光雷达传感器数据
753 auto sensors = GetOrCreateSensor(ESensors::RayCastLidar, stream_id, actor);// 获取或创建传感器
754 if (sensors.first) {// 如果存在第一个传感器
755 std::shared_ptr<CarlaLidarPublisher> publisher = std::dynamic_pointer_cast<CarlaLidarPublisher>(sensors.first);// 将传感器转换为激光雷达发布者
756 size_t width = data._points.size();// 获取点云宽度
757 size_t height = 1;// 设置高度为1
758 publisher->SetData(_seconds, _nanoseconds, height, width, (float*)data._points.data());// 设置数据
759 publisher->Publish();// 发布数据
760 }
761 if (sensors.second) {// 如果存在第二个传感器
762 std::shared_ptr<CarlaTransformPublisher> publisher = std::dynamic_pointer_cast<CarlaTransformPublisher>(sensors.second);// 将传感器转换为变换发布者
763 publisher->SetData(_seconds, _nanoseconds, (const float*)&sensor_transform.location, (const float*)&sensor_transform.rotation);// 设置变换数据
764 publisher->Publish();// 发布变换数据
765 }
766}
767
769 uint64_t sensor_type,// 传感器类型
771 const carla::geom::Transform sensor_transform,// 传感器变换
772 carla::sensor::data::SemanticLidarData &data,// 语义激光雷达数据
773 void *actor) {// 操作者
774 static_assert(sizeof(float) == sizeof(uint32_t), "Invalid float size");// 确保float和uint32_t大小一致
775 log_info("Sensor SemanticLidar to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "points.", data._ser_points.size());// 记录日志:传感器语义激光雷达到ROS数据
776 auto sensors = GetOrCreateSensor(ESensors::RayCastSemanticLidar, stream_id, actor);// 获取或创建传感器
777 if (sensors.first) {// 如果传感器存在
778 std::shared_ptr<CarlaSemanticLidarPublisher> publisher = std::dynamic_pointer_cast<CarlaSemanticLidarPublisher>(sensors.first);// 动态转换到CarlaSemanticLidarPublisher
779 size_t width = data._ser_points.size();// 点的数量
780 size_t height = 1; // 高度设为1
781 publisher->SetData(_seconds, _nanoseconds, 6, height, width, (float*)data._ser_points.data());// 设置数据
782 publisher->Publish();// 发布数据
783 }
784 if (sensors.second) {// 如果第二个传感器存在
785 std::shared_ptr<CarlaTransformPublisher> publisher = std::dynamic_pointer_cast<CarlaTransformPublisher>(sensors.second);// 动态转换到CarlaTransformPublisher
786 publisher->SetData(_seconds, _nanoseconds, (const float*)&sensor_transform.location, (const float*)&sensor_transform.rotation); // 设置变换数据
787 publisher->Publish();// 发布变换数据
788 }
789}
790
792 uint64_t sensor_type,// 传感器类型
794 const carla::geom::Transform sensor_transform,// 传感器变换
795 const carla::sensor::data::RadarData &data,// 雷达数据
796 void *actor) {// 操作者
797 log_info("Sensor Radar to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "points.", data._detections.size());// 记录日志:传感器雷达到ROS数据
798 auto sensors = GetOrCreateSensor(ESensors::Radar, stream_id, actor); // 获取或创建传感器
799 if (sensors.first) {// 如果传感器存在
800 std::shared_ptr<CarlaRadarPublisher> publisher = std::dynamic_pointer_cast<CarlaRadarPublisher>(sensors.first);// 动态转换到CarlaRadarPublisher
801 size_t elements = data.GetDetectionCount();// 获取检测数量
802 size_t width = elements * sizeof(carla::sensor::data::RadarDetection); // 计算宽度
803 size_t height = 1;// 高度设为1
804 publisher->SetData(_seconds, _nanoseconds, height, width, elements, (const uint8_t*)data._detections.data()); // 设置数据
805 publisher->Publish();// 发布数据
806 }
807 if (sensors.second) { // 如果第二个传感器存在
808 std::shared_ptr<CarlaTransformPublisher> publisher = std::dynamic_pointer_cast<CarlaTransformPublisher>(sensors.second);// 动态转换到CarlaTransformPublisher
809 publisher->SetData(_seconds, _nanoseconds, (const float*)&sensor_transform.location, (const float*)&sensor_transform.rotation);// 设置变换数据
810 publisher->Publish(); // 发布变换数据
811 }
812}
813
815 uint64_t sensor_type,// 传感器类型
817 const carla::geom::Transform sensor_transform,// 传感器变换
818 AActor *first_ctor, // 第一个构造函数
819 AActor *second_actor, // 第二个操作者
820 float distance,// 距离
821 void *actor) { // 操作者
822 log_info("Sensor ObstacleDetector to ROS data: frame.", _frame, "sensor.", sensor_type, "stream.", stream_id, "distance.", distance);// 记录日志:传感器障碍物检测到ROS数据
823}
824// 处理来自碰撞传感器的数据
825// 参数
827 uint64_t sensor_type,// 传感器类型
829 const carla::geom::Transform sensor_transform,// 传感器变换
830 uint32_t other_actor,// 其他操作者
831 carla::geom::Vector3D impulse, // 冲击力
832 void* actor) { // 操作者
833 // 获取或创建一个碰撞传感器
834 auto sensors = GetOrCreateSensor(ESensors::CollisionSensor, stream_id, actor); // 获取或创建传感器
835 if (sensors.first) {// 如果传感器存在
836 // 将其转换为CarlaCollisionPublisher类型
837 std::shared_ptr<CarlaCollisionPublisher> publisher = std::dynamic_pointer_cast<CarlaCollisionPublisher>(sensors.first);// 动态转换到CarlaCollisionPublisher
838 publisher->SetData(_seconds, _nanoseconds, other_actor, impulse.x, impulse.y, impulse.z);// 设置碰撞数据
839 publisher->Publish();
840 }
841 if (sensors.second) {// 如果第二个传感器存在
842 // 将其转换为CarlaCollisionPublisher类型
843 std::shared_ptr<CarlaTransformPublisher> publisher = std::dynamic_pointer_cast<CarlaTransformPublisher>(sensors.second);// 动态转换到CarlaTransformPublisher
844 publisher->SetData(_seconds, _nanoseconds, (const float*)&sensor_transform.location, (const float*)&sensor_transform.rotation);// 设置变换数据
845 publisher->Publish();// 发布变换数据
846 }
847}
848// 该函数用于关闭系统
849// 遍历所有发布者和变换对象
850// 重置
851// 重置时钟发布者和控制器
852// 将_enabled设置为false
853void ROS2::Shutdown() {// 关闭
854 for (auto& element : _publishers) {// 遍历发布者
855 element.second.reset();// 重置发布者
856 }
857 for (auto& element : _transforms) {// 遍历变换
858 element.second.reset();// 重置变换
859 }
860 _clock_publisher.reset();// 重置时钟发布者
861 _controller.reset();// 重置控制器
862 _enabled = false;// 禁用
863}
864
865} // namespace ros2
866} // namespace carla
double altitude
经度,初始化为0.0。
Definition GeoLocation.h:27
double latitude
定义 GeoLocation 类,它是一个公开的成员。
Definition GeoLocation.h:23
double longitude
纬度,初始化为0.0。
Definition GeoLocation.h:25
std::unordered_map< void *, std::shared_ptr< CarlaTransformPublisher > > _transforms
Definition ROS2.h:161
void ProcessDataFromRadar(uint64_t sensor_type, carla::streaming::detail::stream_id_type stream_id, const carla::geom::Transform sensor_transform, const carla::sensor::data::RadarData &data, void *actor=nullptr)
Definition ROS2.cpp:791
void ProcessDataFromDVS(uint64_t sensor_type, carla::streaming::detail::stream_id_type stream_id, const carla::geom::Transform sensor_transform, const carla::SharedBufferView buffer, int W, int H, float Fov, void *actor=nullptr)
Definition ROS2.cpp:716
static std::shared_ptr< ROS2 > _instance
Definition ROS2.h:150
std::string GetActorParentRosName(void *actor)
Definition ROS2.cpp:157
void AddActorCallback(void *actor, std::string ros_name, ActorCallback callback)
Definition ROS2.cpp:184
void Enable(bool enable)
Definition ROS2.cpp:77
void RemoveActorRosName(void *actor)
Definition ROS2.cpp:133
void ProcessDataFromIMU(uint64_t sensor_type, carla::streaming::detail::stream_id_type stream_id, const carla::geom::Transform sensor_transform, carla::geom::Vector3D accelerometer, carla::geom::Vector3D gyroscope, float compass, void *actor=nullptr)
Definition ROS2.cpp:694
void Shutdown()
Definition ROS2.cpp:853
std::string GetActorRosName(void *actor)
Definition ROS2.cpp:148
void ProcessDataFromObstacleDetection(uint64_t sensor_type, carla::streaming::detail::stream_id_type stream_id, const carla::geom::Transform sensor_transform, AActor *first_actor, AActor *second_actor, float distance, void *actor=nullptr)
Definition ROS2.cpp:814
void ProcessDataFromCamera(uint64_t sensor_type, carla::streaming::detail::stream_id_type stream_id, const carla::geom::Transform sensor_transform, int W, int H, float Fov, const carla::SharedBufferView buffer, void *actor=nullptr)
Definition ROS2.cpp:489
std::unordered_map< void *, ActorCallback > _actor_callbacks
Definition ROS2.h:163
void ProcessDataFromSemanticLidar(uint64_t sensor_type, carla::streaming::detail::stream_id_type stream_id, const carla::geom::Transform sensor_transform, carla::sensor::data::SemanticLidarData &data, void *actor=nullptr)
Definition ROS2.cpp:768
std::shared_ptr< CarlaClockPublisher > _clock_publisher
Definition ROS2.h:159
void ProcessDataFromCollisionSensor(uint64_t sensor_type, carla::streaming::detail::stream_id_type stream_id, const carla::geom::Transform sensor_transform, uint32_t other_actor, carla::geom::Vector3D impulse, void *actor)
Definition ROS2.cpp:826
std::shared_ptr< CarlaEgoVehicleControlSubscriber > _controller
Definition ROS2.h:158
void AddActorRosName(void *actor, std::string ros_name)
Definition ROS2.cpp:120
void UpdateActorRosName(void *actor, std::string ros_name)
Definition ROS2.cpp:141
uint32_t _nanoseconds
Definition ROS2.h:155
std::unordered_map< void *, std::vector< void * > > _actor_parent_ros_name
Definition ROS2.h:157
void AddActorParentRosName(void *actor, void *parent)
Definition ROS2.cpp:124
void SetTimestamp(double timestamp)
Definition ROS2.cpp:109
void RemoveActorCallback(void *actor)
Definition ROS2.cpp:192
void ProcessDataFromLidar(uint64_t sensor_type, carla::streaming::detail::stream_id_type stream_id, const carla::geom::Transform sensor_transform, carla::sensor::data::LidarData &data, void *actor=nullptr)
Definition ROS2.cpp:746
void SetFrame(uint64_t frame)
Definition ROS2.cpp:90
int32_t _seconds
Definition ROS2.h:154
std::pair< std::shared_ptr< CarlaPublisher >, std::shared_ptr< CarlaTransformPublisher > > GetOrCreateSensor(int type, carla::streaming::detail::stream_id_type id, void *actor)
Definition ROS2.cpp:197
std::unordered_map< void *, std::string > _actor_ros_name
Definition ROS2.h:156
std::unordered_map< void *, std::shared_ptr< CarlaPublisher > > _publishers
Definition ROS2.h:160
uint64_t _frame
Definition ROS2.h:153
void ProcessDataFromGNSS(uint64_t sensor_type, carla::streaming::detail::stream_id_type stream_id, const carla::geom::Transform sensor_transform, const carla::geom::GeoLocation &data, void *actor=nullptr)
Definition ROS2.cpp:674
size_t GetDetectionCount() const
Definition RadarData.h:58
std::vector< RadarDetection > _detections
Definition RadarData.h:74
std::vector< SemanticLidarDetection > _ser_points
std::function< void(void *actor, ROS2CallbackData data)> ActorCallback
@ InstanceSegmentationCamera
Definition ROS2.cpp:67
@ RssSensor
Definition ROS2.cpp:64
@ NormalsCamera
Definition ROS2.cpp:54
@ RayCastSemanticLidar
Definition ROS2.cpp:62
@ GnssSensor
Definition ROS2.cpp:56
@ SceneCaptureCamera
Definition ROS2.cpp:65
@ CameraGBufferUint8
Definition ROS2.cpp:69
@ DVSCamera
Definition ROS2.cpp:55
@ RayCastLidar
Definition ROS2.cpp:63
@ SemanticSegmentationCamera
Definition ROS2.cpp:66
@ DepthCamera
Definition ROS2.cpp:53
@ CameraGBufferFloat
Definition ROS2.cpp:70
@ WorldObserver
Definition ROS2.cpp:68
@ InertialMeasurementUnit
Definition ROS2.cpp:57
@ OpticalFlowCamera
Definition ROS2.cpp:60
@ CollisionSensor
Definition ROS2.cpp:52
@ ObstacleDetectionSensor
Definition ROS2.cpp:59
@ LaneInvasionSensor
Definition ROS2.cpp:58
uint32_t stream_id_type
流ID的类型定义。
Definition Types.h:33
CARLA模拟器的主命名空间。
Definition Carla.cpp:139
std::shared_ptr< BufferView > SharedBufferView
Definition BufferView.h:163
static void log_info(Args &&... args)
Definition Logging.h:86