12#include <sensor_msgs/image_encodings.hpp>
19std::
string yarp2RosPixelCode(
int code)
24 return sensor_msgs::image_encodings::BGR8;
26 return sensor_msgs::image_encodings::BGRA8;
28 return sensor_msgs::image_encodings::BAYER_BGGR16;
30 return sensor_msgs::image_encodings::BAYER_BGGR8;
32 return sensor_msgs::image_encodings::BAYER_GBRG16;
34 return sensor_msgs::image_encodings::BAYER_GBRG8;
36 return sensor_msgs::image_encodings::BAYER_GRBG16;
38 return sensor_msgs::image_encodings::BAYER_GRBG8;
40 return sensor_msgs::image_encodings::BAYER_RGGB16;
42 return sensor_msgs::image_encodings::BAYER_RGGB8;
44 return sensor_msgs::image_encodings::MONO8;
46 return sensor_msgs::image_encodings::MONO16;
48 return sensor_msgs::image_encodings::RGB8;
50 return sensor_msgs::image_encodings::RGBA8;
52 return sensor_msgs::image_encodings::TYPE_32FC1;
54 return sensor_msgs::image_encodings::RGB8;
60 PeriodicThread(s_default_period)
87 yCError(FRAMEGRABBER_NWS_ROS2,
"Device is already opened");
99 if(m_node ==
nullptr) {
100 yCError(FRAMEGRABBER_NWS_ROS2) <<
" opening " <<
m_node_name <<
" Node, check your yarp-ROS2 network configuration\n";
104 publisher_image = m_node->create_publisher<sensor_msgs::msg::Image>(
m_topic_name, 10);
109 publisher_cameraInfo = m_node->create_publisher<sensor_msgs::msg::CameraInfo>(cameraInfoTopicName, 10);
111 yCInfo(FRAMEGRABBER_NWS_ROS2) <<
"Running, waiting for attach...";
121 yCError(FRAMEGRABBER_NWS_ROS2) <<
"Device " << poly <<
" to attach to is not valid ... cannot proceed";
125 PeriodicThread::setPeriod(m_period);
127 poly->
view(iRgbVisualParams);
128 poly->
view(iFrameGrabberImage);
129 poly->
view(iPreciselyTimed);
131 if (iFrameGrabberImage ==
nullptr) {
132 yCError(FRAMEGRABBER_NWS_ROS2) <<
"IFrameGrabberImage interface is not available on the device";
136 if (iRgbVisualParams ==
nullptr) {
137 yCWarning(FRAMEGRABBER_NWS_ROS2) <<
"IRgbVisualParams interface is not available on the device";
140 return PeriodicThread::start();
150 iRgbVisualParams =
nullptr;
151 iFrameGrabberImage =
nullptr;
152 iPreciselyTimed =
nullptr;
178 if (iPreciselyTimed) {
184 if (iFrameGrabberImage)
186 if(publisher_image->get_subscription_count()>0){
187 if (iFrameGrabberImage->
getImage(*yarpimg))
189 sensor_msgs::msg::Image rosimg;
191 rosimg.width = yarpimg->
width();
192 rosimg.height = yarpimg->
height();
193 rosimg.encoding = yarp2RosPixelCode(yarpimg->
getPixelCode());
198 rosimg.is_bigendian = 0;
200 publisher_image->publish(rosimg);
204 yCError(FRAMEGRABBER_NWS_ROS2) <<
"Image not captured (getImage failed). Check hardware configuration.";
210 yCError(FRAMEGRABBER_NWS_ROS2) <<
"Invalid call to interface iFrameGrabberImage";
213 if (iRgbVisualParams && publisher_cameraInfo->get_subscription_count()>0)
215 sensor_msgs::msg::CameraInfo cameraInfo;
216 if (setCamInfo(cameraInfo)) {
218 publisher_cameraInfo->publish(cameraInfo);
223 yCError(FRAMEGRABBER_NWS_ROS2) <<
"Invalid call to interface iRgbVisualParams";
231 param(T& inVar, std::string inName) :
233 parname(
std::move(inName))
241bool FrameGrabber_nws_ros2::setCamInfo(sensor_msgs::msg::CameraInfo& cameraInfo)
245 yCErrorThreadOnce(FRAMEGRABBER_NWS_ROS2) <<
"Unable to get intrinsic param from rgb sensor!";
249 if (!camData.
check(
"distortionModel")) {
250 yCWarning(FRAMEGRABBER_NWS_ROS2) <<
"Missing distortion model";
254 std::string distModel = camData.
find(
"distortionModel").
asString();
255 if (distModel !=
"plumb_bob") {
256 yCError(FRAMEGRABBER_NWS_ROS2) <<
"Distortion model not supported";
271 std::vector<param<double>> parVector;
272 parVector.emplace_back(phyF,
"physFocalLength");
273 parVector.emplace_back(fx,
"focalLengthX");
274 parVector.emplace_back(fy,
"focalLengthY");
275 parVector.emplace_back(cx,
"principalPointX");
276 parVector.emplace_back(cy,
"principalPointY");
277 parVector.emplace_back(k1,
"k1");
278 parVector.emplace_back(k2,
"k2");
279 parVector.emplace_back(t1,
"t1");
280 parVector.emplace_back(t2,
"t2");
281 parVector.emplace_back(k3,
"k3");
283 for(
auto& par : parVector) {
284 if(!camData.
check(par.parname)) {
285 yCWarning(FRAMEGRABBER_NWS_ROS2) <<
"Driver has not the param:" << par.parname;
294 cameraInfo.width = iRgbVisualParams->
getRgbWidth();
296 cameraInfo.distortion_model = distModel;
298 cameraInfo.d.resize(5);
299 cameraInfo.d[0] = k1;
300 cameraInfo.d[1] = k2;
301 cameraInfo.d[2] = t1;
302 cameraInfo.d[3] = t2;
303 cameraInfo.d[4] = k3;
305 cameraInfo.k[0] = fx; cameraInfo.k[1] = 0; cameraInfo.k[2] = cx;
306 cameraInfo.k[3] = 0; cameraInfo.k[4] = fy; cameraInfo.k[5] = cy;
307 cameraInfo.k[6] = 0; cameraInfo.k[7] = 0; cameraInfo.k[8] = 1;
318 cameraInfo.r[0] = 1; cameraInfo.r[1] = 0; cameraInfo.r[2] = 0;
319 cameraInfo.r[3] = 0; cameraInfo.r[4] = 1; cameraInfo.r[5] = 0;
320 cameraInfo.r[6] = 0; cameraInfo.r[7] = 0; cameraInfo.r[8] = 1;
322 cameraInfo.p[0] = fx; cameraInfo.p[1] = 0; cameraInfo.p[2] = cx; cameraInfo.p[3] = 0;
323 cameraInfo.p[4] = 0; cameraInfo.p[5] = fy; cameraInfo.p[6] = cy; cameraInfo.p[7] = 0;
324 cameraInfo.p[8] = 0; cameraInfo.p[9] = 0; cameraInfo.p[10] = 1; cameraInfo.p[11] = 0;
326 cameraInfo.binning_x = cameraInfo.binning_y = 0;
327 cameraInfo.roi.height = cameraInfo.roi.width = cameraInfo.roi.x_offset = cameraInfo.roi.y_offset = 0;
328 cameraInfo.roi.do_rectify =
false;
@ 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
bool parseParams(const yarp::os::Searchable &config) override
Parse the DeviceDriver parameters.
void run() override
Loop function.
bool threadInit() override
Initialization method.
bool detach() override
Detach the object (you must have first called attach).
void threadRelease() override
Release method.
bool attach(yarp::dev::PolyDriver *poly) override
Attach to another object.
~FrameGrabber_nws_ros2() override
bool close() override
Close the DeviceDriver.
bool open(yarp::os::Searchable &config) override
Open the DeviceDriver.
static rclcpp::Node::SharedPtr createNode(std::string name)
bool view(T *&x)
Get an interface to the device driver.
virtual yarp::dev::ReturnValue getImage(ImageType &image)=0
Get an image from the frame grabber.
virtual yarp::os::Stamp getLastInputStamp()=0
Return the time stamp relative to the last acquisition.
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 isValid() const
Check if device is valid.
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.
void update()
Set the timestamp to the current time, and increment the sequence number (wrapping to 0 if the sequen...
virtual yarp::conf::float64_t asFloat64() const
Get 64-bit floating point value.
virtual std::string asString() const
Get string value.
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.
#define yCInfo(component,...)
#define yCError(component,...)
#define yCWarning(component,...)
#define YARP_LOG_COMPONENT(name,...)
#define yCErrorThreadOnce(component,...)
double now()
Return the current time in seconds, relative to an arbitrary starting point.