YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
RgbdToPointCloudSensor_nws_ros2.cpp
Go to the documentation of this file.
1/*
2 * SPDX-FileCopyrightText: 2023 Istituto Italiano di Tecnologia (IIT)
3 * SPDX-License-Identifier: BSD-3-Clause
4 */
5
7
9#include <yarp/os/LogStream.h>
10
12
13#include <rclcpp/time.hpp>
14#include <sensor_msgs/image_encodings.hpp>
15#include <string>
16#include <vector>
17#include <Ros2Utils.h>
18
19using namespace std::chrono_literals;
20using namespace std;
21using namespace yarp::sig;
22using namespace yarp::dev;
23using namespace yarp::os;
24using namespace RGBDToPointCloudRos2Impl;
26
31
32
33// DeviceDriver
35{
36 yCDebug(RGBDTOPOINTCLOUDSENSOR_NWS_ROS2) << "Parameters are: " << config.toString();
37
38 if(!fromConfig(config)) {
39 yCError(RGBDTOPOINTCLOUDSENSOR_NWS_ROS2) << "Failed to open, check previous log for error messages.";
40 return false;
41 }
42
43 if(!initialize_ROS2(config)) {
44 yCError(RGBDTOPOINTCLOUDSENSOR_NWS_ROS2) << "Error initializing ROS topic";
45 return false;
46 }
47
48 return true;
49}
50
51
52bool RgbdToPointCloudSensor_nws_ros2::fromConfig(yarp::os::Searchable &config)
53{
54 parseParams(config);
56
57 return true;
58}
59
60
61bool RgbdToPointCloudSensor_nws_ros2::initialize_ROS2(yarp::os::Searchable &params)
62{
63
64 if(m_namespace.empty()) {
66 }
67 else {
69 }
70 if(!m_node) {
71 yCError(RGBDTOPOINTCLOUDSENSOR_NWS_ROS2) << "Failed to create node";
72 return false;
73 }
74 m_rosPublisher_pointCloud2 = m_node->create_publisher<sensor_msgs::msg::PointCloud2>(m_topic_name, 10);
75 return true;
76}
77
78
86
87
88// PeriodicThread
90{
91 if (m_sensor_p!=nullptr) {
92 static int i = 0;
93 switch (m_sensor_p->getSensorStatus()) {
95 if (!writeData()) {
96 yCError(RGBDTOPOINTCLOUDSENSOR_NWS_ROS2, "Image not captured.. check hardware configuration");
97 }
98 i = 0;
99 break;
101 if(i < 1000) {
102 if((i % 30) == 0) {
103 yCInfo(RGBDTOPOINTCLOUDSENSOR_NWS_ROS2) << "Device not ready, waiting...";
104 }
105 } else {
106 yCWarning(RGBDTOPOINTCLOUDSENSOR_NWS_ROS2) << "Device is taking too long to start..";
107 }
108 i++;
109 break;
110 default:
111 yCError(RGBDTOPOINTCLOUDSENSOR_NWS_ROS2, "Sensor returned error");
112 }
113 } else {
114 yCError(RGBDTOPOINTCLOUDSENSOR_NWS_ROS2, "Sensor interface is not valid");
115 }
116}
117
118
119bool RgbdToPointCloudSensor_nws_ros2::attach(yarp::dev::PolyDriver* poly)
120{
121 if(poly)
122 {
123 poly->view(m_sensor_p);
124 poly->view(m_fgCtrl);
125 }
126
127 if(m_sensor_p == nullptr)
128 {
129 yCError(RGBDTOPOINTCLOUDSENSOR_NWS_ROS2) << "Attached device has no valid IRGBDSensor interface.";
130 return false;
131 }
132
133 if(m_fgCtrl == nullptr)
134 {
135 yCWarning(RGBDTOPOINTCLOUDSENSOR_NWS_ROS2) << "Attached device has no valid IFrameGrabberControls interface.";
136 }
137
138 return PeriodicThread::start();
139}
140
141
143{
146
147 m_sensor_p = nullptr;
148 if (m_fgCtrl)
149 {
150 m_fgCtrl = nullptr;
151 }
152 return true;
153}
154
155bool RgbdToPointCloudSensor_nws_ros2::writeData()
156{
157 yarp::sig::FlexImage colorImage;
159 yarp::os::Stamp colorStamp;
160 yarp::os::Stamp depthStamp;
161
162 if (!m_sensor_p->getImages(colorImage, depthImage, &colorStamp, &depthStamp)) {
163 return false;
164 }
165
166 static Stamp oldColorStamp = Stamp(0, 0);
167 static Stamp oldDepthStamp = Stamp(0, 0);
169 bool rgb_data_ok = true;
170 bool depth_data_ok = true;
171 bool intrinsic_ok = false;
172
173 if ((colorStamp.getTime() - oldColorStamp.getTime()) <= 0) {
174 rgb_data_ok = false;
175 } else {
176 oldColorStamp = colorStamp;
177 }
178
179 if ((depthStamp.getTime() - oldDepthStamp.getTime()) <= 0) {
180 depth_data_ok = false;
181 } else {
182 oldDepthStamp = depthStamp;
183 }
185
186
187 // TBD: We should check here somehow if the timestamp was correctly updated and, if not, update it ourselves.
188 if (rgb_data_ok && m_rosPublisher_pointCloud2->get_subscription_count() > 0) {
189 if (depth_data_ok) {
190 if (intrinsic_ok) {
193 colorImagePixelRGB.setExternal(colorImage.getRawImage(), colorImage.width(), colorImage.height());
194 // create point cloud in yarp format
196 yarp::sig::PixelRgb>(depthImage,
200 sensor_msgs::msg::PointCloud2 pc2Ros;
201
202 // filling ros header
203 pc2Ros.header.frame_id = m_frame_id;
204
205 //pc2Ros.header.stamp.sec = depthStamp.;
206 pc2Ros.header.stamp.sec = int(depthStamp.getTime());
207 pc2Ros.header.stamp.nanosec = int(1000000000UL * (depthStamp.getTime() - int(depthStamp.getTime())));
208
209 // filling ros point field
210 pc2Ros.fields.push_back(sensor_msgs::msg::PointField());
211 pc2Ros.fields.push_back(sensor_msgs::msg::PointField());
212 pc2Ros.fields.push_back(sensor_msgs::msg::PointField());
213 pc2Ros.fields.push_back(sensor_msgs::msg::PointField());
214 pc2Ros.fields[0].name = "x";
215 pc2Ros.fields[0].offset = 0;
216 pc2Ros.fields[0].datatype = 7;
217 pc2Ros.fields[0].count = 1;
218 pc2Ros.fields[1].name = "y";
219 pc2Ros.fields[1].offset = 4;
220 pc2Ros.fields[1].datatype = 7;
221 pc2Ros.fields[1].count = 1;
222 pc2Ros.fields[2].name = "z";
223 pc2Ros.fields[2].offset = 8;
224 pc2Ros.fields[2].datatype = 7;
225 pc2Ros.fields[2].count = 1;
226 pc2Ros.fields[3].name = "rgb";
227 pc2Ros.fields[3].offset = 16;
228 pc2Ros.fields[3].datatype = 7;
229 pc2Ros.fields[3].count = 1;
230
231 std::vector<unsigned char> vec(yarpCloud.getRawData(), yarpCloud.getRawData() + yarpCloud.dataSizeBytes());
232 pc2Ros.data = vec;
233 pc2Ros.width = yarpCloud.width() * yarpCloud.height();
234 pc2Ros.height = 1;
235 pc2Ros.is_dense = yarpCloud.isDense();
236
237 pc2Ros.point_step = sizeof(yarp::sig::DataXYZRGBA);
238 pc2Ros.row_step = static_cast<std::uint32_t>(sizeof(yarp::sig::DataXYZRGBA) * pc2Ros.width);
239 m_rosPublisher_pointCloud2->publish(pc2Ros);
240 }
241 }
242 }
243
244 nodeSeq++;
245
246 return true;
247}
const yarp::os::LogComponent & RGBDTOPOINTCLOUDSENSOR_NWS_ROS2()
static rclcpp::Node::SharedPtr createNode(std::string name)
Definition Ros2Utils.cpp:9
bool parseParams(const yarp::os::Searchable &config) override
Parse the DeviceDriver parameters.
bool close() override
Close the DeviceDriver.
bool detach() override
Detach the object (you must have first called attach).
bool open(yarp::os::Searchable &config) override
Open the DeviceDriver.
bool view(T *&x)
Get an interface to the device driver.
virtual yarp::dev::ReturnValue getImages(yarp::sig::FlexImage &colorFrame, yarp::sig::ImageOf< yarp::sig::PixelFloat > &depthFrame, yarp::os::Stamp *colorStamp=nullptr, yarp::os::Stamp *depthStamp=nullptr)=0
Get the both the color and depth frame in a single call.
virtual yarp::dev::ReturnValue getSensorStatus(RGBDSensor_status &status)=0
Get the current status of the sensor, using enum type.
virtual yarp::dev::ReturnValue getRgbIntrinsicParam(yarp::os::Property &intrinsic)=0
Get the intrinsic parameters of the rgb camera.
A container for a device driver.
Definition PolyDriver.h:23
bool detachAll() final
Detach the object (you must have first called attach).
A mini-server for performing network communication in the background.
@ TraceType
Definition Log.h:92
An abstraction for a periodic thread.
bool setPeriod(double period)
Set the (new) period of the thread.
bool isRunning() const
Returns true when the thread is started, false otherwise.
bool start()
Call this to start the thread.
void stop()
Call this to stop the thread, this call blocks until the thread is terminated (and releaseThread() ca...
A class for storing options and configuration information.
Definition Property.h:33
A base class for nested structures that can be searched.
Definition Searchable.h:31
virtual std::string toString() const =0
Return a standard text representation of the content of the object.
An abstraction for a time stamp and/or sequence number.
Definition Stamp.h:21
double getTime() const
Get the time stamp.
Definition Stamp.cpp:34
Image class with user control of representation details.
Definition Image.h:361
Typed image class.
Definition Image.h:603
size_t width() const
Gets width of image in pixels.
Definition Image.h:171
unsigned char * getRawImage() const
Access to the internal image buffer.
Definition Image.cpp:462
size_t height() const
Gets height of image in pixels.
Definition Image.h:177
The PointCloud class.
Definition PointCloud.h:21
yarp::sig::ImageOf< yarp::sig::PixelFloat > depthImage
This class is an utility.
#define yCInfo(component,...)
#define yCError(component,...)
#define yCTrace(component,...)
#define yCWarning(component,...)
#define yCDebug(component,...)
#define YARP_LOG_COMPONENT(name,...)
STL namespace.
For streams capable of holding different kinds of content, check what they actually have.
An interface to the operating system, including Port based communication.
Definition OSTestMsg.cpp:13
yarp::sig::PointCloud< T1 > depthRgbToPC(const yarp::sig::ImageOf< yarp::sig::PixelFloat > &depth, const yarp::sig::ImageOf< T2 > &color, const yarp::sig::IntrinsicParams &intrinsic, const yarp::sig::utils::OrganizationType organizationType=yarp::sig::utils::OrganizationType::Organized, size_t step_x=1, size_t step_y=1, const std::string &output_order="+X+Y+Z")
depthRgbToPC, compute the colored PointCloud given depth image, color image and the intrinsic paramet...
The main, catch-all namespace for YARP.
Definition dirs.h:16
The IntrinsicParams struct to handle the intrinsic parameter of cameras(RGB and RGBD either).