CARLA
CarlaDepthCameraPublisher.cpp
Go to the documentation of this file.
1 #define _GLIBCXX_USE_CXX11_ABI 0
2 
4 
5 #include <string>
6 
10 
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>
16 
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>
21 
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>
26 
27 
28 namespace carla {
29 namespace ros2 {
30 
31  namespace efd = eprosima::fastdds::dds;
32  using erc = eprosima::fastrtps::types::ReturnCode_t;
33 
35  efd::DomainParticipant* _participant { nullptr };
36  efd::Publisher* _publisher { nullptr };
37  efd::Topic* _topic { nullptr };
38  efd::DataWriter* _datawriter { nullptr };
39  efd::TypeSupport _type { new sensor_msgs::msg::ImagePubSubType() };
42  };
43 
45  efd::DomainParticipant* _participant { nullptr };
46  efd::Publisher* _publisher { nullptr };
47  efd::Topic* _topic { nullptr };
48  efd::DataWriter* _datawriter { nullptr };
49  efd::TypeSupport _type { new sensor_msgs::msg::CameraInfoPubSubType() };
51  bool _init {false};
53  };
54 
56  return _impl_info->_init;
57  }
58 
59  void CarlaDepthCameraPublisher::InitInfoData(uint32_t x_offset, uint32_t y_offset, uint32_t height, uint32_t width, float fov, bool do_rectify) {
60  _impl_info->_info = std::move(sensor_msgs::msg::CameraInfo(height, width, fov));
61  SetInfoRegionOfInterest(x_offset, y_offset, height, width, do_rectify);
62  _impl_info->_init = true;
63  }
64 
66  return InitImage() && InitInfo();
67  }
68 
70  if (_impl->_type == nullptr) {
71  std::cerr << "Invalid TypeSupport" << std::endl;
72  return false;
73  }
74 
75  efd::DomainParticipantQos pqos = efd::PARTICIPANT_QOS_DEFAULT;
76  pqos.name(_name);
77  auto factory = efd::DomainParticipantFactory::get_instance();
78  _impl->_participant = factory->create_participant(0, pqos);
79  if (_impl->_participant == nullptr) {
80  std::cerr << "Failed to create DomainParticipant" << std::endl;
81  return false;
82  }
83  _impl->_type.register_type(_impl->_participant);
84 
85  efd::PublisherQos pubqos = efd::PUBLISHER_QOS_DEFAULT;
86  _impl->_publisher = _impl->_participant->create_publisher(pubqos, nullptr);
87  if (_impl->_publisher == nullptr) {
88  std::cerr << "Failed to create Publisher" << std::endl;
89  return false;
90  }
91 
92  efd::TopicQos tqos = efd::TOPIC_QOS_DEFAULT;
93  const std::string publisher_type {"/image"};
94  const std::string base { "rt/carla/" };
95  std::string topic_name = base;
96  if (!_parent.empty())
97  topic_name += _parent + "/";
98  topic_name += _name;
99  topic_name += publisher_type;
100  _impl->_topic = _impl->_participant->create_topic(topic_name, _impl->_type->getName(), tqos);
101  if (_impl->_topic == nullptr) {
102  std::cerr << "Failed to create Topic" << std::endl;
103  return false;
104  }
105 
106  efd::DataWriterQos wqos = efd::DATAWRITER_QOS_DEFAULT;
107  wqos.endpoint().history_memory_policy = eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE;
108  efd::DataWriterListener* listener = (efd::DataWriterListener*)_impl->_listener._impl.get();
109  _impl->_datawriter = _impl->_publisher->create_datawriter(_impl->_topic, wqos, listener);
110  if (_impl->_datawriter == nullptr) {
111  std::cerr << "Failed to create DataWriter" << std::endl;
112  return false;
113  }
114  _frame_id = _name;
115  return true;
116  }
117 
119  if (_impl_info->_type == nullptr) {
120  std::cerr << "Invalid TypeSupport" << std::endl;
121  return false;
122  }
123 
124  efd::DomainParticipantQos pqos = efd::PARTICIPANT_QOS_DEFAULT;
125  pqos.name(_name);
126  auto factory = efd::DomainParticipantFactory::get_instance();
127  _impl_info->_participant = factory->create_participant(0, pqos);
128  if (_impl_info->_participant == nullptr) {
129  std::cerr << "Failed to create DomainParticipant" << std::endl;
130  return false;
131  }
132  _impl_info->_type.register_type(_impl_info->_participant);
133 
134  efd::PublisherQos pubqos = efd::PUBLISHER_QOS_DEFAULT;
135  _impl_info->_publisher = _impl_info->_participant->create_publisher(pubqos, nullptr);
136  if (_impl_info->_publisher == nullptr) {
137  std::cerr << "Failed to create Publisher" << std::endl;
138  return false;
139  }
140 
141  efd::TopicQos tqos = efd::TOPIC_QOS_DEFAULT;
142  const std::string publisher_type {"/camera_info"};
143  const std::string base { "rt/carla/" };
144  std::string topic_name = base;
145  if (!_parent.empty())
146  topic_name += _parent + "/";
147  topic_name += _name;
148  topic_name += publisher_type;
149  _impl_info->_topic = _impl_info->_participant->create_topic(topic_name, _impl_info->_type->getName(), tqos);
150  if (_impl_info->_topic == nullptr) {
151  std::cerr << "Failed to create Topic" << std::endl;
152  return false;
153  }
154  efd::DataWriterQos wqos = efd::DATAWRITER_QOS_DEFAULT;
155  efd::DataWriterListener* listener = (efd::DataWriterListener*)_impl_info->_listener._impl.get();
156  _impl_info->_datawriter = _impl_info->_publisher->create_datawriter(_impl_info->_topic, wqos, listener);
157  if (_impl_info->_datawriter == nullptr) {
158  std::cerr << "Failed to create DataWriter" << std::endl;
159  return false;
160  }
161 
162  _frame_id = _name;
163  return true;
164  }
165 
167  return PublishImage() && PublishInfo();
168  }
169 
172  eprosima::fastrtps::types::ReturnCode_t rcode = _impl->_datawriter->write(&_impl->_image, instance_handle);
173  if (rcode == erc::ReturnCodeValue::RETCODE_OK) {
174  return true;
175  }
176  if (rcode == erc::ReturnCodeValue::RETCODE_ERROR) {
177  std::cerr << "RETCODE_ERROR" << std::endl;
178  return false;
179  }
180  if (rcode == erc::ReturnCodeValue::RETCODE_UNSUPPORTED) {
181  std::cerr << "RETCODE_UNSUPPORTED" << std::endl;
182  return false;
183  }
184  if (rcode == erc::ReturnCodeValue::RETCODE_BAD_PARAMETER) {
185  std::cerr << "RETCODE_BAD_PARAMETER" << std::endl;
186  return false;
187  }
188  if (rcode == erc::ReturnCodeValue::RETCODE_PRECONDITION_NOT_MET) {
189  std::cerr << "RETCODE_PRECONDITION_NOT_MET" << std::endl;
190  return false;
191  }
192  if (rcode == erc::ReturnCodeValue::RETCODE_OUT_OF_RESOURCES) {
193  std::cerr << "RETCODE_OUT_OF_RESOURCES" << std::endl;
194  return false;
195  }
196  if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ENABLED) {
197  std::cerr << "RETCODE_NOT_ENABLED" << std::endl;
198  return false;
199  }
200  if (rcode == erc::ReturnCodeValue::RETCODE_IMMUTABLE_POLICY) {
201  std::cerr << "RETCODE_IMMUTABLE_POLICY" << std::endl;
202  return false;
203  }
204  if (rcode == erc::ReturnCodeValue::RETCODE_INCONSISTENT_POLICY) {
205  std::cerr << "RETCODE_INCONSISTENT_POLICY" << std::endl;
206  return false;
207  }
208  if (rcode == erc::ReturnCodeValue::RETCODE_ALREADY_DELETED) {
209  std::cerr << "RETCODE_ALREADY_DELETED" << std::endl;
210  return false;
211  }
212  if (rcode == erc::ReturnCodeValue::RETCODE_TIMEOUT) {
213  std::cerr << "RETCODE_TIMEOUT" << std::endl;
214  return false;
215  }
216  if (rcode == erc::ReturnCodeValue::RETCODE_NO_DATA) {
217  std::cerr << "RETCODE_NO_DATA" << std::endl;
218  return false;
219  }
220  if (rcode == erc::ReturnCodeValue::RETCODE_ILLEGAL_OPERATION) {
221  std::cerr << "RETCODE_ILLEGAL_OPERATION" << std::endl;
222  return false;
223  }
224  if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ALLOWED_BY_SECURITY) {
225  std::cerr << "RETCODE_NOT_ALLOWED_BY_SECURITY" << std::endl;
226  return false;
227  }
228  std::cerr << "UNKNOWN" << std::endl;
229  return false;
230  }
231 
234  eprosima::fastrtps::types::ReturnCode_t rcode = _impl_info->_datawriter->write(&_impl_info->_info, instance_handle);
235  if (rcode == erc::ReturnCodeValue::RETCODE_OK) {
236  return true;
237  }
238  if (rcode == erc::ReturnCodeValue::RETCODE_ERROR) {
239  std::cerr << "RETCODE_ERROR" << std::endl;
240  return false;
241  }
242  if (rcode == erc::ReturnCodeValue::RETCODE_UNSUPPORTED) {
243  std::cerr << "RETCODE_UNSUPPORTED" << std::endl;
244  return false;
245  }
246  if (rcode == erc::ReturnCodeValue::RETCODE_BAD_PARAMETER) {
247  std::cerr << "RETCODE_BAD_PARAMETER" << std::endl;
248  return false;
249  }
250  if (rcode == erc::ReturnCodeValue::RETCODE_PRECONDITION_NOT_MET) {
251  std::cerr << "RETCODE_PRECONDITION_NOT_MET" << std::endl;
252  return false;
253  }
254  if (rcode == erc::ReturnCodeValue::RETCODE_OUT_OF_RESOURCES) {
255  std::cerr << "RETCODE_OUT_OF_RESOURCES" << std::endl;
256  return false;
257  }
258  if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ENABLED) {
259  std::cerr << "RETCODE_NOT_ENABLED" << std::endl;
260  return false;
261  }
262  if (rcode == erc::ReturnCodeValue::RETCODE_IMMUTABLE_POLICY) {
263  std::cerr << "RETCODE_IMMUTABLE_POLICY" << std::endl;
264  return false;
265  }
266  if (rcode == erc::ReturnCodeValue::RETCODE_INCONSISTENT_POLICY) {
267  std::cerr << "RETCODE_INCONSISTENT_POLICY" << std::endl;
268  return false;
269  }
270  if (rcode == erc::ReturnCodeValue::RETCODE_ALREADY_DELETED) {
271  std::cerr << "RETCODE_ALREADY_DELETED" << std::endl;
272  return false;
273  }
274  if (rcode == erc::ReturnCodeValue::RETCODE_TIMEOUT) {
275  std::cerr << "RETCODE_TIMEOUT" << std::endl;
276  return false;
277  }
278  if (rcode == erc::ReturnCodeValue::RETCODE_NO_DATA) {
279  std::cerr << "RETCODE_NO_DATA" << std::endl;
280  return false;
281  }
282  if (rcode == erc::ReturnCodeValue::RETCODE_ILLEGAL_OPERATION) {
283  std::cerr << "RETCODE_ILLEGAL_OPERATION" << std::endl;
284  return false;
285  }
286  if (rcode == erc::ReturnCodeValue::RETCODE_NOT_ALLOWED_BY_SECURITY) {
287  std::cerr << "RETCODE_NOT_ALLOWED_BY_SECURITY" << std::endl;
288  return false;
289  }
290  std::cerr << "UNKNOWN" << std::endl;
291  return false;
292  }
293 
294  void CarlaDepthCameraPublisher::SetImageData(int32_t seconds, uint32_t nanoseconds, size_t height, size_t width, const uint8_t* data) { std::vector<uint8_t> vector_data;
295  const size_t size = height * width * 4;
296  vector_data.resize(size);
297  std::memcpy(&vector_data[0], &data[0], size);
298  SetData(seconds, nanoseconds,height, width, std::move(vector_data));
299  }
300 
301  void CarlaDepthCameraPublisher::SetInfoRegionOfInterest( uint32_t x_offset, uint32_t y_offset, uint32_t height, uint32_t width, bool do_rectify) {
303  roi.x_offset(x_offset);
304  roi.y_offset(y_offset);
305  roi.height(height);
306  roi.width(width);
307  roi.do_rectify(do_rectify);
308  _impl_info->_info.roi(roi);
309  }
310 
311  void CarlaDepthCameraPublisher::SetData(int32_t seconds, uint32_t nanoseconds, size_t height, size_t width, std::vector<uint8_t>&& data) {
313  time.sec(seconds);
314  time.nanosec(nanoseconds);
315 
316  std_msgs::msg::Header header;
317  header.stamp(std::move(time));
318  header.frame_id(_frame_id);
319 
320  _impl->_image.header(std::move(header));
321  _impl->_image.width(width);
322  _impl->_image.height(height);
323  _impl->_image.encoding("bgra8");
324  _impl->_image.is_bigendian(0);
325  _impl->_image.step(_impl->_image.width() * sizeof(uint8_t) * 4);
326  _impl->_image.data(std::move(data)); //https://github.com/eProsima/Fast-DDS/issues/2330
327  }
328 
329  void CarlaDepthCameraPublisher::SetCameraInfoData(int32_t seconds, uint32_t nanoseconds) {
331  time.sec(seconds);
332  time.nanosec(nanoseconds);
333 
334  std_msgs::msg::Header header;
335  header.stamp(std::move(time));
336  header.frame_id(_frame_id);
337  _impl_info->_info.header(header);
338  }
339 
340  CarlaDepthCameraPublisher::CarlaDepthCameraPublisher(const char* ros_name, const char* parent) :
341  _impl(std::make_shared<CarlaDepthCameraPublisherImpl>()),
342  _impl_info(std::make_shared<CarlaCameraInfoPublisherImpl>()) {
343  _name = ros_name;
344  _parent = parent;
345  }
346 
348  if (!_impl)
349  return;
350 
351  if (_impl->_datawriter)
352  _impl->_publisher->delete_datawriter(_impl->_datawriter);
353 
354  if (_impl->_publisher)
355  _impl->_participant->delete_publisher(_impl->_publisher);
356 
357  if (_impl->_topic)
358  _impl->_participant->delete_topic(_impl->_topic);
359 
360  if (_impl->_participant)
361  efd::DomainParticipantFactory::get_instance()->delete_participant(_impl->_participant);
362 
363  if (!_impl_info)
364  return;
365 
366  if (_impl_info->_datawriter)
367  _impl_info->_publisher->delete_datawriter(_impl_info->_datawriter);
368 
369  if (_impl_info->_publisher)
370  _impl_info->_participant->delete_publisher(_impl_info->_publisher);
371 
372  if (_impl_info->_topic)
373  _impl_info->_participant->delete_topic(_impl_info->_topic);
374 
375  if (_impl_info->_participant)
376  efd::DomainParticipantFactory::get_instance()->delete_participant(_impl_info->_participant);
377  }
378 
380  _frame_id = other._frame_id;
381  _name = other._name;
382  _parent = other._parent;
383  _impl = other._impl;
384  _impl_info = other._impl_info;
385  }
386 
388  _frame_id = other._frame_id;
389  _name = other._name;
390  _parent = other._parent;
391  _impl = other._impl;
392  _impl_info = other._impl_info;
393 
394  return *this;
395  }
396 
398  _frame_id = std::move(other._frame_id);
399  _name = std::move(other._name);
400  _parent = std::move(other._parent);
401  _impl = std::move(other._impl);
402  _impl_info = std::move(other._impl_info);
403 
404  }
405 
407  _frame_id = std::move(other._frame_id);
408  _name = std::move(other._name);
409  _parent = std::move(other._parent);
410  _impl = std::move(other._impl);
411  _impl_info = std::move(other._impl_info);
412 
413  return *this;
414  }
415 }
416 }
void SetData(int32_t seconds, uint32_t nanoseconds, size_t height, size_t width, std::vector< uint8_t > &&data)
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. ...
Definition: Carla.cpp:133
This class represents the structure Header defined by the user in the IDL file.
Definition: Header.h:72
void SetCameraInfoData(int32_t seconds, uint32_t nanoseconds)
This class represents the structure CameraInfo defined by the user in the IDL file.
Definition: CameraInfo.h:73
This class represents the structure Image defined by the user in the IDL file.
eProsima_user_DllExport void stamp(const builtin_interfaces::msg::Time &_stamp)
This function copies the value in member stamp.
Definition: Header.cpp:131
eProsima_user_DllExport void width(uint32_t _width)
This function sets a value in member width.
CarlaDepthCameraPublisher & operator=(const CarlaDepthCameraPublisher &)
This class represents the TopicDataType of the type Image defined by the user in the IDL file...
void SetImageData(int32_t seconds, uint32_t nanoseconds, size_t height, size_t width, const uint8_t *data)
eProsima_user_DllExport void x_offset(uint32_t _x_offset)
This function sets a value in member x_offset.
eProsima_user_DllExport void sec(int32_t _sec)
This function sets a value in member sec.
Definition: Time.cpp:133
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.
Definition: Time.cpp:161
void SetInfoRegionOfInterest(uint32_t x_offset, uint32_t y_offset, uint32_t height, uint32_t width, bool do_rectify)
This class represents the structure RegionOfInterest defined by the user in the IDL file...
CarlaDepthCameraPublisher(const char *ros_name="", const char *parent="")
const std::string & parent() const
This class represents the structure Time defined by the user in the IDL file.
void InitInfoData(uint32_t x_offset, uint32_t y_offset, uint32_t height, uint32_t width, float fov, bool do_rectify)
eProsima_user_DllExport void height(uint32_t _height)
This function sets a value in member height.
std::shared_ptr< CarlaDepthCameraPublisherImpl > _impl
eProsima_user_DllExport void frame_id(const std::string &_frame_id)
This function copies the value in member frame_id.
Definition: Header.cpp:168
eProsima_user_DllExport void y_offset(uint32_t _y_offset)
This function sets a value in member y_offset.
std::shared_ptr< CarlaCameraInfoPublisherImpl > _impl_info
eProsima_user_DllExport void do_rectify(bool _do_rectify)
This function sets a value in member do_rectify.