1#define _GLIBCXX_USE_CXX11_ABI 0
11#include <fastdds/dds/domain/DomainParticipant.hpp>
12#include <fastdds/dds/publisher/Publisher.hpp>
13#include <fastdds/dds/topic/Topic.hpp>
14#include <fastdds/dds/publisher/DataWriter.hpp>
15#include <fastdds/dds/topic/TypeSupport.hpp>
17#include <fastdds/dds/domain/qos/DomainParticipantQos.hpp>
18#include <fastdds/dds/domain/DomainParticipantFactory.hpp>
19#include <fastdds/dds/publisher/qos/PublisherQos.hpp>
20#include <fastdds/dds/topic/qos/TopicQos.hpp>
22#include <fastrtps/attributes/ParticipantAttributes.h>
23#include <fastrtps/qos/QosPolicies.h>
24#include <fastdds/dds/publisher/qos/DataWriterQos.hpp>
25#include <fastdds/dds/publisher/DataWriterListener.hpp>
38 namespace efd = eprosima::fastdds::dds;
42 using erc = eprosima::fastrtps::types::ReturnCode_t;
81 struct CarlaCameraInfoPublisherImpl {
93 efd::Topic*
_topic {
nullptr };
153 if (
_impl->_type ==
nullptr) {
154 std::cerr <<
"Invalid TypeSupport" << std::endl;
158 efd::DomainParticipantQos pqos = efd::PARTICIPANT_QOS_DEFAULT;
160 auto factory = efd::DomainParticipantFactory::get_instance();
161 _impl->_participant = factory->create_participant(0, pqos);
162 if (
_impl->_participant ==
nullptr) {
163 std::cerr <<
"Failed to create DomainParticipant" << std::endl;
166 _impl->_type.register_type(
_impl->_participant);
168 efd::PublisherQos pubqos = efd::PUBLISHER_QOS_DEFAULT;
169 _impl->_publisher =
_impl->_participant->create_publisher(pubqos,
nullptr);
170 if (
_impl->_publisher ==
nullptr) {
171 std::cerr <<
"Failed to create Publisher" << std::endl;
175 efd::TopicQos tqos = efd::TOPIC_QOS_DEFAULT;
176 const std::string publisher_type {
"/image"};
177 const std::string base {
"rt/carla/" };
178 std::string topic_name = base;
182 topic_name += publisher_type;
183 _impl->_topic =
_impl->_participant->create_topic(topic_name,
_impl->_type->getName(), tqos);
184 if (
_impl->_topic ==
nullptr) {
185 std::cerr <<
"Failed to create Topic" << std::endl;
189 efd::DataWriterQos wqos = efd::DATAWRITER_QOS_DEFAULT;
190 wqos.endpoint().history_memory_policy = eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE;
192 _impl->_datawriter =
_impl->_publisher->create_datawriter(
_impl->_topic, wqos, listener);
193 if (
_impl->_datawriter ==
nullptr) {
194 std::cerr <<
"Failed to create DataWriter" << std::endl;
209 std::cerr <<
"Invalid TypeSupport" << std::endl;
213 efd::DomainParticipantQos pqos = efd::PARTICIPANT_QOS_DEFAULT;
215 auto factory = efd::DomainParticipantFactory::get_instance();
216 _impl_info->_participant = factory->create_participant(0, pqos);
218 std::cerr <<
"Failed to create DomainParticipant" << std::endl;
223 efd::PublisherQos pubqos = efd::PUBLISHER_QOS_DEFAULT;
226 std::cerr <<
"Failed to create Publisher" << std::endl;
230 efd::TopicQos tqos = efd::TOPIC_QOS_DEFAULT;
231 const std::string publisher_type {
"/camera_info"};
232 const std::string base {
"rt/carla/" };
233 std::string topic_name = base;
237 topic_name += publisher_type;
240 std::cerr <<
"Failed to create Topic" << std::endl;
243 efd::DataWriterQos wqos = efd::DATAWRITER_QOS_DEFAULT;
247 std::cerr <<
"Failed to create DataWriter" << std::endl;
274 eprosima::fastrtps::rtps::InstanceHandle_t instance_handle;
275 eprosima::fastrtps::types::ReturnCode_t rcode =
_impl->_datawriter->write(&
_impl->_image, instance_handle);
276 if (rcode == erc::ReturnCodeValue::RETCODE_OK) {
279 if (rcode == erc::ReturnCodeValue::RETCODE_ERROR) {
280 std::cerr <<
"RETCODE_ERROR" << std::endl;
283 if (rcode == erc::ReturnCodeValue::RETCODE_UNSUPPORTED) {
284 std::cerr <<
"RETCODE_UNSUPPORTED" << std::endl;
287 if (rcode == erc::ReturnCodeValue::RETCODE_BAD_PARAMETER) {
288 std::cerr <<
"RETCODE_BAD_PARAMETER" << std::endl;
291 if (rcode == erc::ReturnCodeValue::RETCODE_PRECONDITION_NOT_MET) {
292 std::cerr <<
"RETCODE_PRECONDITION_NOT_MET" << std::endl;
295 if (rcode == erc::ReturnCodeValue::RETCODE_OUT_OF_RESOURCES) {
296 std::cerr <<
"RETCODE_OUT_OF_RESOURCES" << std::endl;
299 if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ENABLED) {
300 std::cerr <<
"RETCODE_NOT_ENABLED" << std::endl;
303 if (rcode == erc::ReturnCodeValue::RETCODE_IMMUTABLE_POLICY) {
304 std::cerr <<
"RETCODE_IMMUTABLE_POLICY" << std::endl;
307 if (rcode == erc::ReturnCodeValue::RETCODE_INCONSISTENT_POLICY) {
308 std::cerr <<
"RETCODE_INCONSISTENT_POLICY" << std::endl;
311 if (rcode == erc::ReturnCodeValue::RETCODE_ALREADY_DELETED) {
312 std::cerr <<
"RETCODE_ALREADY_DELETED" << std::endl;
315 if (rcode == erc::ReturnCodeValue::RETCODE_TIMEOUT) {
316 std::cerr <<
"RETCODE_TIMEOUT" << std::endl;
319 if (rcode == erc::ReturnCodeValue::RETCODE_NO_DATA) {
320 std::cerr <<
"RETCODE_NO_DATA" << std::endl;
323 if (rcode == erc::ReturnCodeValue::RETCODE_ILLEGAL_OPERATION) {
324 std::cerr <<
"RETCODE_ILLEGAL_OPERATION" << std::endl;
327 if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ALLOWED_BY_SECURITY) {
328 std::cerr <<
"RETCODE_NOT_ALLOWED_BY_SECURITY" << std::endl;
331 std::cerr <<
"UNKNOWN" << std::endl;
343 eprosima::fastrtps::rtps::InstanceHandle_t instance_handle;
344 eprosima::fastrtps::types::ReturnCode_t rcode =
_impl_info->_datawriter->write(&
_impl_info->_info, instance_handle);
345 if (rcode == erc::ReturnCodeValue::RETCODE_OK) {
348 if (rcode == erc::ReturnCodeValue::RETCODE_ERROR) {
349 std::cerr <<
"RETCODE_ERROR" << std::endl;
352 if (rcode == erc::ReturnCodeValue::RETCODE_UNSUPPORTED) {
353 std::cerr <<
"RETCODE_UNSUPPORTED" << std::endl;
356 if (rcode == erc::ReturnCodeValue::RETCODE_BAD_PARAMETER) {
357 std::cerr <<
"RETCODE_BAD_PARAMETER" << std::endl;
360 if (rcode == erc::ReturnCodeValue::RETCODE_PRECONDITION_NOT_MET) {
361 std::cerr <<
"RETCODE_PRECONDITION_NOT_MET" << std::endl;
364 if (rcode == erc::ReturnCodeValue::RETCODE_OUT_OF_RESOURCES) {
365 std::cerr <<
"RETCODE_OUT_OF_RESOURCES" << std::endl;
368 if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ENABLED) {
369 std::cerr <<
"RETCODE_NOT_ENABLED" << std::endl;
372 if (rcode == erc::ReturnCodeValue::RETCODE_IMMUTABLE_POLICY) {
373 std::cerr <<
"RETCODE_IMMUTABLE_POLICY" << std::endl;
376 if (rcode == erc::ReturnCodeValue::RETCODE_INCONSISTENT_POLICY) {
377 std::cerr <<
"RETCODE_INCONSISTENT_POLICY" << std::endl;
380 if (rcode == erc::ReturnCodeValue::RETCODE_ALREADY_DELETED) {
381 std::cerr <<
"RETCODE_ALREADY_DELETED" << std::endl;
384 if (rcode == erc::ReturnCodeValue::RETCODE_TIMEOUT) {
385 std::cerr <<
"RETCODE_TIMEOUT" << std::endl;
388 if (rcode == erc::ReturnCodeValue::RETCODE_NO_DATA) {
389 std::cerr <<
"RETCODE_NO_DATA" << std::endl;
392 if (rcode == erc::ReturnCodeValue::RETCODE_ILLEGAL_OPERATION) {
393 std::cerr <<
"RETCODE_ILLEGAL_OPERATION" << std::endl;
396 if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ALLOWED_BY_SECURITY) {
397 std::cerr <<
"RETCODE_NOT_ALLOWED_BY_SECURITY" << std::endl;
400 std::cerr <<
"UNKNOWN" << std::endl;
415 const size_t size = height * width * 4;
416 vector_data.resize(size);
417 std::memcpy(&vector_data[0], &data[0], size);
418 SetData(seconds, nanoseconds,height, width, std::move(vector_data));
456 std_msgs::msg::Header header;
457 header.stamp(std::move(time));
460 _impl->_image.header(std::move(header));
461 _impl->_image.width(width);
462 _impl->_image.height(height);
463 _impl->_image.encoding(
"bgra8");
464 _impl->_image.is_bigendian(0);
465 _impl->_image.step(
_impl->_image.width() *
sizeof(uint8_t) * 4);
466 _impl->_image.data(std::move(data));
481 std_msgs::msg::Header header;
482 header.stamp(std::move(time));
509 if (
_impl->_datawriter)
510 _impl->_publisher->delete_datawriter(
_impl->_datawriter);
512 if (
_impl->_publisher)
513 _impl->_participant->delete_publisher(
_impl->_publisher);
516 _impl->_participant->delete_topic(
_impl->_topic);
518 if (
_impl->_participant)
519 efd::DomainParticipantFactory::get_instance()->delete_participant(
_impl->_participant);
534 efd::DomainParticipantFactory::get_instance()->delete_participant(
_impl_info->_participant);
576 _name = std::move(other._name);
577 _parent = std::move(other._parent);
578 _impl = std::move(other._impl);
592 _name = std::move(other._name);
593 _parent = std::move(other._parent);
594 _impl = std::move(other._impl);
eProsima_user_DllExport void nanosec(uint32_t _nanosec)
此函数设置成员nanosec的值。
eProsima_user_DllExport void sec(int32_t _sec)
此函数设置成员sec的值。
bool PublishImage()
发布图像数据
void InitInfoData(uint32_t x_offset, uint32_t y_offset, uint32_t height, uint32_t width, float fov, bool do_rectify)
初始化CameraInfo数据。
void SetImageData(int32_t seconds, uint32_t nanoseconds, size_t height, size_t width, const uint8_t *data)
设置图像数据
CarlaNormalsCameraPublisher(const char *ros_name="", const char *parent="")
CarlaNormalsCameraPublisher的构造函数
void SetInfoRegionOfInterest(uint32_t x_offset, uint32_t y_offset, uint32_t height, uint32_t width, bool do_rectify)
设置感兴趣区域(Region of Interest, ROI)
bool HasBeenInitialized() const
检查CarlaNormalsCameraPublisher是否已初始化。
CarlaNormalsCameraPublisher & operator=(const CarlaNormalsCameraPublisher &)
赋值运算符重载
bool Init()
初始化CarlaNormalsCameraPublisher。
bool InitImage()
初始化图像发布者。
void SetCameraInfoData(int32_t seconds, uint32_t nanoseconds)
设置相机信息数据的时间戳
bool InitInfo()
初始化CameraInfo发布者。
void SetData(int32_t seconds, uint32_t nanoseconds, size_t height, size_t width, std::vector< uint8_t > &&data)
设置图像数据
std::shared_ptr< CarlaNormalsCameraPublisherImpl > _impl
~CarlaNormalsCameraPublisher()
CarlaNormalsCameraPublisher的析构函数
std::shared_ptr< CarlaCameraInfoPublisherImpl > _impl_info
const std::string & parent() const
此类表示用户在 IDL 文件中定义的 CameraInfo 类型的 TopicDataType。 <>
This class represents the structure CameraInfo defined by the user in the IDL file....
此类表示用户在IDL文件中定义的Image类型的主题数据类型。
This class represents the structure Image defined by the user in the IDL file.这个类表示在 IDL(接口定义语言)文件中由用...
This class represents the structure RegionOfInterest defined by the user in the IDL file.
eProsima_user_DllExport void y_offset(uint32_t _y_offset)
This function sets a value in member y_offset
eProsima_user_DllExport void width(uint32_t _width)
This function sets a value in member width
eProsima_user_DllExport void height(uint32_t _height)
This function sets a value in member height
eProsima_user_DllExport void x_offset(uint32_t _x_offset)
This function sets a value in member x_offset
eProsima_user_DllExport void do_rectify(bool _do_rectify)
This function sets a value in member do_rectify
eprosima::fastrtps::types::ReturnCode_t erc
@using erc
sensor_msgs::msg::CameraInfo _info
相机信息消息实例。
efd::DomainParticipant * _participant
DDS域参与者指针。
efd::TypeSupport _type
DDS类型支持,用于相机信息消息。
efd::Topic * _topic
DDS主题指针。
efd::DataWriter * _datawriter
DDS数据写入器指针。
efd::Publisher * _publisher
DDS发布者指针。
CarlaListener _listener
CARLA监听器实例。
CarlaNormalsCameraPublisher的内部实现结构体。
efd::DataWriter * _datawriter
DDS数据写入器指针。
efd::Topic * _topic
DDS主题指针。
CarlaListener _listener
CarlaListener实例,用于监听DDS事件。
sensor_msgs::msg::Image _image
存储的Image消息。
efd::TypeSupport _type
DDS类型支持,用于Image消息。
efd::Publisher * _publisher
DDS发布者指针。
efd::DomainParticipant * _participant
DDS域参与者指针。