6#define _USE_MATH_DEFINES
27#define DEG2RAD M_PI/180.0
31#define RAD2DEG 180/M_PI
70 m_contains_data=
false;
79 m_port_mutex.unlock();
86 while (m_contains_data==
false)
95 m_port_mutex.unlock();
103 m_info =
"LaserFromExternalPort device";
117 if (general_config.
check(
"input_ports_name"))
122 for (
size_t i = 0; i < portlist->
size(); i++) {
140 if (general_config.
check(
"base_type"))
142 std::string bt = general_config.
find(
"base_type").
asString();
176 if (general_config.
check(
"override"))
190 if (config.
check(
"TRANSFORMS") && config.
check(
"TRANSFORM_CLIENT"))
201 for (
size_t i = 0; i < src_frames_list->
size(); i++)
220 if (client_cfg_string==
"")
228 tcprop.
put(
"device",
"transformClient");
254 PeriodicThread::start();
262 PeriodicThread::stop();
277 std::lock_guard<std::mutex> guard(
m_mutex);
285 std::lock_guard<std::mutex> guard(
m_mutex);
294 std::lock_guard<std::mutex> guard(
m_mutex);
301 std::lock_guard<std::mutex> guard(
m_mutex);
322 double t_off_rad = temp[2];
323 double x_off = m[0][3];
324 double y_off = m[1][3];
326#ifdef DO_NOTHING_DEBUG
329 double t_off_deg = 0;
330 double t_off_rad = 0;
335 for (
size_t i = 0; i < scan_data.
scans.
size(); i++)
337 double distance = scan_data.
scans[i];
338 if (distance == std::numeric_limits<double>::infinity())
342 if (std::isnan(distance))
349 double angle_input_deg = (i * resolution) + scan_data.
angle_min;
350 double angle_input_rad = (angle_input_deg) *
DEG2RAD;
353 double Ay = (sin(angle_input_rad + t_off_rad) * distance);
354 double Ax = (cos(angle_input_rad + t_off_rad) * distance);
357 double By = Ay + y_off;
358 double Bx = Ax + x_off;
360 double angle_output_rad = atan2(By, Bx);
361 double angle_output_deg = angle_output_rad *
RAD2DEG;
376 double newdistance = std::sqrt((Bx * Bx) + (By * By));
418 if (
m_iTc ==
nullptr)
429 if (frame_exists ==
false)
438 for (
size_t i = 0; i < nports; i++)
442 if (frame_exists ==
false)
std::vector< InputPortProcessor > m_input_ports
bool close() override
Close the DeviceDriver.
std::vector< yarp::dev::LaserScan2D > m_last_scan_data
bool open(yarp::os::Searchable &config) override
Open the DeviceDriver.
bool threadInit() override
Initialization method.
std::vector< std::string > m_src_frame_id
std::vector< yarp::os::Stamp > m_last_stamp
bool setScanRate(double rate) override
set the scan rate (scans per seconds)
void calculate(yarp::dev::LaserScan2D scan, yarp::sig::Matrix m)
std::string m_dst_frame_id
bool setHorizontalResolution(double step) override
get the angular step between two measurements (if available)
bool acquireDataFromHW() override final
This method should be implemented by the user, and contain the logic to grab data from the hardware.
bool setDistanceRange(double min, double max) override
set the device detection range.
yarp::sig::Vector m_empty_laser_data
bool m_option_override_limits
yarp::dev::IFrameTransform * m_iTc
void run() override
Loop function.
std::vector< std::string > m_port_names
void threadRelease() override
Release method.
bool setScanLimits(double min, double max) override
set the scan angular range.
yarp::dev::PolyDriver m_tc_driver
bool view(T *&x)
Get an interface to the device driver.
yarp::sig::Vector scans
the scan data, measured in [m].
double angle_min
first angle of the scan [deg]
double angle_max
last angle of the scan [deg]
virtual bool updateLidarData()
This utility method calls in sequence: grabDataFromHW(), updateTimestamp and applyLimitsOnLaserData()...
bool parseConfiguration(yarp::os::Searchable &config)
yarp::sig::Vector m_laser_data
bool isValid() const
Check if device is valid.
bool open(const std::string &txt)
Construct and configure a device by its common name.
A simple collection of objects that can be described and transmitted in a portable way.
size_type size() const
Gets the number of elements in the bottle.
Value & get(size_type index) const
Reads a Value v from a certain part of the list.
std::string toString() const override
Gives a human-readable textual representation of the bottle.
bool getEnvelope(PortReader &envelope) override
A class for storing options and configuration information.
void fromString(const std::string &txt, bool wipe=true)
Interprets a string as a list of properties.
void put(const std::string &key, const std::string &value)
Associate the given key with the given string.
A base class for nested structures that can be searched.
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.
virtual Bottle & findGroup(const std::string &key) const =0
Gets a list corresponding to a given keyword.
An abstraction for a time stamp and/or sequence number.
virtual Bottle * asList() const
Get list value.
virtual std::string asString() const
Get string value.
const Matrix & eye()
Build an identity matrix, don't resize.
void resize(size_t size) override
Resize the vector.
double constrainAngle(double x)
const yarp::os::LogComponent & LASER_FROM_EXTERNAL_PORT()
#define yCInfo(component,...)
#define yCError(component,...)
#define yCAssert(component, x)
#define yCWarning(component,...)
#define yCDebug(component,...)
#define YARP_LOG_COMPONENT(name,...)
For streams capable of holding different kinds of content, check what they actually have.
yarp::sig::Vector dcm2rpy(const yarp::sig::Matrix &R)
Converts a dcm (direction cosine matrix) rotation matrix to roll-pitch-yaw angles (defined in Math....
double now()
Return the current time in seconds, relative to an arbitrary starting point.
void delay(double seconds)
Wait for a certain number of seconds.
An interface to the operating system, including Port based communication.