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 if (!config.check("period", "refresh period of the broadcasted values in s")) {
55 yCDebug(RGBDTOPOINTCLOUDSENSOR_NWS_ROS2) << "Using default 'period' parameter of " << DEFAULT_THREAD_PERIOD << "s";
56 } else {
57 setPeriod(config.find("period").asFloat64());
58 }
59
60 //check if param exist and assign it to corresponding variable.. if it doesn't, initialize the variable with default value.
61 std::vector<param<std::string>> rosStringParam;
62
63 rosStringParam.emplace_back(m_nodeName, nodeName_param );
64 rosStringParam.emplace_back(m_rosFrameId, frameId_param );
65 rosStringParam.emplace_back(m_pointCloudTopicName, pointCloudTopicName_param );
66
67 for (auto &prm : rosStringParam) {
68 if (!config.check(prm.parname)) {
69 yCError(RGBDTOPOINTCLOUDSENSOR_NWS_ROS2) << "Missing " << prm.parname << "check your configuration file";
70 return false;
71 }
72 *(prm.var) = config.find(prm.parname).asString();
73 }
74
75 if (config.check("m_forceInfoSync"))
76 {
77 m_forceInfoSync = config.find("m_forceInfoSync").asBool();
78 }
79
80 return true;
81}
82
83
84bool RgbdToPointCloudSensor_nws_ros2::initialize_ROS2(yarp::os::Searchable &params)
85{
86
87 m_node = NodeCreator::createNode(m_nodeName);
88 m_rosPublisher_pointCloud2 = m_node->create_publisher<sensor_msgs::msg::PointCloud2>(m_pointCloudTopicName, 10);
89 return true;
90}
91
92
100
101
102// PeriodicThread
104{
105 if (m_sensor_p!=nullptr) {
106 static int i = 0;
107 switch (m_sensor_p->getSensorStatus()) {
109 if (!writeData()) {
110 yCError(RGBDTOPOINTCLOUDSENSOR_NWS_ROS2, "Image not captured.. check hardware configuration");
111 }
112 i = 0;
113 break;
115 if(i < 1000) {
116 if((i % 30) == 0) {
117 yCInfo(RGBDTOPOINTCLOUDSENSOR_NWS_ROS2) << "Device not ready, waiting...";
118 }
119 } else {
120 yCWarning(RGBDTOPOINTCLOUDSENSOR_NWS_ROS2) << "Device is taking too long to start..";
121 }
122 i++;
123 break;
124 default:
125 yCError(RGBDTOPOINTCLOUDSENSOR_NWS_ROS2, "Sensor returned error");
126 }
127 } else {
128 yCError(RGBDTOPOINTCLOUDSENSOR_NWS_ROS2, "Sensor interface is not valid");
129 }
130}
131
132
133bool RgbdToPointCloudSensor_nws_ros2::attach(yarp::dev::PolyDriver* poly)
134{
135 if(poly)
136 {
137 poly->view(m_sensor_p);
138 poly->view(m_fgCtrl);
139 }
140
141 if(m_sensor_p == nullptr)
142 {
143 yCError(RGBDTOPOINTCLOUDSENSOR_NWS_ROS2) << "Attached device has no valid IRGBDSensor interface.";
144 return false;
145 }
146
147 if(m_fgCtrl == nullptr)
148 {
149 yCWarning(RGBDTOPOINTCLOUDSENSOR_NWS_ROS2) << "Attached device has no valid IFrameGrabberControls interface.";
150 }
151
152 return PeriodicThread::start();
153}
154
155
157{
160
161 m_sensor_p = nullptr;
162 if (m_fgCtrl)
163 {
164 m_fgCtrl = nullptr;
165 }
166 return true;
167}
168
169bool RgbdToPointCloudSensor_nws_ros2::writeData()
170{
171 yarp::sig::FlexImage colorImage;
173 yarp::os::Stamp colorStamp;
174 yarp::os::Stamp depthStamp;
175
176 if (!m_sensor_p->getImages(colorImage, depthImage, &colorStamp, &depthStamp)) {
177 return false;
178 }
179
180 static Stamp oldColorStamp = Stamp(0, 0);
181 static Stamp oldDepthStamp = Stamp(0, 0);
183 bool rgb_data_ok = true;
184 bool depth_data_ok = true;
185 bool intrinsic_ok = false;
186
187 if ((colorStamp.getTime() - oldColorStamp.getTime()) <= 0) {
188 rgb_data_ok = false;
189 } else {
190 oldColorStamp = colorStamp;
191 }
192
193 if ((depthStamp.getTime() - oldDepthStamp.getTime()) <= 0) {
194 depth_data_ok = false;
195 } else {
196 oldDepthStamp = depthStamp;
197 }
199
200
201 // TBD: We should check here somehow if the timestamp was correctly updated and, if not, update it ourselves.
202 if (rgb_data_ok) {
203 if (depth_data_ok) {
204 if (intrinsic_ok) {
207 colorImagePixelRGB.setExternal(colorImage.getRawImage(), colorImage.width(), colorImage.height());
208 // create point cloud in yarp format
210 yarp::sig::PixelRgb>(depthImage,
214 sensor_msgs::msg::PointCloud2 pc2Ros;
215
216 // filling ros header
217 pc2Ros.header.frame_id = m_rosFrameId;
218
219 //pc2Ros.header.stamp.sec = depthStamp.;
220 pc2Ros.header.stamp.sec = int(depthStamp.getTime());
221 pc2Ros.header.stamp.nanosec = int(1000000000UL * (depthStamp.getTime() - int(depthStamp.getTime())));
222
223 // filling ros point field
224 pc2Ros.fields.push_back(sensor_msgs::msg::PointField());
225 pc2Ros.fields.push_back(sensor_msgs::msg::PointField());
226 pc2Ros.fields.push_back(sensor_msgs::msg::PointField());
227 pc2Ros.fields.push_back(sensor_msgs::msg::PointField());
228 pc2Ros.fields[0].name = "x";
229 pc2Ros.fields[0].offset = 0;
230 pc2Ros.fields[0].datatype = 7;
231 pc2Ros.fields[0].count = 1;
232 pc2Ros.fields[1].name = "y";
233 pc2Ros.fields[1].offset = 4;
234 pc2Ros.fields[1].datatype = 7;
235 pc2Ros.fields[1].count = 1;
236 pc2Ros.fields[2].name = "z";
237 pc2Ros.fields[2].offset = 8;
238 pc2Ros.fields[2].datatype = 7;
239 pc2Ros.fields[2].count = 1;
240 pc2Ros.fields[3].name = "rgb";
241 pc2Ros.fields[3].offset = 16;
242 pc2Ros.fields[3].datatype = 7;
243 pc2Ros.fields[3].count = 1;
244
245 std::vector<unsigned char> vec(yarpCloud.getRawData(), yarpCloud.getRawData() + yarpCloud.dataSizeBytes());
246 pc2Ros.data = vec;
247 pc2Ros.width = yarpCloud.width() * yarpCloud.height();
248 pc2Ros.height = 1;
249 pc2Ros.is_dense = yarpCloud.isDense();
250
251 pc2Ros.point_step = sizeof(yarp::sig::DataXYZRGBA);
252 pc2Ros.row_step = static_cast<std::uint32_t>(sizeof(yarp::sig::DataXYZRGBA) * pc2Ros.width);
253 m_rosPublisher_pointCloud2->publish(pc2Ros);
254 }
255 }
256 }
257
258 nodeSeq++;
259
260 return true;
261}
#define DEFAULT_THREAD_PERIOD
yarp::sig::ImageOf< yarp::sig::PixelFloat > depthImage
const yarp::os::LogComponent & RGBDTOPOINTCLOUDSENSOR_NWS_ROS2()
static rclcpp::Node::SharedPtr createNode(std::string name)
Definition Ros2Utils.cpp:9
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.
bool getRgbIntrinsicParam(yarp::os::Property &intrinsic) override=0
Get the intrinsic parameters of the rgb camera.
virtual bool 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 RGBDSensor_status getSensorStatus()=0
Get the surrent status of the sensor, using enum type.
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:56
virtual bool check(const std::string &key) const =0
Check if there exists a property of the given name.
virtual std::string toString() const =0
Return a standard text representation of the content of the object.
virtual Value & find(const std::string &key) const =0
Gets a value corresponding to a given keyword.
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:374
Typed image class.
Definition Image.h:616
size_t width() const
Gets width of image in pixels.
Definition Image.h:163
unsigned char * getRawImage() const
Access to the internal image buffer.
Definition Image.cpp:542
size_t height() const
Gets height of image in pixels.
Definition Image.h:169
The PointCloud class.
Definition PointCloud.h:22
#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.
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)
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).