Skip to content

Commit

Permalink
other devices modified
Browse files Browse the repository at this point in the history
  • Loading branch information
randaz81 committed Feb 19, 2025
1 parent 8ee1528 commit f0eb9dc
Show file tree
Hide file tree
Showing 8 changed files with 75 additions and 75 deletions.
16 changes: 8 additions & 8 deletions src/devices/laserFromDepth/laserFromDepth.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -100,33 +100,33 @@ bool LaserFromDepth::close()
return true;
}

bool LaserFromDepth::setDistanceRange(double min, double max)
ReturnValue LaserFromDepth::setDistanceRange(double min, double max)
{
std::lock_guard<std::mutex> guard(m_mutex);
m_min_distance = min;
m_max_distance = max;
return true;
return ReturnValue_ok;
}

bool LaserFromDepth::setScanLimits(double min, double max)
ReturnValue LaserFromDepth::setScanLimits(double min, double max)
{
std::lock_guard<std::mutex> guard(m_mutex);
yCWarning(LASER_FROM_DEPTH) << "setScanLimits not yet implemented";
return true;
return ReturnValue_ok;
}

bool LaserFromDepth::setHorizontalResolution(double step)
ReturnValue LaserFromDepth::setHorizontalResolution(double step)
{
std::lock_guard<std::mutex> guard(m_mutex);
yCWarning(LASER_FROM_DEPTH) << "setHorizontalResolution not yet implemented";
return true;
return ReturnValue_ok;
}

bool LaserFromDepth::setScanRate(double rate)
ReturnValue LaserFromDepth::setScanRate(double rate)
{
std::lock_guard<std::mutex> guard(m_mutex);
yCWarning(LASER_FROM_DEPTH) << "setScanRate not yet implemented";
return false;
return ReturnValue::return_code::return_value_error_not_implemented_by_device;
}

bool LaserFromDepth::threadInit()
Expand Down
8 changes: 4 additions & 4 deletions src/devices/laserFromDepth/laserFromDepth.h
Original file line number Diff line number Diff line change
Expand Up @@ -58,10 +58,10 @@ class LaserFromDepth : public PeriodicThread, public yarp::dev::Lidar2DDeviceBas

public:
//IRangefinder2D interface
bool setDistanceRange (double min, double max) override;
bool setScanLimits (double min, double max) override;
bool setHorizontalResolution (double step) override;
bool setScanRate (double rate) override;
yarp::dev::ReturnValue setDistanceRange (double min, double max) override;
yarp::dev::ReturnValue setScanLimits (double min, double max) override;
yarp::dev::ReturnValue setHorizontalResolution (double step) override;
yarp::dev::ReturnValue setScanRate (double rate) override;

public:
//Lidar2DDeviceBase
Expand Down
16 changes: 8 additions & 8 deletions src/devices/laserFromExternalPort/laserFromExternalPort.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -249,35 +249,35 @@ bool LaserFromExternalPort::close()



bool LaserFromExternalPort::setDistanceRange(double min, double max)
ReturnValue LaserFromExternalPort::setDistanceRange(double min, double max)
{
std::lock_guard<std::mutex> guard(m_mutex);
m_min_distance = min;
m_max_distance = max;
return true;
return ReturnValue_ok;
}

bool LaserFromExternalPort::setScanLimits(double min, double max)
ReturnValue LaserFromExternalPort::setScanLimits(double min, double max)
{
std::lock_guard<std::mutex> guard(m_mutex);
yCWarning(LASER_FROM_EXTERNAL_PORT) << "setScanLimits not yet implemented";
return true;
return ReturnValue_ok;
}



bool LaserFromExternalPort::setHorizontalResolution(double step)
ReturnValue LaserFromExternalPort::setHorizontalResolution(double step)
{
std::lock_guard<std::mutex> guard(m_mutex);
yCWarning(LASER_FROM_EXTERNAL_PORT, "setHorizontalResolution not yet implemented");
return true;
return ReturnValue_ok;
}

bool LaserFromExternalPort::setScanRate(double rate)
ReturnValue LaserFromExternalPort::setScanRate(double rate)
{
std::lock_guard<std::mutex> guard(m_mutex);
yCWarning(LASER_FROM_EXTERNAL_PORT, "setScanRate not yet implemented");
return false;
return ReturnValue::return_code::return_value_error_not_implemented_by_device;
}


Expand Down
8 changes: 4 additions & 4 deletions src/devices/laserFromExternalPort/laserFromExternalPort.h
Original file line number Diff line number Diff line change
Expand Up @@ -123,10 +123,10 @@ class LaserFromExternalPort : public yarp::dev::Lidar2DDeviceBase,

public:
//IRangefinder2D interface
bool setDistanceRange (double min, double max) override;
bool setScanLimits (double min, double max) override;
bool setHorizontalResolution (double step) override;
bool setScanRate (double rate) override;
yarp::dev::ReturnValue setDistanceRange (double min, double max) override;
yarp::dev::ReturnValue setScanLimits (double min, double max) override;
yarp::dev::ReturnValue setHorizontalResolution (double step) override;
yarp::dev::ReturnValue setScanRate (double rate) override;

public:
//Lidar2DDeviceBase
Expand Down
16 changes: 8 additions & 8 deletions src/devices/laserFromPointCloud/laserFromPointCloud.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -190,34 +190,34 @@ bool LaserFromPointCloud::close()
return true;
}

bool LaserFromPointCloud::setDistanceRange(double min, double max)
ReturnValue LaserFromPointCloud::setDistanceRange(double min, double max)
{
std::lock_guard<std::mutex> guard(m_mutex);
m_min_distance = min;
m_max_distance = max;
return true;
return ReturnValue_ok;
}


bool LaserFromPointCloud::setScanLimits(double min, double max)
ReturnValue LaserFromPointCloud::setScanLimits(double min, double max)
{
std::lock_guard<std::mutex> guard(m_mutex);
yCWarning(LASER_FROM_POINTCLOUD,"setScanLimits not yet implemented");
return true;
return ReturnValue_ok;
}

bool LaserFromPointCloud::setHorizontalResolution(double step)
ReturnValue LaserFromPointCloud::setHorizontalResolution(double step)
{
std::lock_guard<std::mutex> guard(m_mutex);
yCWarning(LASER_FROM_POINTCLOUD,"setHorizontalResolution not yet implemented");
return true;
return ReturnValue_ok;
}

bool LaserFromPointCloud::setScanRate(double rate)
ReturnValue LaserFromPointCloud::setScanRate(double rate)
{
std::lock_guard<std::mutex> guard(m_mutex);
yCWarning(LASER_FROM_POINTCLOUD,"setScanRate not yet implemented");
return false;
return ReturnValue::return_code::return_value_error_not_implemented_by_device;
}


Expand Down
8 changes: 4 additions & 4 deletions src/devices/laserFromPointCloud/laserFromPointCloud.h
Original file line number Diff line number Diff line change
Expand Up @@ -80,10 +80,10 @@ class LaserFromPointCloud : public PeriodicThread, public yarp::dev::Lidar2DDevi

public:
//IRangefinder2D interface
bool setDistanceRange (double min, double max) override;
bool setScanLimits (double min, double max) override;
bool setHorizontalResolution (double step) override;
bool setScanRate (double rate) override;
yarp::dev::ReturnValue setDistanceRange (double min, double max) override;
yarp::dev::ReturnValue setScanLimits (double min, double max) override;
yarp::dev::ReturnValue setHorizontalResolution (double step) override;
yarp::dev::ReturnValue setScanRate (double rate) override;

public:
//Lidar2DDeviceBase
Expand Down
54 changes: 27 additions & 27 deletions src/devices/laserHokuyo/laserHokuyo.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -280,59 +280,59 @@ bool laserHokuyo::close()
return true;
}

bool laserHokuyo::getDistanceRange(double& min, double& max)
ReturnValue laserHokuyo::getDistanceRange(double& min, double& max)
{
//should return range 0.1-30m (or 100, 30000mm depending on the measurement units)
min = 0.1;
max = 30;
return true;
return ReturnValue_ok;
}

bool laserHokuyo::setDistanceRange(double min, double max)
ReturnValue laserHokuyo::setDistanceRange(double min, double max)
{
yCError(LASERHOKUYO, "setDistanceRange NOT YET IMPLEMENTED");
return false;
return ReturnValue::return_code::return_value_error_not_implemented_by_device;
}

bool laserHokuyo::getScanLimits(double& min, double& max)
ReturnValue laserHokuyo::getScanLimits(double& min, double& max)
{
//degrees
min = min_angle;
max = max_angle;
return true;
return ReturnValue_ok;
}

bool laserHokuyo::setScanLimits(double min, double max)
ReturnValue laserHokuyo::setScanLimits(double min, double max)
{
yCError(LASERHOKUYO, "setScanLimits NOT YET IMPLEMENTED");
return false;
return ReturnValue::return_code::return_value_error_not_implemented_by_device;
}

bool laserHokuyo::getHorizontalResolution(double& step)
ReturnValue laserHokuyo::getHorizontalResolution(double& step)
{
step = 0.25; //deg //1080*0.25=270
return true;
return ReturnValue_ok;
}

bool laserHokuyo::setHorizontalResolution(double step)
ReturnValue laserHokuyo::setHorizontalResolution(double step)
{
yCError(LASERHOKUYO, "setHorizontalResolution NOT YET IMPLEMENTED");
return false;
return ReturnValue::return_code::return_value_error_not_implemented_by_device;
}

bool laserHokuyo::getScanRate(double& rate)
ReturnValue laserHokuyo::getScanRate(double& rate)
{
rate = 20; //20 Hz = 50 ms
return true;
return ReturnValue_ok;
}

bool laserHokuyo::setScanRate(double rate)
ReturnValue laserHokuyo::setScanRate(double rate)
{
yCError(LASERHOKUYO, "setScanRate NOT YET IMPLEMENTED");
return false;
return ReturnValue::return_code::return_value_error_not_implemented_by_device;
}

bool laserHokuyo::getRawData(yarp::sig::Vector &out, double* timestamp)
ReturnValue laserHokuyo::getRawData(yarp::sig::Vector &out, double* timestamp)
{
if (internal_status != HOKUYO_STATUS_NOT_READY)
{
Expand All @@ -341,13 +341,13 @@ bool laserHokuyo::getRawData(yarp::sig::Vector &out, double* timestamp)
out = laser_data;
mutex.unlock();
device_status = yarp::dev::IRangefinder2D::DEVICE_OK_IN_USE;
return true;
return ReturnValue_ok;
}
device_status = yarp::dev::IRangefinder2D::DEVICE_GENERAL_ERROR;
return false;
return ReturnValue::return_code::return_value_error_not_ready;
}

bool laserHokuyo::getLaserMeasurement(std::vector<LaserMeasurementData> &data, double* timestamp)
ReturnValue laserHokuyo::getLaserMeasurement(std::vector<LaserMeasurementData> &data, double* timestamp)
{
if (internal_status != HOKUYO_STATUS_NOT_READY)
{
Expand All @@ -359,7 +359,7 @@ bool laserHokuyo::getLaserMeasurement(std::vector<LaserMeasurementData> &data, d
{
yCError(LASERHOKUYO) << "getLaserMeasurement failed";
mutex.unlock();
return false;
return ReturnValue::return_code::return_value_error_method_failed;
}

double laser_angle_of_view = max_angle - min_angle;
Expand All @@ -370,18 +370,18 @@ bool laserHokuyo::getLaserMeasurement(std::vector<LaserMeasurementData> &data, d
}
mutex.unlock();
device_status = yarp::dev::IRangefinder2D::DEVICE_OK_IN_USE;
return true;
return ReturnValue_ok;
}

device_status = yarp::dev::IRangefinder2D::DEVICE_GENERAL_ERROR;
return false;
return ReturnValue::return_code::return_value_error_not_ready;
}
bool laserHokuyo::getDeviceStatus(Device_status &status)
ReturnValue laserHokuyo::getDeviceStatus(Device_status &status)
{
mutex.lock();
status = device_status;
mutex.unlock();
return true;
return ReturnValue_ok;
}

bool laserHokuyo::threadInit()
Expand Down Expand Up @@ -580,10 +580,10 @@ void laserHokuyo::threadRelease()
yCTrace(LASERHOKUYO, "... done.");
}

bool laserHokuyo::getDeviceInfo(std::string &device_info)
ReturnValue laserHokuyo::getDeviceInfo(std::string &device_info)
{
this->mutex.lock();
device_info = info;
this->mutex.unlock();
return true;
return ReturnValue_ok;
}
24 changes: 12 additions & 12 deletions src/devices/laserHokuyo/laserHokuyo.h
Original file line number Diff line number Diff line change
Expand Up @@ -104,18 +104,18 @@ class laserHokuyo : public PeriodicThread, public yarp::dev::IRangefinder2D, pub

public:
//IRangefinder2D interface
bool getRawData(yarp::sig::Vector &data, double* timestamp) override;
bool getLaserMeasurement(std::vector<yarp::sig::LaserMeasurementData> &data, double* timestamp) override;
bool getDeviceStatus (Device_status &status) override;
bool getDeviceInfo (std::string &device_info) override;
bool getDistanceRange (double& min, double& max) override;
bool setDistanceRange (double min, double max) override;
bool getScanLimits (double& min, double& max) override;
bool setScanLimits (double min, double max) override;
bool getHorizontalResolution (double& step) override;
bool setHorizontalResolution (double step) override;
bool getScanRate (double& rate) override;
bool setScanRate (double rate) override;
yarp::dev::ReturnValue getRawData(yarp::sig::Vector &data, double* timestamp) override;
yarp::dev::ReturnValue getLaserMeasurement(std::vector<yarp::sig::LaserMeasurementData> &data, double* timestamp) override;
yarp::dev::ReturnValue getDeviceStatus (Device_status &status) override;
yarp::dev::ReturnValue getDeviceInfo (std::string &device_info) override;
yarp::dev::ReturnValue getDistanceRange (double& min, double& max) override;
yarp::dev::ReturnValue setDistanceRange (double min, double max) override;
yarp::dev::ReturnValue getScanLimits (double& min, double& max) override;
yarp::dev::ReturnValue setScanLimits (double min, double max) override;
yarp::dev::ReturnValue getHorizontalResolution (double& step) override;
yarp::dev::ReturnValue setHorizontalResolution (double step) override;
yarp::dev::ReturnValue getScanRate (double& rate) override;
yarp::dev::ReturnValue setScanRate (double rate) override;

private:
//laser methods
Expand Down

0 comments on commit f0eb9dc

Please sign in to comment.