/* +------------------------------------------------------------------------+ | Mobile Robot Programming Toolkit (MRPT) | | https://www.mrpt.org/ | | | | Copyright (c) 2005-2026, Individual contributors, see AUTHORS file | | See: https://www.mrpt.org/Authors - All rights reserved. | | Released under BSD License. See: https://www.mrpt.org/License | +------------------------------------------------------------------------+ */ #include "hwdrivers-precomp.h" // Precompiled headers // #include #include #include #include #include #if MRPT_HAS_PHIDGET #include #endif using namespace mrpt::hwdrivers; using namespace mrpt::obs; using namespace mrpt::system; using namespace std; IMPLEMENTS_GENERIC_SENSOR(CPhidgetInterfaceKitProximitySensors, mrpt::hwdrivers) /* ----------------------------------------------------- Constructor ----------------------------------------------------- */ CPhidgetInterfaceKitProximitySensors::CPhidgetInterfaceKitProximitySensors() : mrpt::system::COutputLogger("CPhidgetInterfaceKitProximitySensors") { #if MRPT_HAS_PHIDGET m_carteInterfaceKit = new CPhidgetInterfaceKitHandle; *((CPhidgetInterfaceKitHandle*)m_carteInterfaceKit) = 0; m_sensorLabel = "PhidgetInterfaceKit"; m_sensorIsPlugged.assign(8, false); m_minRange.assign(8, 0.1f); m_maxRange.assign(8, 0.8f); m_sensorPoses.resize(8); m_sensorType.assign(8, UNPLUGGED); #else THROW_EXCEPTION( "MRPT Was compiled without the CPhidget support. Recompile MRPT to use " "this class"); #endif } /* ----------------------------------------------------- loadConfig_sensorSpecific ----------------------------------------------------- */ void CPhidgetInterfaceKitProximitySensors::loadConfig_sensorSpecific( [[maybe_unused]] const mrpt::config::CConfigFileBase& configSource, [[maybe_unused]] const std::string& iniSection) { #if MRPT_HAS_PHIDGET if (!configSource.sectionExists(iniSection)) THROW_EXCEPTION("Can't find section in configuration file"); // looking for the board parameters. // process_rate = 100 // Hz (common to all sensors) // serialNumber = 12345 // The interface kit serial // number. m_process_rate = configSource.read_int(iniSection, string("process_rate"), 50); m_serialNumber = configSource.read_int(iniSection, string("serialNumber"), -1); bool display = configSource.read_bool(iniSection, string("displayRecapitulativeInformations"), false); // Looking for each sensor. for (int i = 1; i <= 8; i++) { string sensorNKeyName = mrpt::format("sensor%d", i); string sensorType = configSource.read_string(iniSection, sensorNKeyName, string("UNPLUGGED")); if (sensorType != string("UNPLUGGED")) { // the sensor is plugged : // // check if the sensor type is supported. if (sensorType == string("EZ1")) { m_sensorType[i - 1] = EZ1; m_minRange[i - 1] = 0.15; // meters m_maxRange[i - 1] = 6.45; // meters } else if (sensorType == string("SHARP-30cm")) { m_sensorType[i - 1] = SHARP_30cm; m_minRange[i - 1] = 0.04; // meters m_maxRange[i - 1] = 0.3; // meters } else if (sensorType == string("SHARP-80cm")) { m_sensorType[i - 1] = SHARP_80cm; m_minRange[i - 1] = 0.06; // meters m_maxRange[i - 1] = 0.8; // meters } else { string err = mrpt::format("Type of sensor %d is not supported", i); m_state = CGenericSensor::ssError; THROW_EXCEPTION(err); } m_sensorIsPlugged[i - 1] = true; // reading the sensor pose. string sensorNPoseX = mrpt::format("pose%d_x", i); string sensorNPoseY = mrpt::format("pose%d_y", i); string sensorNPoseZ = mrpt::format("pose%d_z", i); string sensorNPoseYaw = mrpt::format("pose%d_yaw", i); string sensorNPosePitch = mrpt::format("pose%d_pitch", i); string sensorNPoseRoll = mrpt::format("pose%d_roll", i); float x = configSource.read_float(iniSection, sensorNPoseX, 0.0); float y = configSource.read_float(iniSection, sensorNPoseY, 0.0); float z = configSource.read_float(iniSection, sensorNPoseZ, 0.0); float yaw = configSource.read_float(iniSection, sensorNPoseYaw, 0.0); float pitch = configSource.read_float(iniSection, sensorNPosePitch, 0.0); float roll = configSource.read_float(iniSection, sensorNPoseRoll, 0.0); m_sensorPoses[i - 1] = mrpt::poses::CPose3D(x, y, z, yaw, pitch, roll); } } if (display) { // width = 80; cout.fill(' '); cout << "+-------------------------------------------------------------" "-----------------+" << endl; cout.width(79); cout << "| Phidget interfaceKit board number : " << m_serialNumber; cout << "|" << endl; cout << "| Process rate : " << m_process_rate; cout << "|" << endl; cout << "+---------+---------------------+-----------------------------" "-----------------+" << endl; cout << "| # + Sensor type | Sensor 3D pose " " |" << endl; cout << "+---------+---------------------+-----------------------------" "-----------------+" << endl; for (int i = 0; i < 8; i++) { cout << "|"; cout.width(9); cout << i + 1; cout << " |"; cout.width(19); switch (m_sensorType[i]) { case EZ1: cout << "EZ1 |"; break; case SHARP_30cm: cout << "SHARP_30cm |"; break; case SHARP_80cm: cout << "SHARP_80cm |"; break; case UNPLUGGED: cout << "UNPLUGGED |"; break; } cout.width(43); cout << m_sensorPoses[i]; cout << "|" << endl; } cout << "+-------------------------------------------------------------" "-----------------+" << endl; } #endif } /* ----------------------------------------------------- Initialize ----------------------------------------------------- */ void CPhidgetInterfaceKitProximitySensors::initialize() { #if MRPT_HAS_PHIDGET /*Try to connect to the interface kit board*/ CPhidgetInterfaceKit_create((CPhidgetInterfaceKitHandle*)m_carteInterfaceKit); CPhidget_open(*((CPhidgetHandle*)(m_carteInterfaceKit)), m_serialNumber); int err = CPhidget_waitForAttachment( *((CPhidgetHandle*)(m_carteInterfaceKit)), 200); // wait 200ms for board attachment. // if an error occur, "err" will be a > 0 value. if (err > 0) { m_state = CGenericSensor::ssError; THROW_EXCEPTION("Can't find Phidget IK card, please check your serial number."); } // set frame rate /*int miliseconds = static_cast(1000./d2f(m_process_rate)); for(int i = 0 ; i < 8 ; i++) { if(m_sensorIsPlugged[i]) { int err = CPhidgetInterfaceKit_setDataRate(*((CPhidgetInterfaceKitHandle*)(m_carteInterfaceKit)), i, miliseconds); if(err > 0) { string error = mrpt::format("Can't set process rate to %d ms on channel %d of the Phidget IK Board.", miliseconds, i); m_state = CGenericSensor::ssError; THROW_EXCEPTION(error); } } }*/ // seems to be used only in the event based programming // way. // compute (min/max) of (min/max) ranges. m_minOfMinRanges = *min_element(m_minRange.begin(), m_minRange.end()); m_maxOfMaxRanges = *max_element(m_maxRange.begin(), m_maxRange.end()); // driver is ready. m_state = CGenericSensor::ssWorking; #endif } /* ----------------------------------------------------- Destructor ----------------------------------------------------- */ CPhidgetInterfaceKitProximitySensors::~CPhidgetInterfaceKitProximitySensors() { #if MRPT_HAS_PHIDGET if (*((CPhidgetHandle*)m_carteInterfaceKit)) { CPhidget_close(*((CPhidgetHandle*)(m_carteInterfaceKit))); CPhidget_delete(*((CPhidgetHandle*)(m_carteInterfaceKit))); } #endif } /*------------------------------------------------------------- doProcess -------------------------------------------------------------*/ void CPhidgetInterfaceKitProximitySensors::doProcess() { CObservationRange::Ptr obs = std::make_shared(); try { getObservation(*obs); m_state = ssWorking; // if at least one data have been sensed : if (obs->sensedData.size() > 0) { appendObservation(obs); } } catch (...) { m_state = ssError; THROW_EXCEPTION("No observation received from the Phidget board!"); } } /*------------------------------------------------------------- getObservation -------------------------------------------------------------*/ void CPhidgetInterfaceKitProximitySensors::getObservation( [[maybe_unused]] mrpt::obs::CObservationRange& obs) { #if MRPT_HAS_PHIDGET obs.timestamp = mrpt::Clock::now(); obs.sensorLabel = m_sensorLabel; obs.minSensorDistance = m_minOfMinRanges; obs.maxSensorDistance = m_maxOfMaxRanges; obs.sensorConeAperture = DEG2RAD(2.0f); // TODO : Adapt to real sensor cone apperture. obs.sensedData.clear(); int sensorValue; for (int i = 0; i < 8; i++) { if (m_sensorIsPlugged[i]) { mrpt::obs::CObservationRange::TMeasurement obsRange; int err = CPhidgetInterfaceKit_getSensorValue( *((CPhidgetInterfaceKitHandle*)(m_carteInterfaceKit)), i, &sensorValue); if (err > 0) { string error("Error durring acquiering sensor value on channel : %d", i); THROW_EXCEPTION(error); } switch (m_sensorType[i]) { case EZ1: // TODO : find the conversion formula. obsRange.sensedDistance = 1.0; break; case SHARP_30cm: obsRange.sensedDistance = 2076. / (d2f(sensorValue) - 11.); break; case SHARP_80cm: obsRange.sensedDistance = 4800. / (d2f(sensorValue) - 16.92); break; default: obsRange.sensedDistance = -1; break; } obsRange.sensorID = i; obsRange.sensorPose = m_sensorPoses[i].asTPose(); // The pose of the IR sensor on the robot obs.sensedData.push_back(obsRange); } } #endif }