6#ifndef _USE_MATH_DEFINES
7#define _USE_MATH_DEFINES
20using namespace std::chrono_literals;
42 driver->
view(m_iLidar);
51 if (
nullptr == m_iLidar)
76 m_isDeviceReady =
true;
92 if (!m_isDeviceReady)
return;
94 auto message = std_msgs::msg::String();
95 sensor_msgs::msg::LaserScan
rosData;
97 if (m_iLidar!=
nullptr && m_publisher_laser->get_subscription_count() > 0)
125 rosData.angle_increment = m_resolution *
M_PI / 180.0;
128 rosData.range_min = m_minDistance;
129 rosData.range_max = m_maxDistance;
137 if (std::isnan(
ranges[i]))
139 rosData.ranges[i] = std::numeric_limits<double>::infinity();
148 m_publisher_laser->publish(
rosData);
165 for (
size_t i = 0; i < subdevice_joints; i++) {
173 m_ros_struct.name = jointNames;
174 m_ros_struct.header.stamp.sec =
rosData.header.stamp.sec;
175 m_ros_struct.header.stamp.nanosec =
rosData.header.stamp.nanosec;
176 if(m_publisher_joint->get_subscription_count() > 0)
178 m_publisher_joint->publish(m_ros_struct);
187 m_driver_cb = driver;
189 m_driver_cb->
view(iPositionControl);
190 if (!iPositionControl) {
195 m_driver_cb->
view(iEncodersTimed);
196 if (!iEncodersTimed) {
201 m_driver_cb->
view(iTorqueControl);
202 if (!iTorqueControl) {
206 m_driver_cb->
view(iAxisInfo);
223 subdevice_joints =
static_cast<size_t>(
tmp_axes);
225 m_times.
resize(subdevice_joints);
226 m_ros_struct.name.resize(subdevice_joints);
227 m_ros_struct.position.resize(subdevice_joints);
228 m_ros_struct.velocity.resize(subdevice_joints);
229 m_ros_struct.effort.resize(subdevice_joints);
235 m_isDeviceReady =
true;
248 if( m_node ==
nullptr) {
252 m_publisher_laser = m_node->create_publisher<sensor_msgs::msg::LaserScan>(
m_topic_lidar, 10);
253 m_publisher_joint = m_node->create_publisher<sensor_msgs::msg::JointState>(
m_topic_joint, 10);
276 std::vector<std::string>
tmpVect;
277 for (
size_t i = 0; i < subdevice_joints; i++) {
double convertDegreesToRadians(double degrees)
const yarp::os::LogComponent & RANGEFINDER2D_NWS_ROS2()
static rclcpp::Node::SharedPtr createNode(std::string name)
std::string m_topic_joint
bool parseParams(const yarp::os::Searchable &config) override
Parse the DeviceDriver parameters.
std::string m_topic_lidar
bool detach() override
Detach the object (you must have first called attach).
bool close() override
Close the DeviceDriver.
void run() override
Loop function.
bool attach(yarp::dev::PolyDriver *driver) override
Attach to another object.
Rangefinder2D_controlBoard_nws_ros2()
bool setDevice(yarp::dev::DeviceDriver *device)
bool open(yarp::os::Searchable &config) override
Open the DeviceDriver.
Interface implemented by all device drivers.
bool view(T *&x)
Get an interface to the device driver.
virtual yarp::dev::ReturnValue getJointType(int axis, yarp::dev::JointTypeEnum &type)
virtual yarp::dev::ReturnValue getAxisName(int axis, std::string &name)=0
virtual yarp::dev::ReturnValue getEncodersTimed(double *encs, double *time)=0
Read the instantaneous acceleration of all axes.
virtual yarp::dev::ReturnValue getAxes(size_t &ax)=0
Get the number of controlled axes.
virtual ReturnValue getRawData(yarp::sig::Vector &data, double *timestamp=nullptr)=0
Get the device measurements.
virtual ReturnValue getDistanceRange(double &min, double &max)=0
get the device detection range
virtual ReturnValue getScanLimits(double &min, double &max)=0
get the scan angular range.
virtual ReturnValue getHorizontalResolution(double &step)=0
get the angular step between two measurements.
virtual ReturnValue getDeviceStatus(Device_status &status)=0
get the device status
A container for a device driver.
bool isValid() const
Check if device is valid.
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.
double getPeriod() const
Return the current period of the thread.
void stop()
Call this to stop the thread, this call blocks until the thread is terminated (and releaseThread() ca...
A base class for nested structures that can be searched.
Scalar * data()
Return a pointer to the first element of the vector.
void resize(size_t size)
Resize the vector.
#define yCInfo(component,...)
#define yCError(component,...)
#define yCAssert(component, x)
#define yCWarning(component,...)
#define YARP_LOG_COMPONENT(name,...)
For streams capable of holding different kinds of content, check what they actually have.
@ VOCAB_JOINTTYPE_REVOLUTE
double now()
Return the current time in seconds, relative to an arbitrary starting point.
An interface to the operating system, including Port based communication.
The main, catch-all namespace for YARP.