16#include <sensor_msgs/image_encodings.hpp>
18using namespace std::chrono_literals;
29const std::string colorTopicName_param =
"colorTopicName";
30const std::string depthTopicName_param =
"depthTopicName";
31const std::string depthInfoTopicName_param =
"depthInfoTopicName";
32const std::string colorInfoTopicName_param =
"colorInfoTopicName";
36std::string yarp2RosPixelCode(
int code)
41 return sensor_msgs::image_encodings::BGR8;
43 return sensor_msgs::image_encodings::BGRA8;
45 return sensor_msgs::image_encodings::BAYER_BGGR16;
47 return sensor_msgs::image_encodings::BAYER_BGGR8;
49 return sensor_msgs::image_encodings::BAYER_GBRG16;
51 return sensor_msgs::image_encodings::BAYER_GBRG8;
53 return sensor_msgs::image_encodings::BAYER_GRBG16;
55 return sensor_msgs::image_encodings::BAYER_GRBG8;
57 return sensor_msgs::image_encodings::BAYER_RGGB16;
59 return sensor_msgs::image_encodings::BAYER_RGGB8;
61 return sensor_msgs::image_encodings::MONO8;
63 return sensor_msgs::image_encodings::MONO16;
65 return sensor_msgs::image_encodings::RGB8;
67 return sensor_msgs::image_encodings::RGBA8;
69 return sensor_msgs::image_encodings::TYPE_32FC1;
71 return sensor_msgs::image_encodings::RGB8;
87 yCDebug(RGBDSENSOR_NWS_ROS2) <<
"Parameters are: " << config.
toString();
89 if(!fromConfig(config)) {
90 yCError(RGBDSENSOR_NWS_ROS2) <<
"Failed to open, check previous log for error messages.";
94 if(!initialize_ROS2(config)) {
95 yCError(RGBDSENSOR_NWS_ROS2) <<
"Error initializing ROS topic";
108 yCError(RGBDSENSOR_NWS_ROS2) <<
"node_name cannot begin with an initial /";
113 yCError(RGBDSENSOR_NWS_ROS2) <<
"depth_topic_name must begin with an initial /";
119 yCError(RGBDSENSOR_NWS_ROS2) <<
"color_topic_name must begin with an initial /";
139 yCError(RGBDSENSOR_NWS_ROS2) <<
"Failed to create node";
142 rosPublisher_color = m_node->create_publisher<sensor_msgs::msg::Image>(
m_color_topic_name, 10);
143 rosPublisher_depth = m_node->create_publisher<sensor_msgs::msg::Image>(
m_depth_topic_name, 10);
144 rosPublisher_colorCaminfo = m_node->create_publisher<sensor_msgs::msg::CameraInfo>(m_color_info_topic_name, 10);
145 rosPublisher_depthCaminfo = m_node->create_publisher<sensor_msgs::msg::CameraInfo>(m_depth_info_topic_name, 10);
147 std::string status_topic_name =
"/" +
m_node_name +
"/publisher_status";
148 m_rosPublisher_status = m_node->create_publisher<std_msgs::msg::UInt8>(status_topic_name, 10);
156 yCTrace(RGBDSENSOR_NWS_ROS2,
"Close");
169 if (sensor_p!=
nullptr) {
174 yCError(RGBDSENSOR_NWS_ROS2,
"Image not captured.. check hardware configuration");
181 yCInfo(RGBDSENSOR_NWS_ROS2) <<
"Device not ready, waiting...";
184 yCWarning(RGBDSENSOR_NWS_ROS2) <<
"Device is taking too long to start..";
189 yCError(RGBDSENSOR_NWS_ROS2,
"Sensor returned error");
192 yCError(RGBDSENSOR_NWS_ROS2,
"Sensor interface is not valid");
205 poly->
view(sensor_p);
209 if(sensor_p ==
nullptr)
211 yCError(RGBDSENSOR_NWS_ROS2) <<
"Attached device has no valid IRGBDSensor interface.";
215 if(fgCtrl ==
nullptr)
217 yCWarning(RGBDSENSOR_NWS_ROS2) <<
"Attached device has no valid IFrameGrabberControls interface.";
220 return PeriodicThread::start();
237bool RgbdSensor_nws_ros2::setCamInfo(sensor_msgs::msg::CameraInfo& cameraInfo,
238 const std::string& frame_id,
239 const SensorType& sensorType)
252 std::string distModel;
253 std::string currentSensor;
255 std::vector<param<double> > parVector;
258 currentSensor = sensorType == COLOR_SENSOR ?
"Rgb" :
"Depth";
263 yCError(RGBDSENSOR_NWS_ROS2) <<
"Unable to get intrinsic param from" << currentSensor <<
"sensor!";
267 if(!camData.
check(
"distortionModel"))
269 yCWarning(RGBDSENSOR_NWS_ROS2) <<
"Missing distortion model";
275 parVector.emplace_back(phyF,
"physFocalLength");
276 parVector.emplace_back(fx,
"focalLengthX");
277 parVector.emplace_back(fy,
"focalLengthY");
278 parVector.emplace_back(cx,
"principalPointX");
279 parVector.emplace_back(cy,
"principalPointY");
281 if (distModel !=
"none")
283 parVector.emplace_back(k1,
"k1");
284 parVector.emplace_back(k2,
"k2");
285 parVector.emplace_back(t1,
"t1");
286 parVector.emplace_back(t2,
"t2");
287 parVector.emplace_back(k3,
"k3");
290 for(
auto& par : parVector) {
291 if(!camData.
check(par.parname)) {
292 yCWarning(RGBDSENSOR_NWS_ROS2) <<
"Driver has not the param:" << par.parname;
298 cameraInfo.header.frame_id = frame_id;
301 cameraInfo.distortion_model = distModel;
303 if (distModel !=
"none")
305 cameraInfo.d.resize(5);
306 cameraInfo.d[0] = k1;
307 cameraInfo.d[1] = k2;
308 cameraInfo.d[2] = t1;
309 cameraInfo.d[3] = t2;
310 cameraInfo.d[4] = k3;
313 cameraInfo.k[0] = fx; cameraInfo.k[1] = 0; cameraInfo.k[2] = cx;
314 cameraInfo.k[3] = 0; cameraInfo.k[4] = fy; cameraInfo.k[5] = cy;
315 cameraInfo.k[6] = 0; cameraInfo.k[7] = 0; cameraInfo.k[8] = 1;
326 cameraInfo.r[0] = 1; cameraInfo.r[1] = 0; cameraInfo.r[2] = 0;
327 cameraInfo.r[3] = 0; cameraInfo.r[4] = 1; cameraInfo.r[5] = 0;
328 cameraInfo.r[6] = 0; cameraInfo.r[7] = 0; cameraInfo.r[8] = 1;
330 cameraInfo.p[0] = fx; cameraInfo.p[1] = 0; cameraInfo.p[2] = cx; cameraInfo.p[3] = 0;
331 cameraInfo.p[4] = 0; cameraInfo.p[5] = fy; cameraInfo.p[6] = cy; cameraInfo.p[7] = 0;
332 cameraInfo.p[8] = 0; cameraInfo.p[9] = 0; cameraInfo.p[10] = 1; cameraInfo.p[11] = 0;
334 cameraInfo.binning_x = cameraInfo.binning_y = 0;
335 cameraInfo.roi.height = cameraInfo.roi.width = cameraInfo.roi.x_offset = cameraInfo.roi.y_offset = 0;
336 cameraInfo.roi.do_rectify =
false;
341bool RgbdSensor_nws_ros2::writeData()
347 builtin_interfaces::msg::Time ros2Stamp;
355 bool rgb_data_ok =
true;
356 bool depth_data_ok =
true;
358 if (((colorStamp.
getTime() - oldColorStamp.
getTime()) > 0) ==
false) {
362 oldColorStamp = colorStamp;
365 if (((depthStamp.
getTime() - oldDepthStamp.
getTime()) > 0) ==
false) {
369 oldDepthStamp = depthStamp;
381 ros2Stamp.sec =
static_cast<int>(colorStamp.
getTime());
382 ros2Stamp.nanosec =
static_cast<int>(1000000000UL * (colorStamp.
getTime() - int(colorStamp.
getTime())));
383 if(rosPublisher_color->get_subscription_count() > 0)
385 sensor_msgs::msg::Image rColorImage;
387 rColorImage.width = colorImage.
width();
388 rColorImage.height = colorImage.
height();
390 rColorImage.encoding = yarp2RosPixelCode(colorImage.
getPixelCode());
393 rColorImage.header.stamp = ros2Stamp;
394 rColorImage.is_bigendian = 0;
396 rosPublisher_color->publish(rColorImage);
399 if (m_forceInfoSync) {
402 if(rosPublisher_colorCaminfo->get_subscription_count() > 0)
404 rosPublisher_colorCaminfo->publish(m_camInfoData.
colorCamInfo);
410 ros2Stamp.sec =
static_cast<int>(depthStamp.
getTime());
411 ros2Stamp.nanosec =
static_cast<int>(1000000000UL * (depthStamp.
getTime() - int(depthStamp.
getTime())));
412 if(rosPublisher_depth->get_subscription_count() > 0)
414 sensor_msgs::msg::Image rDepthImage;
422 rDepthImage.header.stamp = ros2Stamp;
423 rDepthImage.is_bigendian = 0;
425 rosPublisher_depth->publish(rDepthImage);
428 if (m_forceInfoSync) {
431 if(rosPublisher_depthCaminfo->get_subscription_count() > 0)
433 rosPublisher_depthCaminfo->publish(m_camInfoData.
depthCamInfo);
438 std_msgs::msg::UInt8 status_msg;
439 status_msg.data = rgb_data_ok*1 + depth_data_ok*2;
440 if(m_rosPublisher_status->get_subscription_count() > 0)
441 m_rosPublisher_status->publish(status_msg);
@ VOCAB_PIXEL_ENCODING_BAYER_BGGR16
@ VOCAB_PIXEL_ENCODING_BAYER_BGGR8
@ VOCAB_PIXEL_ENCODING_BAYER_RGGB8
@ VOCAB_PIXEL_ENCODING_BAYER_GRBG8
@ VOCAB_PIXEL_ENCODING_BAYER_GBRG16
@ VOCAB_PIXEL_ENCODING_BAYER_GRBG16
@ VOCAB_PIXEL_ENCODING_BAYER_GBRG8
@ VOCAB_PIXEL_ENCODING_BAYER_RGGB16
static rclcpp::Node::SharedPtr createNode(std::string name)
std::string m_depth_topic_name
std::string m_color_topic_name
std::string m_color_frame_id
bool parseParams(const yarp::os::Searchable &config) override
Parse the DeviceDriver parameters.
std::string m_depth_frame_id
bool detach() override
Detach the object (you must have first called attach).
bool attach(yarp::dev::PolyDriver *poly) override
Attach to another object.
bool open(yarp::os::Searchable &config) override
Open the DeviceDriver.
void run() override
Loop function.
bool close() override
Close the DeviceDriver.
bool view(T *&x)
Get an interface to the device driver.
virtual int getDepthWidth()=0
Return the height of each frame.
virtual yarp::dev::ReturnValue getDepthIntrinsicParam(yarp::os::Property &intrinsic)=0
Get the intrinsic parameters of the depth camera.
virtual int getDepthHeight()=0
Return the height of each frame.
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.
virtual int getRgbHeight()=0
Return the height of each frame.
virtual int getRgbWidth()=0
Return the width of each frame.
A container for a device driver.
bool isRunning() const
Returns true when the thread is started, false otherwise.
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.
Value & find(const std::string &key) const override
Gets a value corresponding to a given keyword.
bool check(const std::string &key) const override
Check if there exists a property of the given name.
A base class for nested structures that can be searched.
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.
double getTime() const
Get the time stamp.
virtual yarp::conf::float64_t asFloat64() const
Get 64-bit floating point value.
virtual std::string asString() const
Get string value.
Image class with user control of representation details.
int getPixelCode() const override
Gets pixel type identifier.
size_t width() const
Gets width of image in pixels.
size_t getRowSize() const
Size of the underlying image buffer rows.
unsigned char * getRawImage() const
Access to the internal image buffer.
size_t getRawImageSize() const
Access to the internal buffer size information (this is how much memory has been allocated for the im...
size_t height() const
Gets height of image in pixels.
virtual int getPixelCode() const
Gets pixel type identifier.
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,...)
const std::string frameId_param
const std::string nodeName_param
The main, catch-all namespace for YARP.
sensor_msgs::msg::CameraInfo depthCamInfo
sensor_msgs::msg::CameraInfo colorCamInfo