6 #define _USE_MATH_DEFINES
27 #define DEG2RAD M_PI/180.0
31 #define RAD2DEG 180/M_PI
70 m_contains_data=
false;
77 getEnvelope(m_lastStamp);
79 m_port_mutex.unlock();
86 while (m_contains_data==
false)
95 m_port_mutex.unlock();
103 m_info =
"LaserFromExternalPort device";
109 if (this->parseConfiguration(config) ==
false)
117 if (general_config.
check(
"input_ports_name"))
122 for (
size_t i = 0; i < portlist->
size(); i++) {
123 m_port_names.push_back(portlist->
get(i).
asString());
128 m_port_names.push_back(
"/laserFromExternalPort:i");
131 for (
size_t i = 0; i < m_port_names.size(); i++)
134 m_input_ports.push_back(proc);
136 m_last_stamp.resize(m_port_names.size());
137 m_last_scan_data.resize(m_port_names.size());
140 if (general_config.
check(
"base_type"))
142 std::string bt = general_config.
find(
"base_type").
asString();
151 m_empty_laser_data = m_laser_data;
154 for (
size_t i = 0; i < m_empty_laser_data.size(); i++) {
155 m_empty_laser_data[i] = std::numeric_limits<double>::infinity();
160 for (
size_t i = 0; i < m_empty_laser_data.size(); i++) {
161 m_empty_laser_data[i] = std::nanf(
"");
166 for (
size_t i = 0; i < m_empty_laser_data.size(); i++) {
167 m_empty_laser_data[i] = 0;
176 if (general_config.
check(
"override"))
178 if (m_input_ports.size() != 1)
185 m_option_override_limits =
true;
190 if (config.
check(
"TRANSFORMS") && config.
check(
"TRANSFORM_CLIENT"))
196 if (src_frames_list->
size() != m_input_ports.size())
201 for (
size_t i = 0; i < src_frames_list->
size(); i++)
203 m_src_frame_id.push_back(src_frames_list->
get(i).
asString());
211 m_dst_frame_id = transforms_config.
find(
"dst_frame").
asString();
212 if (m_dst_frame_id==
"")
220 if (client_cfg_string==
"")
228 tcprop.
put(
"device",
"transformClient");
230 m_tc_driver.open(tcprop);
231 if (!m_tc_driver.isValid())
236 m_tc_driver.view(m_iTc);
245 for (
size_t i=0; i<m_input_ports.size(); i++)
247 m_input_ports[i].useCallback();
248 if (m_input_ports[i].open(m_port_names[i]) ==
false)
264 for (
auto it=m_input_ports.begin(); it!= m_input_ports.end(); it++)
277 std::lock_guard<std::mutex> guard(m_mutex);
278 m_min_distance = min;
279 m_max_distance = max;
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;
365 if (angle_output_deg > m_max_angle) {
continue; }
366 if (angle_output_deg < m_min_angle) {
continue; }
369 int new_i = lrint ((angle_output_deg - m_min_angle) / m_resolution);
370 if (
static_cast<size_t>(new_i) == m_laser_data.size()) {new_i=0;}
376 double newdistance = std::sqrt((Bx * Bx) + (By * By));
379 if (std::isnan(m_laser_data[new_i]))
381 m_laser_data[new_i] = newdistance;
383 else if (newdistance < m_laser_data[new_i])
385 m_laser_data[new_i] = newdistance;
396 m_laser_data = m_empty_laser_data;
398 size_t nports = m_input_ports.size();
401 m_input_ports[0].getLast(m_last_scan_data[0], m_last_stamp[0]);
402 size_t received_scans = m_last_scan_data[0].scans.size();
404 if (m_option_override_limits)
407 m_sensorsNum = received_scans;
408 m_max_angle = m_last_scan_data[0].angle_max;
409 m_min_angle = m_last_scan_data[0].angle_min;
410 m_max_distance = m_last_scan_data[0].range_max;
411 m_min_distance = m_last_scan_data[0].range_min;
412 m_resolution = received_scans / (m_max_angle - m_min_angle);
413 if (m_laser_data.size() != m_sensorsNum) {
414 m_laser_data.resize(m_sensorsNum);
418 if (m_iTc ==
nullptr)
420 for (
size_t elem = 0; elem < m_sensorsNum; elem++)
422 m_laser_data[elem] = m_last_scan_data[0].scans[elem];
428 bool frame_exists = m_iTc->getTransform(m_src_frame_id[0], m_dst_frame_id, m);
429 if (frame_exists ==
false)
433 calculate(m_last_scan_data[0], m);
438 for (
size_t i = 0; i < nports; i++)
441 bool frame_exists = m_iTc->getTransform(m_src_frame_id[i], m_dst_frame_id, m);
442 if (frame_exists ==
false)
446 m_input_ports[i].getLast(m_last_scan_data[i], m_last_stamp[i]);
447 calculate(m_last_scan_data[i], m);
bool close() override
Close the DeviceDriver.
bool open(yarp::os::Searchable &config) override
Open the DeviceDriver.
bool threadInit() override
Initialization method.
bool setScanRate(double rate) override
set the scan rate (scans per seconds)
void calculate(yarp::dev::LaserScan2D scan, yarp::sig::Matrix m)
bool setHorizontalResolution(double step) override
get the angular step between two measurments (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.
void run() override
Loop function.
void threadRelease() override
Release method.
bool setScanLimits(double min, double max) override
set the scan angular range.
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]
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 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.
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 Value & find(const std::string &key) const =0
Gets a value corresponding to a given keyword.
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 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.
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.