13#include <rclcpp/time.hpp>
14#include <sensor_msgs/image_encodings.hpp>
19using namespace std::chrono_literals;
38 if(!fromConfig(config)) {
43 if(!initialize_ROS2(config)) {
74 m_rosPublisher_pointCloud2 = m_node->create_publisher<sensor_msgs::msg::PointCloud2>(
m_topic_name, 10);
91 if (m_sensor_p!=
nullptr) {
123 poly->
view(m_sensor_p);
124 poly->
view(m_fgCtrl);
127 if(m_sensor_p ==
nullptr)
133 if(m_fgCtrl ==
nullptr)
147 m_sensor_p =
nullptr;
155bool RgbdToPointCloudSensor_nws_ros2::writeData()
188 if (
rgb_data_ok && m_rosPublisher_pointCloud2->get_subscription_count() > 0) {
200 sensor_msgs::msg::PointCloud2
pc2Ros;
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;
239 m_rosPublisher_pointCloud2->publish(
pc2Ros);
const yarp::os::LogComponent & RGBDTOPOINTCLOUDSENSOR_NWS_ROS2()
static rclcpp::Node::SharedPtr createNode(std::string name)
bool parseParams(const yarp::os::Searchable &config) override
Parse the DeviceDriver parameters.
RgbdToPointCloudSensor_nws_ros2()
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.
void run() override
Loop function.
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.
bool detachAll() final
Detach the object (you must have first called attach).
A mini-server for performing network communication in the background.
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.
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.
Image class with user control of representation details.
size_t width() const
Gets width of image in pixels.
unsigned char * getRawImage() const
Access to the internal image buffer.
size_t height() const
Gets height of image in pixels.
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,...)
constexpr double DEFAULT_THREAD_PERIOD
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, 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.
The IntrinsicParams struct to handle the intrinsic parameter of cameras(RGB and RGBD either).