1 #define _GLIBCXX_USE_CXX11_ABI 0 14 #include <fastdds/dds/domain/DomainParticipant.hpp> 15 #include <fastdds/dds/publisher/Publisher.hpp> 16 #include <fastdds/dds/topic/Topic.hpp> 17 #include <fastdds/dds/publisher/DataWriter.hpp> 18 #include <fastdds/dds/topic/TypeSupport.hpp> 20 #include <fastdds/dds/domain/qos/DomainParticipantQos.hpp> 21 #include <fastdds/dds/domain/DomainParticipantFactory.hpp> 22 #include <fastdds/dds/publisher/qos/PublisherQos.hpp> 23 #include <fastdds/dds/topic/qos/TopicQos.hpp> 25 #include <fastrtps/attributes/ParticipantAttributes.h> 26 #include <fastrtps/qos/QosPolicies.h> 27 #include <fastdds/dds/publisher/qos/DataWriterQos.hpp> 28 #include <fastdds/dds/publisher/DataWriterListener.hpp> 34 namespace efd = eprosima::fastdds::dds;
35 using erc = eprosima::fastrtps::types::ReturnCode_t;
50 efd::Topic*
_topic {
nullptr };
74 SetInfoRegionOfInterest(x_offset, y_offset, height, width, do_rectify);
79 return InitImage() && InitInfo() && InitPointCloud();
83 if (_impl->_type ==
nullptr) {
84 std::cerr <<
"Invalid TypeSupport" << std::endl;
88 efd::DomainParticipantQos pqos = efd::PARTICIPANT_QOS_DEFAULT;
90 auto factory = efd::DomainParticipantFactory::get_instance();
91 _impl->_participant = factory->create_participant(0, pqos);
92 if (_impl->_participant ==
nullptr) {
93 std::cerr <<
"Failed to create DomainParticipant" << std::endl;
96 _impl->_type.register_type(_impl->_participant);
98 efd::PublisherQos pubqos = efd::PUBLISHER_QOS_DEFAULT;
99 _impl->_publisher = _impl->_participant->create_publisher(pubqos,
nullptr);
100 if (_impl->_publisher ==
nullptr) {
101 std::cerr <<
"Failed to create Publisher" << std::endl;
105 efd::TopicQos tqos = efd::TOPIC_QOS_DEFAULT;
106 const std::string publisher_type {
"/image"};
107 const std::string base {
"rt/carla/" };
108 std::string topic_name = base;
109 if (!_parent.empty())
110 topic_name += _parent +
"/";
112 topic_name += publisher_type;
113 _impl->_topic = _impl->_participant->create_topic(topic_name, _impl->_type->getName(), tqos);
114 if (_impl->_topic ==
nullptr) {
115 std::cerr <<
"Failed to create Topic" << std::endl;
119 efd::DataWriterQos wqos = efd::DATAWRITER_QOS_DEFAULT;
120 wqos.endpoint().history_memory_policy = eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE;
121 efd::DataWriterListener* listener = (efd::DataWriterListener*)_impl->_listener._impl.get();
122 _impl->_datawriter = _impl->_publisher->create_datawriter(_impl->_topic, wqos, listener);
123 if (_impl->_datawriter ==
nullptr) {
124 std::cerr <<
"Failed to create DataWriter" << std::endl;
132 if (_info->_type ==
nullptr) {
133 std::cerr <<
"Invalid TypeSupport" << std::endl;
137 efd::DomainParticipantQos pqos = efd::PARTICIPANT_QOS_DEFAULT;
139 auto factory = efd::DomainParticipantFactory::get_instance();
140 _info->_participant = factory->create_participant(0, pqos);
141 if (_info->_participant ==
nullptr) {
142 std::cerr <<
"Failed to create DomainParticipant" << std::endl;
145 _info->_type.register_type(_info->_participant);
147 efd::PublisherQos pubqos = efd::PUBLISHER_QOS_DEFAULT;
148 _info->_publisher = _info->_participant->create_publisher(pubqos,
nullptr);
149 if (_info->_publisher ==
nullptr) {
150 std::cerr <<
"Failed to create Publisher" << std::endl;
154 efd::TopicQos tqos = efd::TOPIC_QOS_DEFAULT;
155 const std::string publisher_type {
"/camera_info"};
156 const std::string base {
"rt/carla/" };
157 std::string topic_name = base;
158 if (!_parent.empty())
159 topic_name += _parent +
"/";
161 topic_name += publisher_type;
162 _info->_topic = _info->_participant->create_topic(topic_name, _info->_type->getName(), tqos);
163 if (_info->_topic ==
nullptr) {
164 std::cerr <<
"Failed to create Topic" << std::endl;
167 efd::DataWriterQos wqos = efd::DATAWRITER_QOS_DEFAULT;
168 efd::DataWriterListener* listener = (efd::DataWriterListener*)_info->_listener._impl.get();
169 _info->_datawriter = _info->_publisher->create_datawriter(_info->_topic, wqos, listener);
170 if (_info->_datawriter ==
nullptr) {
171 std::cerr <<
"Failed to create DataWriter" << std::endl;
180 if (_point_cloud->_type ==
nullptr) {
181 std::cerr <<
"Invalid TypeSupport" << std::endl;
185 efd::DomainParticipantQos pqos = efd::PARTICIPANT_QOS_DEFAULT;
187 auto factory = efd::DomainParticipantFactory::get_instance();
188 _point_cloud->_participant = factory->create_participant(0, pqos);
189 if (_point_cloud->_participant ==
nullptr) {
190 std::cerr <<
"Failed to create DomainParticipant" << std::endl;
193 _point_cloud->_type.register_type(_point_cloud->_participant);
195 efd::PublisherQos pubqos = efd::PUBLISHER_QOS_DEFAULT;
196 _point_cloud->_publisher = _point_cloud->_participant->create_publisher(pubqos,
nullptr);
197 if (_point_cloud->_publisher ==
nullptr) {
198 std::cerr <<
"Failed to create Publisher" << std::endl;
202 efd::TopicQos tqos = efd::TOPIC_QOS_DEFAULT;
203 const std::string publisher_type {
"/point_cloud"};
204 const std::string base {
"rt/carla/" };
205 std::string topic_name = base;
206 if (!_parent.empty())
207 topic_name += _parent +
"/";
209 topic_name += publisher_type;
210 _point_cloud->_topic = _point_cloud->_participant->create_topic(topic_name, _point_cloud->_type->getName(), tqos);
211 if (_point_cloud->_topic ==
nullptr) {
212 std::cerr <<
"Failed to create Topic" << std::endl;
216 efd::DataWriterQos wqos = efd::DATAWRITER_QOS_DEFAULT;
217 wqos.endpoint().history_memory_policy = eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE;
218 efd::DataWriterListener* listener = (efd::DataWriterListener*)_point_cloud->_listener._impl.get();
219 _point_cloud->_datawriter = _point_cloud->_publisher->create_datawriter(_point_cloud->_topic, wqos, listener);
220 if (_point_cloud->_datawriter ==
nullptr) {
221 std::cerr <<
"Failed to create DataWriter" << std::endl;
229 return PublishImage() && PublishInfo() && PublishPointCloud();
234 eprosima::fastrtps::types::ReturnCode_t rcode = _impl->_datawriter->write(& _impl->_image, instance_handle);
235 if (rcode == erc::ReturnCodeValue::RETCODE_OK) {
238 if (rcode == erc::ReturnCodeValue::RETCODE_ERROR) {
239 std::cerr <<
"RETCODE_ERROR" << std::endl;
242 if (rcode == erc::ReturnCodeValue::RETCODE_UNSUPPORTED) {
243 std::cerr <<
"RETCODE_UNSUPPORTED" << std::endl;
246 if (rcode == erc::ReturnCodeValue::RETCODE_BAD_PARAMETER) {
247 std::cerr <<
"RETCODE_BAD_PARAMETER" << std::endl;
250 if (rcode == erc::ReturnCodeValue::RETCODE_PRECONDITION_NOT_MET) {
251 std::cerr <<
"RETCODE_PRECONDITION_NOT_MET" << std::endl;
254 if (rcode == erc::ReturnCodeValue::RETCODE_OUT_OF_RESOURCES) {
255 std::cerr <<
"RETCODE_OUT_OF_RESOURCES" << std::endl;
258 if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ENABLED) {
259 std::cerr <<
"RETCODE_NOT_ENABLED" << std::endl;
262 if (rcode == erc::ReturnCodeValue::RETCODE_IMMUTABLE_POLICY) {
263 std::cerr <<
"RETCODE_IMMUTABLE_POLICY" << std::endl;
266 if (rcode == erc::ReturnCodeValue::RETCODE_INCONSISTENT_POLICY) {
267 std::cerr <<
"RETCODE_INCONSISTENT_POLICY" << std::endl;
270 if (rcode == erc::ReturnCodeValue::RETCODE_ALREADY_DELETED) {
271 std::cerr <<
"RETCODE_ALREADY_DELETED" << std::endl;
274 if (rcode == erc::ReturnCodeValue::RETCODE_TIMEOUT) {
275 std::cerr <<
"RETCODE_TIMEOUT" << std::endl;
278 if (rcode == erc::ReturnCodeValue::RETCODE_NO_DATA) {
279 std::cerr <<
"RETCODE_NO_DATA" << std::endl;
282 if (rcode == erc::ReturnCodeValue::RETCODE_ILLEGAL_OPERATION) {
283 std::cerr <<
"RETCODE_ILLEGAL_OPERATION" << std::endl;
286 if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ALLOWED_BY_SECURITY) {
287 std::cerr <<
"RETCODE_NOT_ALLOWED_BY_SECURITY" << std::endl;
290 std::cerr <<
"UNKNOWN" << std::endl;
296 eprosima::fastrtps::types::ReturnCode_t rcode = _info->_datawriter->write(& _info->_ci, instance_handle);
297 if (rcode == erc::ReturnCodeValue::RETCODE_OK) {
300 if (rcode == erc::ReturnCodeValue::RETCODE_ERROR) {
301 std::cerr <<
"RETCODE_ERROR" << std::endl;
304 if (rcode == erc::ReturnCodeValue::RETCODE_UNSUPPORTED) {
305 std::cerr <<
"RETCODE_UNSUPPORTED" << std::endl;
308 if (rcode == erc::ReturnCodeValue::RETCODE_BAD_PARAMETER) {
309 std::cerr <<
"RETCODE_BAD_PARAMETER" << std::endl;
312 if (rcode == erc::ReturnCodeValue::RETCODE_PRECONDITION_NOT_MET) {
313 std::cerr <<
"RETCODE_PRECONDITION_NOT_MET" << std::endl;
316 if (rcode == erc::ReturnCodeValue::RETCODE_OUT_OF_RESOURCES) {
317 std::cerr <<
"RETCODE_OUT_OF_RESOURCES" << std::endl;
320 if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ENABLED) {
321 std::cerr <<
"RETCODE_NOT_ENABLED" << std::endl;
324 if (rcode == erc::ReturnCodeValue::RETCODE_IMMUTABLE_POLICY) {
325 std::cerr <<
"RETCODE_IMMUTABLE_POLICY" << std::endl;
328 if (rcode == erc::ReturnCodeValue::RETCODE_INCONSISTENT_POLICY) {
329 std::cerr <<
"RETCODE_INCONSISTENT_POLICY" << std::endl;
332 if (rcode == erc::ReturnCodeValue::RETCODE_ALREADY_DELETED) {
333 std::cerr <<
"RETCODE_ALREADY_DELETED" << std::endl;
336 if (rcode == erc::ReturnCodeValue::RETCODE_TIMEOUT) {
337 std::cerr <<
"RETCODE_TIMEOUT" << std::endl;
340 if (rcode == erc::ReturnCodeValue::RETCODE_NO_DATA) {
341 std::cerr <<
"RETCODE_NO_DATA" << std::endl;
344 if (rcode == erc::ReturnCodeValue::RETCODE_ILLEGAL_OPERATION) {
345 std::cerr <<
"RETCODE_ILLEGAL_OPERATION" << std::endl;
348 if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ALLOWED_BY_SECURITY) {
349 std::cerr <<
"RETCODE_NOT_ALLOWED_BY_SECURITY" << std::endl;
352 std::cerr <<
"UNKNOWN" << std::endl;
358 eprosima::fastrtps::types::ReturnCode_t rcode = _point_cloud->_datawriter->write(&_point_cloud->_pc, instance_handle);
359 if (rcode == erc::ReturnCodeValue::RETCODE_OK) {
362 if (rcode == erc::ReturnCodeValue::RETCODE_ERROR) {
363 std::cerr <<
"RETCODE_ERROR" << std::endl;
366 if (rcode == erc::ReturnCodeValue::RETCODE_UNSUPPORTED) {
367 std::cerr <<
"RETCODE_UNSUPPORTED" << std::endl;
370 if (rcode == erc::ReturnCodeValue::RETCODE_BAD_PARAMETER) {
371 std::cerr <<
"RETCODE_BAD_PARAMETER" << std::endl;
374 if (rcode == erc::ReturnCodeValue::RETCODE_PRECONDITION_NOT_MET) {
375 std::cerr <<
"RETCODE_PRECONDITION_NOT_MET" << std::endl;
378 if (rcode == erc::ReturnCodeValue::RETCODE_OUT_OF_RESOURCES) {
379 std::cerr <<
"RETCODE_OUT_OF_RESOURCES" << std::endl;
382 if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ENABLED) {
383 std::cerr <<
"RETCODE_NOT_ENABLED" << std::endl;
386 if (rcode == erc::ReturnCodeValue::RETCODE_IMMUTABLE_POLICY) {
387 std::cerr <<
"RETCODE_IMMUTABLE_POLICY" << std::endl;
390 if (rcode == erc::ReturnCodeValue::RETCODE_INCONSISTENT_POLICY) {
391 std::cerr <<
"RETCODE_INCONSISTENT_POLICY" << std::endl;
394 if (rcode == erc::ReturnCodeValue::RETCODE_ALREADY_DELETED) {
395 std::cerr <<
"RETCODE_ALREADY_DELETED" << std::endl;
398 if (rcode == erc::ReturnCodeValue::RETCODE_TIMEOUT) {
399 std::cerr <<
"RETCODE_TIMEOUT" << std::endl;
402 if (rcode == erc::ReturnCodeValue::RETCODE_NO_DATA) {
403 std::cerr <<
"RETCODE_NO_DATA" << std::endl;
406 if (rcode == erc::ReturnCodeValue::RETCODE_ILLEGAL_OPERATION) {
407 std::cerr <<
"RETCODE_ILLEGAL_OPERATION" << std::endl;
410 if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ALLOWED_BY_SECURITY) {
411 std::cerr <<
"RETCODE_NOT_ALLOWED_BY_SECURITY" << std::endl;
414 std::cerr <<
"UNKNOWN" << std::endl;
419 std::vector<uint8_t> im_data;
420 const size_t im_size = width * height * 3;
421 im_data.resize(im_size);
423 for (
size_t i = 0; i < elements; ++i, ++vec_event) {
424 size_t index = (vec_event->
y * width + vec_event->
x) * 3 + (static_cast<int>(vec_event->
pol) * 2);
425 im_data[index] = 255;
428 SetData(seconds, nanoseconds, height, width, std::move(im_data));
437 header.
stamp(std::move(time));
439 _impl->_image.header(header);
440 _info->_ci.header(header);
441 _point_cloud->_pc.header(header);
443 _impl->_image.width(width);
444 _impl->_image.height(height);
445 _impl->_image.encoding(
"bgr8");
446 _impl->_image.is_bigendian(0);
447 _impl->_image.step(_impl->_image.width() *
sizeof(uint8_t) * 3);
448 _impl->_image.data(std::move(data));
457 header.
stamp(std::move(time));
473 std::vector<uint8_t> vector_data;
474 const size_t size = height * width;
475 vector_data.resize(size);
476 std::memcpy(&vector_data[0], &data[0], size);
479 descriptor1.
name(
"x");
482 descriptor1.
count(1);
484 descriptor2.
name(
"y");
487 descriptor2.
count(1);
489 descriptor3.
name(
"t");
492 descriptor3.
count(1);
494 descriptor3.
name(
"pol");
497 descriptor3.
count(1);
500 _point_cloud->_pc.width(width);
501 _point_cloud->_pc.height(height);
502 _point_cloud->_pc.is_bigendian(
false);
503 _point_cloud->_pc.fields({descriptor1, descriptor2, descriptor3, descriptor4});
504 _point_cloud->_pc.point_step(point_size);
505 _point_cloud->_pc.row_step(width * point_size);
506 _point_cloud->_pc.is_dense(
false);
507 _point_cloud->_pc.data(std::move(vector_data));
522 if (
_impl->_datawriter)
523 _impl->_publisher->delete_datawriter(
_impl->_datawriter);
525 if (
_impl->_publisher)
526 _impl->_participant->delete_publisher(
_impl->_publisher);
529 _impl->_participant->delete_topic(
_impl->_topic);
531 if (
_impl->_participant)
532 efd::DomainParticipantFactory::get_instance()->delete_participant(
_impl->_participant);
537 if (
_info->_datawriter)
538 _info->_publisher->delete_datawriter(
_info->_datawriter);
540 if (
_info->_publisher)
541 _info->_participant->delete_publisher(
_info->_publisher);
544 _info->_participant->delete_topic(
_info->_topic);
546 if (
_info->_participant)
547 efd::DomainParticipantFactory::get_instance()->delete_participant(
_info->_participant);
562 efd::DomainParticipantFactory::get_instance()->delete_participant(
_point_cloud->_participant);
587 _name = std::move(other._name);
588 _parent = std::move(other._parent);
589 _impl = std::move(other._impl);
590 _info = std::move(other._info);
596 _name = std::move(other._name);
597 _parent = std::move(other._parent);
598 _impl = std::move(other._impl);
599 _info = std::move(other._info);
bool HasBeenInitialized() const
This class represents the structure PointCloud2 defined by the user in the IDL file.
~CarlaDVSCameraPublisher()
void SetInfoRegionOfInterest(uint32_t x_offset, uint32_t y_offset, uint32_t height, uint32_t width, bool do_rectify)
CarlaDVSCameraPublisher(const char *ros_name="", const char *parent="")
void SetPointCloudData(size_t height, size_t width, size_t elements, const uint8_t *data)
This class represents the structure PointField defined by the user in the IDL file.
void SetImageData(int32_t seconds, uint32_t nanoseconds, size_t elements, size_t height, size_t width, const uint8_t *data)
eProsima_user_DllExport void name(const std::string &_name)
This function copies the value in member name.
eprosima::fastrtps::rtps::InstanceHandle_t InstanceHandle_t
eprosima::fastrtps::types::ReturnCode_t erc
This file contains definitions of common data structures used in traffic manager. ...
This class represents the structure CameraInfo defined by the user in the IDL file.
This class represents the structure Image defined by the user in the IDL file.
efd::DataWriter * _datawriter
void SetData(int32_t seconds, uint32_t nanoseconds, size_t height, size_t width, std::vector< uint8_t > &&data)
eProsima_user_DllExport void offset(uint32_t _offset)
This function sets a value in member offset.
eProsima_user_DllExport void width(uint32_t _width)
This function sets a value in member width.
eProsima_user_DllExport void datatype(uint8_t _datatype)
This function sets a value in member datatype.
This class represents the TopicDataType of the type Image defined by the user in the IDL file...
const uint8_t PointField__UINT16
This class represents the TopicDataType of the type PointCloud2 defined by the user in the IDL file...
sensor_msgs::msg::Image _image
CarlaDVSCameraPublisher & operator=(const CarlaDVSCameraPublisher &)
eProsima_user_DllExport void x_offset(uint32_t _x_offset)
This function sets a value in member x_offset.
const uint8_t PointField__FLOAT64
eProsima_user_DllExport void sec(int32_t _sec)
This function sets a value in member sec.
std::shared_ptr< CarlaPointCloudPublisherImpl > _point_cloud
eProsima_user_DllExport void count(uint32_t _count)
This function sets a value in member count.
This class represents the TopicDataType of the type CameraInfo defined by the user in the IDL file...
eProsima_user_DllExport void nanosec(uint32_t _nanosec)
This function sets a value in member nanosec.
const uint8_t PointField__INT8
void SetCameraInfoData(int32_t seconds, uint32_t nanoseconds)
This class represents the structure RegionOfInterest defined by the user in the IDL file...
const std::string & parent() const
This class represents the structure Time defined by the user in the IDL file.
std::shared_ptr< CarlaCameraInfoPublisherImpl > _info
eProsima_user_DllExport void height(uint32_t _height)
This function sets a value in member height.
eProsima_user_DllExport void y_offset(uint32_t _y_offset)
This function sets a value in member y_offset.
void InitInfoData(uint32_t x_offset, uint32_t y_offset, uint32_t height, uint32_t width, float fov, bool do_rectify)
efd::Publisher * _publisher
std::shared_ptr< CarlaDVSCameraPublisherImpl > _impl
eProsima_user_DllExport void do_rectify(bool _do_rectify)
This function sets a value in member do_rectify.
efd::DomainParticipant * _participant