diff --git a/src/RobotInterface/YarpImplementation/CMakeLists.txt b/src/RobotInterface/YarpImplementation/CMakeLists.txt index 0098970af6..5b468d0b3d 100644 --- a/src/RobotInterface/YarpImplementation/CMakeLists.txt +++ b/src/RobotInterface/YarpImplementation/CMakeLists.txt @@ -6,9 +6,10 @@ if(FRAMEWORK_COMPILE_YarpImplementation) add_bipedal_locomotion_library( NAME RobotInterfaceYarpImplementation - SOURCES src/YarpRobotControl.cpp - PUBLIC_HEADERS include/BipedalLocomotion/RobotInterface/YarpRobotControl.h - PUBLIC_LINK_LIBRARIES BipedalLocomotion::RobotInterface YARP::YARP_dev + SOURCES src/YarpRobotControl.cpp src/YarpSensorBridge.cpp + PUBLIC_HEADERS include/BipedalLocomotion/RobotInterface/YarpRobotControl.h include/BipedalLocomotion/RobotInterface/YarpSensorBridge.h + PRIVATE_HEADERS include/BipedalLocomotion/RobotInterface/YarpSensorBridgeImpl.h + PUBLIC_LINK_LIBRARIES BipedalLocomotion::RobotInterface BipedalLocomotion::System YARP::YARP_dev YARP::YARP_os YARP::YARP_sig INSTALLATION_FOLDER RobotInterface) endif() diff --git a/src/RobotInterface/YarpImplementation/include/BipedalLocomotion/RobotInterface/YarpSensorBridge.h b/src/RobotInterface/YarpImplementation/include/BipedalLocomotion/RobotInterface/YarpSensorBridge.h new file mode 100644 index 0000000000..acc5176bce --- /dev/null +++ b/src/RobotInterface/YarpImplementation/include/BipedalLocomotion/RobotInterface/YarpSensorBridge.h @@ -0,0 +1,393 @@ +/** + * @file YarpSensorBridge.h + * @authors Prashanth Ramadoss + * @copyright 2020 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version. + */ + +#ifndef BIPEDAL_LOCOMOTION_ROBOT_INTERFACE_YARP_SENSOR_BRIDGE_H +#define BIPEDAL_LOCOMOTION_ROBOT_INTERFACE_YARP_SENSOR_BRIDGE_H + +// std +#include + +// YARP +#include + +#include +#include + +namespace BipedalLocomotion +{ +namespace RobotInterface +{ + + +/** + * YarpSensorBridge Yarp implementation of the ISensorBridge interface + * Currently available interfaces + * - Remapped Remote Control Board for joint states + * - Inertial Measurement Units through generic sensor interface and remapped multiple analog sensor interface + * - Whole Body Dynamics Estimated end effector wrenches through a generic sensor interface + * - Force Torque Sensors through analog sensor interface and remapped multiple analog sensor interface + * - Depth Cameras through RGBD sensor interface + * - Camera images through OpenCV Grabber interface + * + * The YarpSensorBridge expects a list of device drivers through the yarp::dev::PolyDriverList object. + * Each PolyDriver object in the list is compared with the configured sensor names and the assumptions listed below + * to infer the sensor types and relevant interfaces in order to to read the relevant data. + * + * MAJOR ASSUMPTIONS + * - Every sensor unit(device driver) attached to this Bridge is identified by a unique name + * - A single instance of a remote control board remapper and a multiple analog sensor remapper is expected if the suer wants to use the control board interfaces and multiple analog sensor interfaces + * - Any generic sensor interface with channel dimensions of 6 is considered to be a cartesian wrench interface + * - Any generic sensor interface with channel dimensions of 12 is considered as a IMU interface (server inertial) + * - Any analog sensor interface with channel dimensions of 6 is considered as a force torque sensor interface + * - The images are available through a FrameGrabber interface (RGB only) and a RGBD interface (RGB and Depth). + * - The current internal design (read all sensors in a serial fashion) may not be suitable for a heavy measurement set + * + * The parameters for writing the configuration file for this class is given as, + * | Group | Parameter | Type | Description | + * |:--------------------------:|:-------------------------------:|:-----------------:|:---------------------------------------------- :| + * | |check_for_nan | int (0/1) |flag to activate checking for NANs in the incoming measurement buffers, not applicable for images| + * | |stream_joint_states | int (0/1) |Flag to activate the attachment to IMU sensor devices | + * | |stream_inertials | int (0/1) |Flag to activate the attachment to IMU sensor devices | + * | |stream_cartesian_wrenches | int (0/1) |Flag to activate the attachment to Cartesian wrench related devices | + * | |stream_forcetorque_sensors | int (0/1) |Flag to activate the attachment to six axis FT sensor devices | + * | |stream_cameras | int (0/1) |Flag to activate the attachment to Cameras devices | + * |RemoteControlBoardRemapper | | |Expects only one remapped remotecontrolboard device attached to it, if there multiple remote control boards, then use a remapper to create a single remotecontrolboard | + * | |joints_list | vector of strings |The joints list used to open the remote control board remapper | + * |InertialSensors | | |Expects IMU to be opened as genericSensorClient devices communicating through the inertial server and other inertials as a part multiple analog sensors remapper ("multipleanalogsensorsremapper") | + * | |imu_list | vector of strings |list of the names of devices opened as genericSensorClient device and having a channel dimension of 12 | + * | |gyroscopes_list | vector of strings |list of the names of devices opened with ThreeAxisGyroscope interface remapped through the "multipleanalogsensorsremapper" interfaces and having a channel dimension of 3 | + * | |accelerometers_list | vector of strings |list of the names of devices opened with ThreeAxisLinearAccelerometers interface remapped through the "multipleanalogsensorsremapper" interfaces and having a channel dimension of 3 | + * | |orientation_sensors_list | vector of strings |list of the names of devices opened with OrientationSensors interface remapped through the "multipleanalogsensorsremapper" interfaces and having a channel dimension of 3 | + * | |magnetometers_list | vector of strings |list of the names of devices opened with ThreeAxisMagnetometer interface remapped through the "multipleanalogsensorsremapper" interfaces and having a channel dimension of 3 | + * |CartesianWrenches | | |Expects the devices wrapping the cartesian wrenches ports to be opened as "genericSensorClient" device and have a channel dimension of 6 | + * | |cartesian_wrenches_list | vector of strings |list of the names of devices opened as genericSensorClient device and having a channel dimension of 6 | + * |SixAxisForceTorqueSensors | | |Expects the Six axis FT sensors to be opened with SixAxisForceTorqueSensors interface remapped through multiple analog sensors remapper ("multipleanalogsensorsremapper") or to be opened as analog sensor ("analogsensorclient") device having channel dimensions as 6| + * | |sixaxis_forcetorque_sensors_list | vector of strings |list of six axis FT sensors (the difference between a MAS FT and an analog FT is done internally assuming that the names are distinct form each other)| + * |Cameras | | |Expects cameras to be opened either as remote frame grabber ("RemoteFrameGrabber") with IFrameGrabber interface or rgbd sensor ("RGBDSensorClient") with IRGBDSensor interface| + * | |rgbd_list | vector of strings |list containing the devices opened as RGBDSensorClients containing the IRGBD sensor interface | + * | |rgbd_image_width | vector of strings |list containing the image width dimensions of RGBD cameras. Required parameter if cameras are enabled. The list must be the same size and order as rgbd_list | + * | |rgbd_image_height | vector of strings |list containing the image height dimensions of RGBD cameras. Required parameter if cameras are enabled. The list must be the same size and order as rgbd_list | + * | |rgb_cameras_list | vector of strings |list containing the devices opened as RemoteFrameGrabber devices containing the IFrameGrabber interface| + * | |rgb_image_width | vector of strings |list containing the image width dimensions of RGB cameras. Required parameter if cameras are enabled. The list must be the same size and order as rgb_list | + * | |rgb_image_height | vector of strings |list containing the image height dimensions of RGB cameras. Required parameter if cameras are enabled. The list must be the same size and order as rgb_list | + * + */ +class YarpSensorBridge : public ISensorBridge, + public BipedalLocomotion::System::Advanceable +{ +public: + /** + * Constructor + */ + YarpSensorBridge(); + + /** + * Destructor + */ + ~YarpSensorBridge(); + + /** + * Initialize estimator + * @param[in] handler Parameters handler + */ + bool initialize(std::weak_ptr handler) final; + + /** + * Set the list of device drivers from which the sensor measurements need to be streamed + * @param deviceDriversList device drivers holding the pointer to sensor interfaces + * @return True/False in case of success/failure. + */ + bool setDriversList(const yarp::dev::PolyDriverList& deviceDriversList); + + /** + * @brief Advance the internal state. This may change the value retrievable from get(). + * @return True if the advance is successful. + */ + bool advance() final; + + /** + * @brief Determines the validity of the object retrieved with get() + * @return True if the object is valid, false otherwise. + */ + bool isValid() const final; + + /** + * @brief list of sensors that was failed to be read in the current advance() step + * @return list of sensors as a vector of strings + */ + std::vector getFailedSensorReads(); + + /** + * @brief Get the object. + * @return a const reference of the requested object. + */ + const SensorBridgeMetaData& get() const final; + + /** + * Get joints list + * @param[out] jointsList list of joints attached to the bridge + * @return true/false in case of success/failure + */ + bool getJointsList(std::vector& jointsList) final; + + /** + * Get imu sensors + * @param[out] IMUsList list of IMUs attached to the bridge + * @return true/false in case of success/failure + */ + bool getIMUsList(std::vector& IMUsList) final; + + /** + * Get linear accelerometers + * @param[out] linearAccelerometersList list of linear accelerometers attached to the bridge + * @return true/false in case of success/failure + */ + bool getLinearAccelerometersList(std::vector& linearAccelerometersList) final; + + /** + * Get gyroscopes + * @param[out] gyroscopesList list of gyroscopes attached to the bridge + * @return true/false in case of success/failure + */ + bool getGyroscopesList(std::vector& gyroscopesList) final; + + /** + * Get orientation sensors + * @param[out] orientationSensorsList list of orientation sensors attached to the bridge + * @return true/false in case of success/failure + */ + bool getOrientationSensorsList(std::vector& orientationSensorsList) final; + + /** + * Get magnetometers sensors + * @param[out] magnetometersList list of magnetometers attached to the bridge + * @return true/false in case of success/failure + */ + bool getMagnetometersList(std::vector& magnetometersList) final; + + /** + * Get 6 axis FT sensors + * @param[out] sixAxisForceTorqueSensorsList list of 6 axis force torque sensors attached to the bridge + * @return true/false in case of success/failure + */ + bool getSixAxisForceTorqueSensorsList(std::vector& sixAxisForceTorqueSensorsList) final; + + /** + * Get 6 axis FT sensors + * @param[out] threeAxisForceTorqueSensorsList list of 3 axis force torque sensors attached to the bridge + * @return true/false in case of success/failure + */ + bool getThreeAxisForceTorqueSensorsList(std::vector& threeAxisForceTorqueSensorsList) final; + + /** + * Get cartesian wrenches + * @param[out] cartesianWrenchesList list of cartesian wrenches attached to the bridge + * @return true/false in case of success/failure + */ + bool getCartesianWrenchesList(std::vector& cartesianWrenchesList) final; + + /** + * Get rgb cameras + * @param[out] rgbCamerasList list of rgb cameras attached to the bridge + * @return true/false in case of success/failure + */ + bool getRGBCamerasList(std::vector& rgbCamerasList) final; + + /** + * Get RGBD cameras + * @param[out] rgbdCamerasList list of depth cameras attached to the bridge + * @return true/false in case of success/failure + */ + bool getRGBDCamerasList(std::vector& rgbdCamerasList) final; + + /** + * Get joint position in radians + * @param[in] jointName name of the joint + * @param[out] jointPosition joint position in radians + * @param[out] receiveTimeInSeconds time at which the measurement was received + * @return true/false in case of success/failure + */ + bool getJointPosition(const std::string& jointName, + double& jointPosition, + double* receiveTimeInSeconds = nullptr) final; + + /** + * Get all joints' positions in radians + * @param[out] parameter all joints' position in radians + * @param[out] receiveTimeInSeconds time at which the measurement was received + * + * @warning the size is decided at the configuration and remains fixed, + * and internal checks must be done at the implementation level by the Derived class. + * This means that the user must pass a resized argument "jointPositions" to this method + * + * @return true/false in case of success/failure + */ + bool getJointPositions(Eigen::Ref jointPositions, + double* receiveTimeInSeconds = nullptr) final; + + /** + * Get joint velocity in rad/s + * @param[in] jointName name of the joint + * @param[out] jointVelocity joint velocity in radians per second + * @param[out] receiveTimeInSeconds time at which the measurement was received + * @return true/false in case of success/failure + */ + bool getJointVelocity(const std::string& jointName, + double& jointVelocity, + double* receiveTimeInSeconds = nullptr) final; + + /** + * Get all joints' velocities in rad/s + * @param[out] parameter all joints' velocities in radians per second + * @param[out] receiveTimeInSeconds time at which the measurement was received + * + * @warning the size is decided at the configuration and remains fixed, + * and internal checks must be done at the implementation level by the Derived class. + * This means that the user must pass a resized argument "jointVelocties" to this method + * + * @return true/false in case of success/failure + */ + bool getJointVelocities(Eigen::Ref jointVelocties, + double* receiveTimeInSeconds = nullptr) final; + + /** + * Get IMU measurement + * The serialization of measurments is as follows, + * (rpy acc omega mag) + * - rpy in radians Roll-Pitch-Yaw Euler angles + * - acc in m/s^2 linear accelerometer measurements + * - omega in rad/s gyroscope measurements + * - mag in tesla magnetometer measurements + * @param[in] imuName name of the IMU + * @param[out] imuMeasurement imu measurement of size 12 + * @param[out] receiveTimeInSeconds time at which the measurement was received + * @return true/false in case of success/failure + */ + bool getIMUMeasurement(const std::string& imuName, + Eigen::Ref imuMeasurement, + double* receiveTimeInSeconds = nullptr) final; + + /** + * Get linear accelerometer measurement in m/s^2 + * @param[in] accName name of the linear accelerometer + * @param[out] accMeasurement linear accelerometer measurements of size 3 + * @param[out] receiveTimeInSeconds time at which the measurement was received + * @return true/false in case of success/failure + */ + bool getLinearAccelerometerMeasurement(const std::string& accName, + Eigen::Ref accMeasurement, + double* receiveTimeInSeconds = nullptr) final; + + /** + * Get gyroscope measurement in rad/s + * @param[in] gyroName name of the gyroscope + * @param[out] gyroMeasurement gyroscope measurements of size 3 + * @param[out] receiveTimeInSeconds time at which the measurement was received + * @return true/false in case of success/failure + */ + bool getGyroscopeMeasure(const std::string& gyroName, + Eigen::Ref gyroMeasurement, + double* receiveTimeInSeconds = nullptr) final; + + /** + * Get orientation sensor measurement in radians as roll pitch yaw Euler angles + * @param[in] rpyName name of the orientation sensor + * @param[out] rpyMeasurement rpy measurements of size 3 + * @param[out] receiveTimeInSeconds time at which the measurement was received + * @return true/false in case of success/failure + */ + bool getOrientationSensorMeasurement(const std::string& rpyName, + Eigen::Ref rpyMeasurement, + double* receiveTimeInSeconds = nullptr) final; + + /** + * Get magentometer measurement in tesla + * @param[in] magName name of the magnetometer + * @param[out] magMeasurement magnetometer measurements of size 3 + * @param[out] receiveTimeInSeconds time at which the measurement was received + * @return true/false in case of success/failure + */ + bool getMagnetometerMeasurement(const std::string& magName, + Eigen::Ref magMeasurement, + double* receiveTimeInSeconds = nullptr) final; + + /** + * Get six axis force torque measurement + * @param[in] ftName name of the FT sensor + * @param[out] ftMeasurement FT measurements of size 6 containing 3d forces and 3d torques + * @param[out] receiveTimeInSeconds time at which the measurement was received + * @return true/false in case of success/failure + */ + bool getSixAxisForceTorqueMeasurement(const std::string& ftName, + Eigen::Ref ftMeasurement, + double* receiveTimeInSeconds = nullptr) final; + + /** + * Get three axis force-torque measurement containing normal force (N) and tangential moments (Nm) + * @param[in] ftName name of the FT sensor + * @param[out] ftMeasurement FT measurements of size 3 containing tau_x tau_y and fz + * @param[out] receiveTimeInSeconds time at which the measurement was received + * @return true/false in case of success/failure + */ + bool getThreeAxisForceTorqueMeasurement(const std::string& ftName, + Eigen::Ref ftMeasurement, + double* receiveTimeInSeconds = nullptr) final; + + /** + * Get 6D end effector wrenches in N and Nm for forces and torques respectively + * @param[in] cartesianWrenchName name of the end effector wrench + * @param[out] cartesianWrenchMeasurement end effector wrench measurement of size 6 + * @param[out] rreceiveTimeInSeconds time at which the measurement was received + * @return true/false in case of success/failure + */ + bool getCartesianWrench(const std::string& cartesianWrenchName, + Eigen::Ref cartesianWrenchMeasurement, + double* receiveTimeInSeconds = nullptr) final; + + /** + * Get color image from the camera + * @param[in] camName name of the camera + * @param[out] colorImg image as Eigen matrix object + * @param[out] receiveTimeInSeconds time at which the measurement was received + * + * @warning the size is decided at the configuration and remains fixed, + * and internal checks must be done at the implementation level by the Derived class. + * This means that the user must pass a resized argument "colorImg" to this method + * + * @return true/false in case of success/failure + */ + bool getColorImage(const std::string& camName, + Eigen::Ref colorImg, + double* receiveTimeInSeconds = nullptr) final; + + /** + * Get depth image + * @param[in] camName name of the gyroscope + * @param[out] depthImg depth image as a Eigen matrix object + * @param[out] receiveTimeInSeconds time at which the measurement was received + * + * @warning the size is decided at the configuration and remains fixed, + * and internal checks must be done at the implementation level by the Derived class. + * This means that the user must pass a resized argument "depthImg" to this method + * + * @return true/false in case of success/failure + */ + bool getDepthImage(const std::string& camName, + Eigen::Ref depthImg, + double* receiveTimeInSeconds = nullptr) final; + +private: + /** Private implementation */ + struct Impl; + std::unique_ptr m_pimpl; +}; + +} // namespace RobotInterface +} // namespace BipedalLocomotion + + +#endif // BIPEDAL_LOCOMOTION_ROBOT_INTERFACE_YARP_SENSOR_BRIDGE_H diff --git a/src/RobotInterface/YarpImplementation/include/BipedalLocomotion/RobotInterface/YarpSensorBridgeImpl.h b/src/RobotInterface/YarpImplementation/include/BipedalLocomotion/RobotInterface/YarpSensorBridgeImpl.h new file mode 100644 index 0000000000..f98e364ba4 --- /dev/null +++ b/src/RobotInterface/YarpImplementation/include/BipedalLocomotion/RobotInterface/YarpSensorBridgeImpl.h @@ -0,0 +1,1708 @@ +/** + * @file YarpSensorBridgeImpl.h + * @authors Prashanth Ramadoss + * @copyright 2020 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version. + */ + +#ifndef BIPEDAL_LOCOMOTION_ROBOT_INTERFACE_YARP_SENSOR_BRIDGE_IMPL_H +#define BIPEDAL_LOCOMOTION_ROBOT_INTERFACE_YARP_SENSOR_BRIDGE_IMPL_H + +#include +#include + +// YARP os +#include + +// YARP sig +#include +#include + +// YARP Sensor Interfaces +#include +#include +#include + +// YARP Camera Interfaces +#include +#include + +// YARP Control Board Interfaces +#include +#include + +// std +#include +#include +#include + +namespace BipedalLocomotion +{ +namespace RobotInterface +{ + +struct YarpSensorBridge::Impl +{ + using StampedYARPVector = std::pair; + using StampedYARPImage = std::pair; + + /** + * Struct holding remapped remote control board interfaces + */ + struct ControlBoardRemapperInterfaces + { + yarp::dev::IEncodersTimed* encoders; + yarp::dev::IAxisInfo* axis; + }; + + ControlBoardRemapperInterfaces controlBoardRemapperInterfaces; + + /** + * Struct holding remapped MAS interfaces -inertial sensors related + */ + struct WholeBodyMASInertialsInterface + { + yarp::dev::IThreeAxisLinearAccelerometers* accelerometers; + yarp::dev::IThreeAxisGyroscopes* gyroscopes; + yarp::dev::IThreeAxisMagnetometers* magnetometers; + yarp::dev::IOrientationSensors* orientationSensors; + }; + + WholeBodyMASInertialsInterface wholeBodyMASInertialsInterface; + + /** + * Struct holding remapped MAS interfaces - FT sensors related + */ + struct WholeBodyMASForceTorquesInterface + { + yarp::dev::ISixAxisForceTorqueSensors* sixAxisFTSensors; + }; + + WholeBodyMASForceTorquesInterface wholeBodyMASForceTorquesInterface; + + + struct MASSensorIndexMaps + { + std::unordered_map accelerometer; + std::unordered_map gyroscopes; + std::unordered_map magnetometers; + std::unordered_map orientationSensors; + std::unordered_map sixAxisFTSensors; + }; + + MASSensorIndexMaps masSensorIndexMaps; + + /** + * Struct holding measurements polled from remapped remote control board interfaces + */ + struct ControlBoardRemapperMeasures + { + yarp::sig::Vector remappedJointIndices; + yarp::sig::Vector jointPositions; + yarp::sig::Vector jointVelocities; + double receivedTimeInSeconds; + }; + + ControlBoardRemapperMeasures controlBoardRemapperMeasures; + + std::unordered_map wholeBodyAnalogIMUInterface; /** < map of IMU sensors attached through generic sensor interfaces */ + std::unordered_map wholeBodyCartesianWrenchInterface; /** < map of cartesian wrench streams attached through generic sensor interfaces */ + std::unordered_map wholeBodyAnalogSixAxisFTSensorsInterface; /** < map of six axis force torque sensors attached through analog sensor interfaces */ + std::unordered_map wholeBodyFrameGrabberInterface; /** < map of cameras attached through frame grabber interfaces */ + std::unordered_map wholeBodyRGBDInterface; /** < map of cameras attached through RGBD interfaces */ + + std::unordered_map wholeBodyIMUMeasures; /** < map holding analog IMU sensor measurements */ + std::unordered_map wholeBodyFTMeasures; /** < map holding six axis force torque measures */ + std::unordered_map wholeBodyInertialMeasures; /** < map holding three axis inertial sensor measures */ + std::unordered_map wholeBodyCartesianWrenchMeasures; /** < map holding cartesian wrench measures */ + std::unordered_map wholeBodyCameraRGBImages; /** < map holding images **/ + std::unordered_map wholeBodyCameraDepthImages; /** < map holding images **/ + + const int nrChannelsInYARPGenericIMUSensor{12}; + const int nrChannelsInYARPGenericCartesianWrench{6}; + const int nrChannelsInYARPAnalogSixAxisFTSensor{6}; + + std::vector failedSensorReads; + SensorBridgeMetaData metaData; /**< struct holding meta data **/ + bool bridgeInitialized{false}; /**< flag set to true if the bridge is successfully initialized */ + bool driversAttached{false}; /**< flag set to true if the bridge is successfully attached to required device drivers */ + bool checkForNAN{false}; /**< flag to enable binary search for NANs in the incoming measurement buffers */ + + + using SubConfigLoader = bool (YarpSensorBridge::Impl::*)(std::weak_ptr, + SensorBridgeMetaData&); + /** + * Utility function to get index from vector + */ + bool getIndexFromVector(const std::vector& vec, + const std::string& query, + int& index) + { + auto iter{std::lower_bound(vec.begin(), vec.end(), query)}; + if (iter == vec.end()) + { + return false; + } + + index = iter - vec.begin(); + return true; + } + + /** + * Checks is a stream is enabled in configuration and + * loads the relevant stream group from configuration + */ + bool subConfigLoader(const std::string& enableStreamString, + const std::string& streamGroupString, + const SubConfigLoader loader, + std::weak_ptr handler, + SensorBridgeMetaData& metaData, + bool& enableStreamFlag) + { + constexpr std::string_view logPrefix = "[YarpSensorBridge::Impl::subConfigLoader] "; + + auto ptr = handler.lock(); + if (ptr == nullptr) + { + std::cerr << logPrefix << "The handler is not pointing to an already initialized memory." + << std ::endl; + return false; + } + + int success; + if (ptr->getParameter(enableStreamString, success)) + { + enableStreamFlag = static_cast(success); + if (enableStreamFlag) + { + auto groupHandler = ptr->getGroup(streamGroupString); + if (! (this->*loader)(groupHandler, metaData) ) + { + std::cerr << logPrefix << streamGroupString << " group could not be initialized from the configuration file." + << std ::endl; + return false; + } + } + } + + return true; + } + + /** + * Configure remote control board remapper meta data + * Related to kinematics and other joint/motor relevant quantities + */ + bool configureRemoteControlBoardRemapper(std::weak_ptr handler, + SensorBridgeMetaData& metaData) + { + constexpr std::string_view logPrefix = "[YarpSensorBridge::Impl::configureRemoteControlBoardRemapper] "; + auto ptr = handler.lock(); + if (ptr == nullptr) { return false; } + + if (!ptr->getParameter("joints_list", metaData.sensorsList.jointsList)) + { + std::cerr << logPrefix << " Required parameter \"joints_list\" not available in the configuration" + << std::endl; + return false; + } + + metaData.bridgeOptions.nrJoints = metaData.sensorsList.jointsList.size(); + return true; + } + + /** + * Configure inertial sensors meta data + */ + bool configureInertialSensors(std::weak_ptr handler, + SensorBridgeMetaData& metaData) + { + constexpr std::string_view logPrefix = "[YarpSensorBridge::Impl::configureInertialSensors] "; + auto ptr = handler.lock(); + if (ptr == nullptr) { return false; } + + if (ptr->getParameter("imu_list", metaData.sensorsList.IMUsList)) + { + metaData.bridgeOptions.isIMUEnabled = true; + } + + if (ptr->getParameter("accelerometers_list", metaData.sensorsList.linearAccelerometersList)) + { + metaData.bridgeOptions.isLinearAccelerometerEnabled = true; + } + + if (ptr->getParameter("gyroscopes_list", metaData.sensorsList.gyroscopesList)) + { + metaData.bridgeOptions.isGyroscopeEnabled = true; + } + + if (ptr->getParameter("orientation_sensors_list", metaData.sensorsList.orientationSensorsList)) + { + metaData.bridgeOptions.isOrientationSensorEnabled = true; + } + + if (ptr->getParameter("magnetometers_list", metaData.sensorsList.magnetometersList)) + { + metaData.bridgeOptions.isMagnetometerEnabled = true; + } + + return true; + } + + /** + * Configure six axis force torque sensors meta data + */ + bool configureSixAxisForceTorqueSensors(std::weak_ptr handler, + SensorBridgeMetaData& metaData) + { + constexpr std::string_view logPrefix = "[YarpSensorBridge::Impl::configureSixAxisForceTorqueSensors] "; + auto ptr = handler.lock(); + if (ptr == nullptr) + { + return false; + } + + if (!ptr->getParameter("sixaxis_forcetorque_sensors_list", metaData.sensorsList.sixAxisForceTorqueSensorsList)) + { + std::cerr << logPrefix << " Required parameter \"sixaxis_forcetorque_sensors_list\" not available in the configuration" + << std::endl; + return false; + } + + return true; + } + + /** + * Configure cartesian wrenches meta data + */ + bool configureCartesianWrenches(std::weak_ptr handler, + SensorBridgeMetaData& metaData) + { + constexpr std::string_view logPrefix = "[YarpSensorBridge::Impl::configureCartesianWrenchSensors] "; + auto ptr = handler.lock(); + if (ptr == nullptr) + { + return false; + } + + if (!ptr->getParameter("cartesian_wrenches_list", metaData.sensorsList.cartesianWrenchesList)) + { + std::cerr << logPrefix << " Required parameter \"cartesian_wrenches_list\" not available in the configuration" + << std::endl; + return false; + } + + return true; + } + + /** + * Configure cameras meta data + */ + bool configureCameras(std::weak_ptr handler, + SensorBridgeMetaData& metaData) + { + constexpr std::string_view logPrefix = "[YarpSensorBridge::Impl::configureCameras] "; + auto ptr = handler.lock(); + if (ptr == nullptr) { return false; } + + if (ptr->getParameter("rgb_cameras_list", metaData.sensorsList.rgbCamerasList)) + { + metaData.bridgeOptions.isCameraEnabled = true; + + std::vector rgbWidth, rgbHeight; + if (ptr->getParameter("rgb_image_width", rgbWidth)) + { + std::cerr << logPrefix << " Required parameter \"rgb_image_width\" not available in the configuration" + << std::endl; + return false; + } + + if (ptr->getParameter("rgb_image_height", rgbHeight)) + { + std::cerr << logPrefix << " Required parameter \"rgb_image_height\" not available in the configuration" + << std::endl; + return false; + } + + if ( (rgbWidth.size() != metaData.sensorsList.rgbCamerasList.size()) || + (rgbHeight.size() != metaData.sensorsList.rgbCamerasList.size()) ) + { + std::cerr << logPrefix << " Parameters list size mismatch" << std::endl; + return false; + } + + for (int idx = 0; idx < rgbHeight.size(); idx++) + { + std::pair imgDimensions(rgbWidth[idx], rgbHeight[idx]); + auto cameraName{metaData.sensorsList.rgbCamerasList[idx]}; + metaData.bridgeOptions.rgbImgDimensions[cameraName] = imgDimensions; + } + } + + if (ptr->getParameter("rgbd_cameras_list", metaData.sensorsList.rgbdCamerasList)) + { + metaData.bridgeOptions.isCameraEnabled = true; + + std::vector rgbdCamWidth, rgbdCamHeight; + if (ptr->getParameter("rgbd_image_width", rgbdCamWidth)) + { + std::cerr << logPrefix << " Required parameter \"rgbd_image_width\" not available in the configuration" + << std::endl; + return false; + } + + if (ptr->getParameter("rgbd_image_height", rgbdCamHeight)) + { + std::cerr << logPrefix << " Required parameter \"rgbd_image_height\" not available in the configuration" + << std::endl; + return false; + } + + if ( (rgbdCamWidth.size() != metaData.sensorsList.rgbdCamerasList.size()) || + (rgbdCamHeight.size() != metaData.sensorsList.rgbdCamerasList.size()) ) + { + std::cerr << logPrefix << " Parameters list size mismatch" << std::endl; + return false; + } + + for (int idx = 0; idx < rgbdCamHeight.size(); idx++) + { + std::pair imgDimensions(rgbdCamWidth[idx], rgbdCamHeight[idx]); + auto cameraName{metaData.sensorsList.rgbdCamerasList[idx]}; + metaData.bridgeOptions.rgbdImgDimensions[cameraName] = imgDimensions; + } + } + + return true; + } + + /** + * Attach device with IGenericSensor or IAnalogSensor interfaces + * Important assumptions here, + * - Any generic sensor with 12 channels is a IMU sensor + * - Any generic sensor with 6 channels is a cartesian wrench sensor + * - Any analog sensor with 6 channels is a six axis force torque sensor + */ + template + bool attachGenericOrAnalogSensor(const yarp::dev::PolyDriverList& devList, + const std::string& sensorName, + const int& nrChannelsInSensor, + std::unordered_map& sensorMap) + { + constexpr std::string_view logPrefix = "[YarpSensorBridge::Impl::attachGenericOrAnalogSensor] "; + for (int devIdx = 0; devIdx < devList.size(); devIdx++) + { + if (sensorName != devList[devIdx]->key) + { + continue; + } + + SensorType* sensorInterface{nullptr}; + if (devList[devIdx]->poly->view(sensorInterface)) + { + if (sensorInterface == nullptr) + { + std::cerr << logPrefix << sensorName << " Could not view interface." << std::endl; + return false; + } + + int nrChannels; + if constexpr (std::is_same_v) + { + nrChannels = sensorInterface->getChannels(); + } + else if constexpr (std::is_same_v) + { + sensorInterface->getChannels(&nrChannels); + } + + if (nrChannels != nrChannelsInSensor) + { + std::cerr << logPrefix << sensorName << " Mismatch in the expected number of channels in the sensor stream." << std::endl; + return false; + } + + sensorMap[devList[devIdx]->key] = sensorInterface; + } + else + { + std::cerr << logPrefix << sensorName << " Could not view interface." << std::endl; + return false; + } + } + return true; + } + + template + bool attachAllGenericOrAnalogSensors(const yarp::dev::PolyDriverList& devList, + std::unordered_map& sensorMap, + const int& nrChannelsInSensor, + const std::vector& sensorList, + std::string_view interfaceType) + { + constexpr std::string_view logPrefix = "[YarpSensorBridge::Impl::attachAllGenericOrAnalogSensors] "; + + bool allSensorsAttachedSuccesful{true}; + for (auto genericSensorCartesianWrench : sensorList) + { + if (!attachGenericOrAnalogSensor(devList, + genericSensorCartesianWrench, + nrChannelsInSensor, + sensorMap)) + { + allSensorsAttachedSuccesful = false; + break; + } + } + + if (!allSensorsAttachedSuccesful || + (sensorMap.size() != sensorList.size()) ) + { + std::cerr << logPrefix << " Could not attach all desired " << interfaceType << " ." << std::endl; + return false; + } + + return true; + } + + /** + * Attach Remapped Multiple Analog Sensor Interfaces and check available sensors + */ + template + bool attachAndCheckMASSensors(const yarp::dev::PolyDriverList& devList, + MASSensorType* sensorInterface, + const std::vector& sensorList, + const std::string_view interfaceName) + { + constexpr std::string_view logPrefix = "[YarpSensorBridge::Impl::attachAndCheckMASSensors] "; + if (!attachRemappedMASSensor(devList, sensorInterface)) + { + std::cerr << logPrefix << " Could not find " << interfaceName << " interface." << std::endl; + return false; + } + + if (!checkAttachedMASSensors(devList, sensorInterface, sensorList)) + { + std::cerr << logPrefix << " Could not find atleast one of the required sensors." << std::endl; + return false; + } + + return true; + } + + /** + * Attach Remapped Multiple Analog Sensor Interfaces + * Looks for a specific MAS Sensor interface in the attached MAS Remapper + */ + template + bool attachRemappedMASSensor(const yarp::dev::PolyDriverList& devList, + MASSensorType* masSensorInterface) + { + constexpr std::string_view logPrefix = "[YarpSensorBridge::Impl::attachRemappedMASSensor] "; + bool broken{false}; + for (int devIdx = 0; devIdx < devList.size(); devIdx++) + { + MASSensorType* sensorInterface{nullptr}; + devList[devIdx]->poly->view(sensorInterface); + if (sensorInterface == nullptr) + { + continue; + } + + // break out of the loop if we find relevant MAS interface + masSensorInterface = sensorInterface; + broken = true; + break; + } + + if (!broken || masSensorInterface == nullptr) + { + std::cerr << logPrefix << " Could not view interface." << std::endl; + return false; + } + return true; + } + + /** + * Check if all the desired MAS sensors are availabled in the attached MAS interface + */ + template + bool checkAttachedMASSensors(const yarp::dev::PolyDriverList& devList, + MASSensorType* sensorInterface, + const std::vector& sensorList) + { + constexpr std::string_view logPrefix = "[YarpSensorBridge::Impl::checkAttachedMASSensors] "; + + auto nrSensors{getNumberOfMASSensors(sensorInterface)}; + if (nrSensors != sensorList.size()) + { + std::cerr << logPrefix << "Expecting the same number of MAS sensors attached to the Bridge as many mentioned in the initialization step" << std::endl; + return false; + } + + for (const auto& masSensorName : sensorList) + { + bool sensorFound{false}; + for (std::size_t attachedIdx = 0; attachedIdx < nrSensors; attachedIdx++) + { + std::string attachedSensorName; + if (!getMASSensorName(sensorInterface, attachedIdx, attachedSensorName)) + { + continue; + } + + if (attachedSensorName == masSensorName) + { + sensorFound = true; + + // update mas sensor index map for lookup + if constexpr (std::is_same_v) + { + masSensorIndexMaps.gyroscopes[masSensorName] = attachedIdx; + } + else if constexpr (std::is_same_v) + { + masSensorIndexMaps.accelerometer[masSensorName] = attachedIdx; + } + else if constexpr (std::is_same_v) + { + masSensorIndexMaps.magnetometers[masSensorName] = attachedIdx; + } + else if constexpr (std::is_same_v) + { + masSensorIndexMaps.orientationSensors[masSensorName] = attachedIdx; + } + else if constexpr (std::is_same_v) + { + masSensorIndexMaps.sixAxisFTSensors[masSensorName] = attachedIdx; + } + break; + } + } + + if (!sensorFound) + { + std::cerr << logPrefix << "Bridge could not attach to MAS Sensor " << masSensorName << "." << std::endl; + } + } + + return true; + } + + /** + * Get number of MAS Sensors + */ + template + std::size_t getNumberOfMASSensors(MASSensorType* sensorInterface) + { + if constexpr (std::is_same_v) + { + return sensorInterface->getNrOfThreeAxisGyroscopes(); + } + else if constexpr (std::is_same_v) + { + return sensorInterface->getNrOfThreeAxisLinearAccelerometers(); + } + else if constexpr (std::is_same_v) + { + return sensorInterface->getNrOfThreeAxisMagnetometers(); + } + else if constexpr (std::is_same_v) + { + return sensorInterface->getNrOfOrientationSensors(); + } + else if constexpr (std::is_same_v) + { + return sensorInterface->getNrOfSixAxisForceTorqueSensors(); + } + + return 0; + } + + /** + * Get name of MAS Sensors + */ + template + bool getMASSensorName(MASSensorType* sensorInterface, + const std::size_t& sensIdx, + std::string& sensorName) + { + if constexpr (std::is_same_v) + { + return sensorInterface->getThreeAxisGyroscopeName(sensIdx, sensorName); + } + else if constexpr (std::is_same_v) + { + return sensorInterface->getThreeAxisLinearAccelerometerName(sensIdx, sensorName); + } + else if constexpr (std::is_same_v) + { + return sensorInterface->getThreeAxisMagnetometerName(sensIdx, sensorName); + } + else if constexpr (std::is_same_v) + { + return sensorInterface->getOrientationSensorName(sensIdx, sensorName); + } + else if constexpr (std::is_same_v) + { + return sensorInterface->getSixAxisForceTorqueSensorName(sensIdx, sensorName); + } + return true; + } + + /** + * Get all sensor names in a MAS Inerface + */ + template + std::vector getAllSensorsInMASInterface(MASSensorType* sensorInterface) + { + std::vector availableSensorNames; + if constexpr (std::is_same_v) + { + auto nrSensors = sensorInterface->getNrOfThreeAxisGyroscopes(); + for (std::size_t sensIdx = 0; sensIdx < nrSensors; sensIdx++) + { + std::string sensName; + sensorInterface->getThreeAxisGyroscopeName(sensIdx, sensName); + availableSensorNames.push_back(sensName); + } + } + else if constexpr (std::is_same_v) + { + auto nrSensors = sensorInterface->getNrOfThreeAxisLinearAccelerometers(); + for (std::size_t sensIdx = 0; sensIdx < nrSensors; sensIdx++) + { + std::string sensName; + sensorInterface->getThreeAxisLinearAccelerometerName(sensIdx, sensName); + availableSensorNames.push_back(sensName); + } + } + else if constexpr (std::is_same_v) + { + auto nrSensors = sensorInterface->getNrOfThreeAxisMagnetometers(); + for (std::size_t sensIdx = 0; sensIdx < nrSensors; sensIdx++) + { + std::string sensName; + sensorInterface->getThreeAxisMagnetometerName(sensIdx, sensName); + availableSensorNames.push_back(sensName); + } + } + else if constexpr (std::is_same_v) + { + auto nrSensors = sensorInterface->getNrOfOrientationSensors(); + for (std::size_t sensIdx = 0; sensIdx < nrSensors; sensIdx++) + { + std::string sensName; + sensorInterface->getOrientationSensorName(sensIdx, sensName); + availableSensorNames.push_back(sensName); + } + } + else if constexpr (std::is_same_v) + { + auto nrSensors = sensorInterface->getNrOfSixAxisForceTorqueSensors(); + for (std::size_t sensIdx = 0; sensIdx < nrSensors; sensIdx++) + { + std::string sensName; + sensorInterface->getSixAxisForceTorqueSensorName(sensIdx, sensName); + availableSensorNames.push_back(sensName); + } + } + return availableSensorNames; + } + + /** + * Attach cameras + */ + template + bool attachCamera(const yarp::dev::PolyDriverList& devList, + const std::string sensorName, + std::unordered_map& sensorMap) + { + constexpr std::string_view logPrefix = "[YarpSensorBridge::Impl::attachCamera] "; + for (int devIdx = 0; devIdx < devList.size(); devIdx++) + { + if (sensorName != devList[devIdx]->key) + { + continue; + } + + CameraType* cameraInterface{nullptr}; + if (devList[devIdx]->poly->view(cameraInterface)) + { + if (cameraInterface == nullptr) + { + std::cerr << logPrefix << " Could not view interface." << std::endl; + return false; + } + sensorMap[devList[devIdx]->key] = cameraInterface; + } + } + return true; + } + + /** + * Check if sensor is available in the relevant sensor map + */ + template + bool checkSensor(const std::unordered_map& sensorMap, + const std::string& sensorName) + { + if (sensorMap.find(sensorName) == sensorMap.end()) + { + return false; + } + + return true; + } + + /** + * Check if sensor is available in the relevant sensor measurement map + */ + template + bool checkValidSensorMeasure(std::string_view logPrefix, + const std::unordered_map& sensorMap, + const std::string& sensorName) + { + if (!checkValid(logPrefix)) + { + return false; + } + + if (sensorMap.find(sensorName) == sensorMap.end()) + { + std::cerr << logPrefix << sensorName << " sensor unavailable in the measurement map" << std::endl; + return false; + } + + return true; + } + + /** + * Check if the bridge is successfully initialized and attached to required device drivers + */ + bool checkValid(const std::string_view methodName) + { + if (!(bridgeInitialized && driversAttached)) + { + std::cerr << methodName << " SensorBridge is not ready. Please initialize and set drivers list."; + return false; + } + return true; + } + + /** + * Attach generic IMU sensor types and MAS inertials + */ + bool attachAllInertials(const yarp::dev::PolyDriverList& devList) + { + constexpr std::string_view logPrefix = "[YarpSensorBridge::Impl::attachAllInertials] "; + if (metaData.bridgeOptions.isIMUEnabled) + { + // check if a generic sensor has 12 channels implying it is a IMU sensor through a GenericSensor Interfaces + std::string_view interfaceType{"Generic IMU Interface"}; + if (!attachAllGenericOrAnalogSensors(devList, + wholeBodyAnalogIMUInterface, + nrChannelsInYARPGenericIMUSensor, + metaData.sensorsList.IMUsList, + interfaceType)) + { + return false; + } + } + + if (metaData.bridgeOptions.isLinearAccelerometerEnabled) + { + std::string_view interfaceType{"IThreeAxisLinearAccelerometers"}; + if (!attachAndCheckMASSensors(devList, wholeBodyMASInertialsInterface.accelerometers, + metaData.sensorsList.linearAccelerometersList, interfaceType)) + { + return false; + } + } + + if (metaData.bridgeOptions.isGyroscopeEnabled) + { + std::string_view interfaceType{"IThreeAxisGyroscopes"}; + if (!attachAndCheckMASSensors(devList, wholeBodyMASInertialsInterface.gyroscopes, + metaData.sensorsList.gyroscopesList, interfaceType)) + { + return false; + } + } + + if (metaData.bridgeOptions.isOrientationSensorEnabled) + { + std::string_view interfaceType{"IOrientationSensors"}; + if (!attachAndCheckMASSensors(devList, wholeBodyMASInertialsInterface.orientationSensors, + metaData.sensorsList.orientationSensorsList, interfaceType)) + { + return false; + } + } + + if (metaData.bridgeOptions.isMagnetometerEnabled) + { + std::string_view interfaceType{"IThreeAxisMagnetometers"}; + if (!attachAndCheckMASSensors(devList, wholeBodyMASInertialsInterface.magnetometers, + metaData.sensorsList.magnetometersList, interfaceType)) + { + return false; + } + } + return true; + } + + /** + * Attach a remapped control board and check the availability of desired interface + * Further, resize joint data buffers and check if + * the control board joints list and the desired joints list match + * Also, maintain a remapping index buffer for adapting to arbitrary joint list serializations + */ + bool attachRemappedRemoteControlBoard(const yarp::dev::PolyDriverList& devList) + { + if (!metaData.bridgeOptions.isKinematicsEnabled) + { + // do nothing + return true; + } + + constexpr std::string_view logPrefix = "[YarpSensorBridge::Impl::attachRemappedRemoteControlBoard] "; + // expects only one remotecontrolboard device attached to it, if found break! + // if there multiple remote control boards, then use a remapper to create a single remotecontrolboard + bool ok{true}; + for (int devIdx = 0; devIdx < devList.size(); devIdx++) + { + ok = ok && devList[devIdx]->poly->view(controlBoardRemapperInterfaces.axis); + ok = ok && devList[devIdx]->poly->view(controlBoardRemapperInterfaces.encoders); + if (ok) + { + break; + } + } + + if (!ok) + { + std::cerr << logPrefix << " Could not find a remapped remote control board with the desired interfaces" << std::endl; + return false; + } + + resetControlBoardBuffers(); + if (!compareControlBoardJointsList()) + { + std::cerr << logPrefix << " Could not attach to remapped control board interface" << std::endl; + return false; + } + + return true; + } + + /** + * resize and set control board buffers to zero + */ + void resetControlBoardBuffers() + { + // firstly resize all the controlboard data buffers + controlBoardRemapperMeasures.remappedJointIndices.resize(metaData.bridgeOptions.nrJoints); + controlBoardRemapperMeasures.jointPositions.resize(metaData.bridgeOptions.nrJoints); + controlBoardRemapperMeasures.jointVelocities.resize(metaData.bridgeOptions.nrJoints); + + // zero buffers + controlBoardRemapperMeasures.remappedJointIndices.zero(); + controlBoardRemapperMeasures.jointPositions.zero(); + controlBoardRemapperMeasures.jointVelocities.zero(); + } + + /** + * check and match control board joints with the sensorbridge joints list + */ + bool compareControlBoardJointsList() + { + constexpr std::string_view logPrefix = "[YarpSensorBridge::Impl::compareControlBoardJointsList] "; + // get the names of all the joints available in the attached remote control board remapper + std::vector controlBoardJoints; + int controlBoardDOFs; + controlBoardRemapperInterfaces.encoders->getAxes(&controlBoardDOFs); + for (int DOF = 0; DOF < controlBoardDOFs; DOF++) + { + std::string joint; + controlBoardRemapperInterfaces.axis->getAxisName(DOF, joint); + controlBoardJoints.push_back(joint); + } + + // check if the joints in the desired joint list are available in the controlboard joints list + // if available get the control board index at which the desired joint is available + // this is required in order to remap the control board joints on to the desired joints + // this needs to be optimized with better STL algorithms + bool jointFound{false}; + for (int desiredDOF = 0; desiredDOF < metaData.sensorsList.jointsList.size(); desiredDOF++) + { + jointFound = false; + for (int DOF = 0; DOF < controlBoardDOFs; DOF++) + { + if (metaData.sensorsList.jointsList[desiredDOF] == controlBoardJoints[DOF]) + { + controlBoardRemapperMeasures.remappedJointIndices[desiredDOF] = DOF; + jointFound = true; + break; + } + } + + if (!jointFound) + { + std::cerr << logPrefix << " Could not find a desired joint from the configuration in the attached control board remapper." << std::endl; + return false; + } + } + + if (!jointFound) + { + return false; + } + + std::cout << logPrefix << "Found all joints with the remapped index " << std::endl; + for (int idx =0; idx < controlBoardRemapperMeasures.remappedJointIndices.size(); idx++) + { + std::cout << logPrefix << "Remapped Index: " << controlBoardRemapperMeasures.remappedJointIndices[idx] + << " , Joint name: " << controlBoardJoints[controlBoardRemapperMeasures.remappedJointIndices[idx]] + << std::endl; + } + return true; + } + + bool attachAllSixAxisForceTorqueSensors(const yarp::dev::PolyDriverList& devList) + { + if (!metaData.bridgeOptions.isSixAxisForceTorqueSensorEnabled) + { + // do nothing + return true; + } + + std::vector analogFTSensors; + // attach MAS sensors + if (attachRemappedMASSensor(devList, + wholeBodyMASForceTorquesInterface.sixAxisFTSensors)) + { + // get MAS FT sensor names + auto MASFTs = getAllSensorsInMASInterface(wholeBodyMASForceTorquesInterface.sixAxisFTSensors); + auto copySensorsList = metaData.sensorsList.sixAxisForceTorqueSensorsList; + + // compare with sensorList - those not available in the MAS interface list + // are assumed to be analog FT sensors + std::sort(MASFTs.begin(), MASFTs.end()); + std::sort(copySensorsList.begin(), copySensorsList.end()); + std::set_intersection(MASFTs.begin(), MASFTs.end(), + copySensorsList.begin(), copySensorsList.end(), + std::back_inserter(analogFTSensors)); + } + else + { + // if there are no MAS FT sensors then all the FT sensors in the configuration + // are analog FT sensors + analogFTSensors = metaData.sensorsList.sixAxisForceTorqueSensorsList; + } + + // check if a generic sensor has 12 channels implying it is a IMU sensor through a GenericSensor Interfaces + std::string_view interfaceType{"Analog Six Axis FT Interface"}; + if (!attachAllGenericOrAnalogSensors(devList, + wholeBodyAnalogSixAxisFTSensorsInterface, + nrChannelsInYARPAnalogSixAxisFTSensor, + analogFTSensors, + interfaceType)) + { + return false; + } + + return true; + } + + /** + * Attach to cartesian wrench interface + */ + bool attachCartesianWrenchInterface(const yarp::dev::PolyDriverList& devList) + { + if (!metaData.bridgeOptions.isCartesianWrenchEnabled) + { + // do nothing + return true; + } + + if (metaData.bridgeOptions.isCartesianWrenchEnabled) + { + // check if a generic sensor has 6 channels implying it is a cartesian wrench sensor through a GenericSensor Interfaces + std::string_view interfaceType{"Cartesian Wrench Interface"}; + if (!attachAllGenericOrAnalogSensors(devList, + wholeBodyCartesianWrenchInterface, + nrChannelsInYARPGenericCartesianWrench, + metaData.sensorsList.cartesianWrenchesList, + interfaceType)) + { + return false; + } + + } + return true; + } + + /** + * Attach all cameras + */ + bool attachAllCameras(const yarp::dev::PolyDriverList& devList) + { + if (!metaData.bridgeOptions.isCameraEnabled) + { + // do nothing + return true; + } + + std::string_view interfaceType{"RGB Cameras"}; + if (!attachAllCamerasOfSpecificType(devList, + metaData.sensorsList.rgbCamerasList, + metaData.bridgeOptions.rgbImgDimensions, + interfaceType, + wholeBodyFrameGrabberInterface, + wholeBodyCameraRGBImages)) + { + return false; + } + + std::string_view interfaceTypeDepth{"RGBD Cameras"}; + if (!attachAllCamerasOfSpecificType(devList, + metaData.sensorsList.rgbdCamerasList, + metaData.bridgeOptions.rgbdImgDimensions, + interfaceTypeDepth, + wholeBodyRGBDInterface, + wholeBodyCameraDepthImages)) + { + return false; + } + + // resize also rgb images of RGBD cameras + if (!resizeImageBuffers(metaData.sensorsList.rgbdCamerasList, + metaData.bridgeOptions.rgbdImgDimensions, + wholeBodyCameraRGBImages)) + { + return false; + } + + return true; + } + + /** + * Attach all cameras of specific type and resize image buffers + */ + template + bool attachAllCamerasOfSpecificType(const yarp::dev::PolyDriverList& devList, + const std::vector& camList, + const std::unordered_map >& imgDimensionsMap, + std::string_view interfaceType, + std::unordered_map& sensorMap, + std::unordered_map& imgBuffersMap) + { + constexpr std::string_view logPrefix = "[YarpSensorBridge::Impl::attachAllCamerasOfSpecificType] "; + for (auto cam : camList) + { + if (!attachCamera(devList, cam, sensorMap)) + { + return false; + } + } + + if (sensorMap.size() != camList.size()) + { + std::cout << logPrefix << " could not attach all desired cameras of type " << interfaceType << "." << std::endl; + return false; + } + + if (!resizeImageBuffers(camList, + imgDimensionsMap, + imgBuffersMap)) + { + std::cout << logPrefix << " Failed to resize camera buffers of type " << interfaceType << "." << std::endl; + return false; + } + return true; + } + + /** + * Resize image buffers + */ + bool resizeImageBuffers(const std::vector& camList, + const std::unordered_map >& imgDimensionsMap, + std::unordered_map& imgBuffersMap) + { + for (const auto& cam : camList) + { + auto iter = imgDimensionsMap.find(cam); + if (iter == imgDimensionsMap.end()) + { + return false; + } + + auto imgDim = iter->second; + imgBuffersMap[cam].first.resize(imgDim.first, imgDim.second); + } + return true; + } + + /** + * utility function + */ + double deg2rad(double deg) + { + return deg * M_PI / 180.0; + } + + /** + * Read generic or analog sensor stream and update internal measurement buffer + */ + template + bool readAnalogOrGenericSensor(const std::string& sensorName, + const int& nrChannelsInYARPSensor, + std::unordered_map& interfaceMap, + std::unordered_map& measurementMap, + bool checkForNan = false) + { + bool ok{true}; + constexpr std::string_view logPrefix = "[YarpSensorBridge::Impl::readAnalogOrGenericSensor] "; + if (!checkSensor(interfaceMap, sensorName)) + { + return false; + } + + auto iter = interfaceMap.find(sensorName); + auto interface = iter->second; + + yarp::sig::Vector sensorMeasure; + sensorMeasure.resize(nrChannelsInYARPSensor); + + if constexpr (std::is_same_v) + { + auto retValue = interface->read(sensorMeasure); + ok = ok && (retValue == yarp::dev::IAnalogSensor::AS_OK); + } + else if constexpr (std::is_same_v) + { + ok = ok && interface->read(sensorMeasure); + } + + if (!ok) + { + std::cerr << logPrefix << " Unable to read from " << sensorName << " , use previous measurement" << std::endl; + return false; + } + + + if (checkForNan) + { + if (std::binary_search(sensorMeasure.begin(), sensorMeasure.end(), NAN)) + { + std::cerr << logPrefix << " NAN values read from " << sensorName << " , use previous measurement" << std::endl; + return false; + } + } + + // for IMU measurements convert angular velocity measurements to radians per s + // and convert orientation measurements to radians + // enforcing the assumption that if dimensions of sensor channel is 12 then its an IMU sensor + if (nrChannelsInYARPSensor == nrChannelsInYARPGenericIMUSensor) + { + // See http://wiki.icub.org/wiki/Inertial_Sensor + // 0, 1 and 2 indices correspond to orientation measurements + sensorMeasure[0] = deg2rad(sensorMeasure[0]); + sensorMeasure[1] = deg2rad(sensorMeasure[1]); + sensorMeasure[2] = deg2rad(sensorMeasure[2]); + // 6, 7 and 8 indices correspond to angular velocity measurements + sensorMeasure[6] = deg2rad(sensorMeasure[6]); + sensorMeasure[7] = deg2rad(sensorMeasure[7]); + sensorMeasure[8] = deg2rad(sensorMeasure[8]); + } + + measurementMap[sensorName].first = sensorMeasure; + measurementMap[sensorName].second = yarp::os::Time::now(); + return true; + } + + + template + bool readMASSensor(MASSensorType* interface, + const std::string& sensorName, + const std::unordered_map& sensorIdxMap, + std::unordered_map& measurementMap, + bool checkForNan = false) + { + constexpr std::string_view logPrefix = "[YarpSensorBridge::Impl::readMASSensor] "; + auto iter = sensorIdxMap.find(sensorName); + if (iter == sensorIdxMap.end()) + { + return false; + } + + std::size_t sensIdx = iter->second; + yarp::sig::Vector sensorMeasure; + bool ok{false}; + double txTimeStamp; + if constexpr (std::is_same_v) + { + constexpr int dim{3}; + sensorMeasure.resize(dim); + ok = interface->getThreeAxisGyroscopeMeasure(sensIdx, sensorMeasure, txTimeStamp); + + // convert YARP measures from deg/s to rad/s + for (std::size_t elemIdx = 0; elemIdx < dim; elemIdx++) + { + sensorMeasure(elemIdx) = deg2rad(sensorMeasure(elemIdx)); + } + } + else if constexpr (std::is_same_v) + { + constexpr int dim{3}; + sensorMeasure.resize(dim); + ok = interface->getThreeAxisLinearAccelerometerMeasure(sensIdx, sensorMeasure, txTimeStamp); + } + else if constexpr (std::is_same_v) + { + constexpr int dim{3}; + sensorMeasure.resize(dim); + ok = interface->getThreeAxisMagnetometerMeasure(sensIdx, sensorMeasure, txTimeStamp); + } + else if constexpr (std::is_same_v) + { + constexpr int dim{3}; + sensorMeasure.resize(dim); + ok = interface->getOrientationSensorMeasureAsRollPitchYaw(sensIdx, sensorMeasure, txTimeStamp); + // convert YARP measures from deg to rad + for (std::size_t elemIdx = 0; elemIdx < dim; elemIdx++) + { + sensorMeasure(elemIdx) = deg2rad(sensorMeasure(elemIdx)); + } + } + else if constexpr (std::is_same_v) + { + constexpr int dim{6}; + sensorMeasure.resize(dim); + ok = interface->getSixAxisForceTorqueSensorMeasure(sensIdx, sensorMeasure, txTimeStamp); + } + + if (!ok) + { + std::cerr << logPrefix << " Unable to read from " << sensorName << ", use previous measurement" << std::endl; + return false; + } + + if (checkForNan) + { + if (std::binary_search(sensorMeasure.begin(), sensorMeasure.end(), NAN)) + { + std::cerr << logPrefix << " NAN values read from " << sensorName << ", use previous measurement" << std::endl; + return false; + } + } + + measurementMap[sensorName].first = sensorMeasure; + measurementMap[sensorName].second = yarp::os::Time::now(); + + return true; + } + + template + bool readAllMASSensors(MASSensorType* interface, + std::unordered_map sensIdxMap, + std::unordered_map& measurementMap, + std::vector& failedSensorReads, + bool checkForNan = false) + { + constexpr std::string_view logPrefix = "[YarpSensorBridge::Impl::readAllMASSensors] "; + bool allSensorsReadCorrectly{true}; + failedSensorReads.clear(); + for( auto const& sensorIdx : sensIdxMap ) + { + const auto& sensorName = sensorIdx.first; + bool ok = readMASSensor(interface, + sensorName, + sensIdxMap, + measurementMap, + checkForNan); + if (!ok) + { + std::cerr << logPrefix << " Read MAS sensor failed for " << sensorName << std::endl; + failedSensorReads.emplace_back(sensorName); + } + allSensorsReadCorrectly = ok && allSensorsReadCorrectly; + } + + return allSensorsReadCorrectly; + } + + template + bool readCameraImage(const std::string& cameraName, + const std::string& imageType, + std::unordered_map& interfaceMap, + std::unordered_map& imageMap) + { + bool ok{true}; + constexpr std::string_view logPrefix = "[YarpSensorBridge::Impl::readCameraImage] "; + + if (!checkSensor(interfaceMap, cameraName)) + { + return false; + } + + auto iter = interfaceMap.find(cameraName); + auto interface = iter->second; + + // StampedYARPImage is defined as a pair of yarp::sig::image and double + yarp::os::Stamp* txTimestamp{nullptr}; + if constexpr (std::is_same_v) + { + if (imageType != "RGB") + { + std::cerr << logPrefix << " Frame Grabber " << cameraName << "handles only RGB image" << std::endl; + return false; + } + yarp::sig::ImageOf& img = dynamic_cast&> (imageMap[cameraName].first); + ok = interface->getImage(img); + } + else if constexpr (std::is_same_v) + { + if (imageType == "RGB") + { + yarp::sig::FlexImage& img = dynamic_cast (imageMap[cameraName].first); + img.setPixelCode(VOCAB_PIXEL_RGB); + ok = interface->getRgbImage(img, txTimestamp); + + } + else if (imageType == "DEPTH") + { + yarp::sig::ImageOf& img = dynamic_cast&> (imageMap[cameraName].first); + ok = interface->getDepthImage(img, txTimestamp); + } + } + + if (!ok) + { + std::cerr << logPrefix << " Unable to read from " << cameraName << ", use previous image" << std::endl; + return false; + } + + imageMap[cameraName].second = yarp::os::Time::now(); + return true; + } + + /** + * Read control board remapper interfaces + */ + bool readControlBoardInterface(bool checkForNan = false) + { + if (!metaData.bridgeOptions.isKinematicsEnabled) + { + // do nothing + return true; + } + + constexpr std::string_view logPrefix = "[YarpSensorBridge::Impl::readControlBoardInterface] "; + yarp::sig::Vector tempJointPositions, tempJointVelocities; + tempJointPositions.resize(controlBoardRemapperMeasures.jointPositions.size()); + tempJointVelocities.resize(controlBoardRemapperMeasures.jointPositions.size()); + + bool ok{true}; + ok = ok && controlBoardRemapperInterfaces.encoders->getEncoders(tempJointPositions.data()); + ok = ok && controlBoardRemapperInterfaces.encoders->getEncoderSpeeds(tempJointVelocities.data()); + + if (!ok) + { + std::cerr << logPrefix << " Unable to read from encoders interface, use previous measurement" << std::endl; + return false; + } + + if (checkForNan) + { + if (std::binary_search(tempJointPositions.begin(), tempJointPositions.end(), NAN)) + { + std::cerr << logPrefix << " NAN values read from encoders interface, use previous measurement" << std::endl; + return false; + } + + if (std::binary_search(tempJointVelocities.begin(), tempJointVelocities.end(), NAN)) + { + std::cerr << logPrefix << " NAN values read from encoders interface, use previous measurement" << std::endl; + return false; + } + } + + // convert from degrees to radians - YARP convention is to store joint positions in degrees + for (std::size_t idx = 0; idx < tempJointPositions.size(); idx++) + { + controlBoardRemapperMeasures.jointPositions[idx] = deg2rad(tempJointPositions[controlBoardRemapperMeasures.remappedJointIndices[idx]]); + controlBoardRemapperMeasures.jointVelocities[idx] = deg2rad(tempJointVelocities[controlBoardRemapperMeasures.remappedJointIndices[idx]]); + } + + controlBoardRemapperMeasures.receivedTimeInSeconds = yarp::os::Time::now(); + + return true; + } + + bool readAllIMUs(std::vector& failedSensorReads) + { + if (!metaData.bridgeOptions.isIMUEnabled) + { + // do nothing + return true; + } + + constexpr std::string_view logPrefix = "[YarpSensorBridge::Impl::readAllIMUs] "; + bool allIMUsReadCorrectly{true}; + failedSensorReads.clear(); + for( auto const& imu : wholeBodyAnalogIMUInterface ) + { + const auto& imuName = imu.first; + bool ok = readAnalogOrGenericSensor(imuName, + nrChannelsInYARPGenericIMUSensor, + wholeBodyAnalogIMUInterface, + wholeBodyIMUMeasures, + checkForNAN); + if (!ok) + { + std::cerr << logPrefix << " Read IMU failed for " << imuName << std::endl; + failedSensorReads.emplace_back(imuName); + } + allIMUsReadCorrectly = ok && allIMUsReadCorrectly; + } + + return allIMUsReadCorrectly; + } + + bool readAllCartesianWrenches(std::vector& failedSensorReads) + { + if (!metaData.bridgeOptions.isCartesianWrenchEnabled) + { + // do nothing + return true; + } + + constexpr std::string_view logPrefix = "[YarpSensorBridge::Impl::readAllCartesianWrenches] "; + bool allWrenchesReadCorrectly{true}; + failedSensorReads.clear(); + for( auto const& wrenchUnit : wholeBodyCartesianWrenchInterface ) + { + const auto& wrenchName = wrenchUnit.first; + bool ok = readAnalogOrGenericSensor(wrenchName, + nrChannelsInYARPGenericCartesianWrench, + wholeBodyCartesianWrenchInterface, + wholeBodyCartesianWrenchMeasures, + checkForNAN); + if (!ok) + { + std::cerr << logPrefix << " Read cartesian wrench failed for " << wrenchName << std::endl; + failedSensorReads.emplace_back(wrenchName); + } + allWrenchesReadCorrectly = ok && allWrenchesReadCorrectly; + } + + return allWrenchesReadCorrectly; + } + + bool readAllMASLinearAccelerometers(std::vector& failedSensorReads) + { + if (!metaData.bridgeOptions.isLinearAccelerometerEnabled) + { + // do nothing + return true; + } + + return readAllMASSensors(wholeBodyMASInertialsInterface.accelerometers, + masSensorIndexMaps.accelerometer, + wholeBodyInertialMeasures, + failedSensorReads, + checkForNAN); + } + + bool readAllMASGyroscopes(std::vector& failedSensorReads) + { + if (!metaData.bridgeOptions.isGyroscopeEnabled) + { + // do nothing + return true; + } + + return readAllMASSensors(wholeBodyMASInertialsInterface.gyroscopes, + masSensorIndexMaps.gyroscopes, + wholeBodyInertialMeasures, + failedSensorReads, + checkForNAN); + } + + bool readAllMASOrientationSensors(std::vector& failedSensorReads) + { + if (!metaData.bridgeOptions.isOrientationSensorEnabled) + { + // do nothing + return true; + } + + return readAllMASSensors(wholeBodyMASInertialsInterface.orientationSensors, + masSensorIndexMaps.orientationSensors, + wholeBodyInertialMeasures, + failedSensorReads, + checkForNAN); + } + + bool readAllMASMagnetometers(std::vector& failedSensorReads) + { + if (!metaData.bridgeOptions.isMagnetometerEnabled) + { + // do nothing + return true; + } + + return readAllMASSensors(wholeBodyMASInertialsInterface.magnetometers, + masSensorIndexMaps.magnetometers, + wholeBodyInertialMeasures, + failedSensorReads, + checkForNAN); + } + + bool readAllMASSixAxisForceTorqueSensors(std::vector& failedSensorReads) + { + if (!metaData.bridgeOptions.isSixAxisForceTorqueSensorEnabled) + { + // do nothing + return true; + } + + return readAllMASSensors(wholeBodyMASForceTorquesInterface.sixAxisFTSensors, + masSensorIndexMaps.sixAxisFTSensors, + wholeBodyFTMeasures, + failedSensorReads, + checkForNAN); + } + + bool readAllFrameGrabberCameras(std::vector& failedSensorReads) + { + if (!metaData.bridgeOptions.isCameraEnabled) + { + // do nothing + return true; + } + + constexpr std::string_view logPrefix = "[YarpSensorBridge::Impl::readAllFrameGrabberCameras] "; + bool allRGBCamerasReadCorrectly{true}; + failedSensorReads.clear(); + for( auto const& camera : wholeBodyFrameGrabberInterface ) + { + std::string imageType{"RGB"}; + const auto& cameraName = camera.first; + bool ok = readCameraImage(cameraName, + imageType, + wholeBodyFrameGrabberInterface, + wholeBodyCameraRGBImages); + if (!ok) + { + std::cerr << logPrefix << " Read RGB image failed for " << cameraName << std::endl; + failedSensorReads.emplace_back(cameraName); + } + allRGBCamerasReadCorrectly = ok && allRGBCamerasReadCorrectly; + } + + return allRGBCamerasReadCorrectly; + } + + bool readAllRGBDCameras(std::vector& failedSensorReads) + { + if (!metaData.bridgeOptions.isCameraEnabled) + { + // do nothing + return true; + } + + constexpr std::string_view logPrefix = "[YarpSensorBridge::Impl::readAllRGBDCameras] "; + bool allRGBDCamerasReadCorrectly{true}; + failedSensorReads.clear(); + for( auto const& camera : wholeBodyRGBDInterface ) + { + std::string imageType{"RGB"}; + const auto& cameraName = camera.first; + bool ok = readCameraImage(cameraName, + imageType, + wholeBodyRGBDInterface, + wholeBodyCameraRGBImages); + + imageType = "DEPTH"; + ok = readCameraImage(cameraName, + imageType, + wholeBodyRGBDInterface, + wholeBodyCameraDepthImages) && ok; + + if (!ok) + { + std::cerr << logPrefix << " Read RGB/Depth image failed for " << cameraName << std::endl; + failedSensorReads.emplace_back(cameraName); + } + + allRGBDCamerasReadCorrectly = ok && allRGBDCamerasReadCorrectly; + } + + return allRGBDCamerasReadCorrectly; + } + + bool readAllSensors(std::vector& failedReadAllSensors) + { + failedReadAllSensors.clear(); + std::vector failedReads; + + if (readControlBoardInterface(checkForNAN)) + { + failedReadAllSensors.emplace_back(std::string("RemoteControlBoardRemapper")); + } + + if (!readAllIMUs(failedReads)) + { + failedReadAllSensors.insert(failedReadAllSensors.end(), failedReads.begin(), failedReads.end()); + } + + if (!readAllCartesianWrenches(failedReads)) + { + failedReadAllSensors.insert(failedReadAllSensors.end(), failedReads.begin(), failedReads.end()); + } + + if (!readAllMASLinearAccelerometers(failedReads)) + { + failedReadAllSensors.insert(failedReadAllSensors.end(), failedReads.begin(), failedReads.end()); + } + + if (!readAllMASGyroscopes(failedReads)) + { + failedReadAllSensors.insert(failedReadAllSensors.end(), failedReads.begin(), failedReads.end()); + } + + if (!readAllMASOrientationSensors(failedReads)) + { + failedReadAllSensors.insert(failedReadAllSensors.end(), failedReads.begin(), failedReads.end()); + } + + if (!readAllMASMagnetometers(failedReads)) + { + failedReadAllSensors.insert(failedReadAllSensors.end(), failedReads.begin(), failedReads.end()); + } + + if (!readAllMASSixAxisForceTorqueSensors(failedReads)) + { + failedReadAllSensors.insert(failedReadAllSensors.end(), failedReads.begin(), failedReads.end()); + } + + if (!readAllRGBDCameras(failedReads)) + { + failedReadAllSensors.insert(failedReadAllSensors.end(), failedReads.begin(), failedReads.end()); + } + + if (!readAllFrameGrabberCameras(failedReads)) + { + failedReadAllSensors.insert(failedReadAllSensors.end(), failedReads.begin(), failedReads.end()); + } + return true; + } +}; + +} +} +#endif // BIPEDAL_LOCOMOTION_ROBOT_INTERFACE_YARP_SENSOR_BRIDGE_IMPL_H + + diff --git a/src/RobotInterface/YarpImplementation/src/YarpSensorBridge.cpp b/src/RobotInterface/YarpImplementation/src/YarpSensorBridge.cpp new file mode 100644 index 0000000000..66d8af88b9 --- /dev/null +++ b/src/RobotInterface/YarpImplementation/src/YarpSensorBridge.cpp @@ -0,0 +1,444 @@ +/** + * @file YarpSensorBridge.cpp + * @authors Prashanth Ramadoss + * @copyright 2020 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version. + */ + +#include +#include + +using namespace BipedalLocomotion::RobotInterface; +using namespace BipedalLocomotion::GenericContainer; +using namespace BipedalLocomotion::ParametersHandler; + +YarpSensorBridge::YarpSensorBridge() : m_pimpl(std::make_unique()) +{ +} + +YarpSensorBridge::~YarpSensorBridge() = default; + +bool YarpSensorBridge::initialize(std::weak_ptr handler) +{ + constexpr std::string_view logPrefix = "[YarpSensorBridge::initialize] "; + + auto ptr = handler.lock(); + if (ptr == nullptr) + { + std::cerr << logPrefix << "The handler is not pointing to an already initialized memory." + << std ::endl; + return false; + } + + int temp; + ptr->getParameter("check_for_nan", temp); + m_pimpl->checkForNAN = static_cast(temp); + + bool ret{true}; + ret = ret && m_pimpl->subConfigLoader("stream_joint_states", "RemoteControlBoardRemapper", + &YarpSensorBridge::Impl::configureRemoteControlBoardRemapper, + handler, + m_pimpl->metaData, + m_pimpl->metaData.bridgeOptions.isKinematicsEnabled); + + bool useInertialSensors{false}; + ret = ret && m_pimpl->subConfigLoader("stream_inertials", "InertialSensors", + &YarpSensorBridge::Impl::configureInertialSensors, + handler, + m_pimpl->metaData, + useInertialSensors); + + ret = ret && m_pimpl->subConfigLoader("stream_forcetorque_sensors", "SixAxisForceTorqueSensors", + &YarpSensorBridge::Impl::configureSixAxisForceTorqueSensors, + handler, + m_pimpl->metaData, + m_pimpl->metaData.bridgeOptions.isSixAxisForceTorqueSensorEnabled); + + ret = ret && m_pimpl->subConfigLoader("stream_cartesian_wrenches", "CartesianWrenches", + &YarpSensorBridge::Impl::configureCartesianWrenches, + handler, + m_pimpl->metaData, + m_pimpl->metaData.bridgeOptions.isCartesianWrenchEnabled); + + bool useCameras{false}; + ret = ret && m_pimpl->subConfigLoader("stream_cameras", "Cameras", + &YarpSensorBridge::Impl::configureCameras, + handler, + m_pimpl->metaData, + useCameras); + + + m_pimpl->bridgeInitialized = true; + return true; +} + +bool YarpSensorBridge::setDriversList(const yarp::dev::PolyDriverList& deviceDriversList) +{ + constexpr std::string_view logPrefix = "[YarpSensorBridge::setDriversList] "; + + if (!m_pimpl->bridgeInitialized) + { + std::cerr << logPrefix << "Please initialize YarpSensorBridge before calling setDriversList(...)." + << std ::endl; + return false; + } + + bool ret{true}; + ret = ret && m_pimpl->attachRemappedRemoteControlBoard(deviceDriversList); + ret = ret && m_pimpl->attachAllInertials(deviceDriversList); + ret = ret && m_pimpl->attachAllSixAxisForceTorqueSensors(deviceDriversList); + ret = ret && m_pimpl->attachCartesianWrenchInterface(deviceDriversList); + ret = ret && m_pimpl->attachAllCameras(deviceDriversList); + + if (!ret) + { + std::cerr << logPrefix << "Failed to attach to one or more device drivers." + << std ::endl; + return false; + } + m_pimpl->driversAttached = true; + return true; +} + +bool YarpSensorBridge::advance() +{ + constexpr std::string_view logPrefix = "[YarpSensorBridge::advance] "; + if (!m_pimpl->checkValid("[YarpSensorBridge::advance]")) + { + std::cerr << logPrefix << "Please initialize and set drivers list before running advance()." + << std ::endl; + return false; + } + + m_pimpl->readAllSensors(m_pimpl->failedSensorReads); + + return true; +} + +bool YarpSensorBridge::isValid() const +{ + return m_pimpl->checkValid("[YarpSensorBridge::isValid]"); +} + +const SensorBridgeMetaData& YarpSensorBridge::get() const { return m_pimpl->metaData; } + +bool YarpSensorBridge::getJointsList(std::vector& jointsList) +{ + if (!m_pimpl->checkValid("[YarpSensorBridge::getJointsList]")) + { + return false; + } + jointsList = m_pimpl->metaData.sensorsList.jointsList; + return true; +} + +bool YarpSensorBridge::getIMUsList(std::vector& IMUsList) +{ + if (!m_pimpl->checkValid("[YarpSensorBridge::getIMUsList]")) + { + return false; + } + IMUsList = m_pimpl->metaData.sensorsList.IMUsList; + return true; +} + +bool YarpSensorBridge::getLinearAccelerometersList(std::vector& linearAccelerometersList) +{ + if (!m_pimpl->checkValid("[YarpSensorBridge::getLinearAccelerometersList]")) + { + return false; + } + linearAccelerometersList = m_pimpl->metaData.sensorsList.linearAccelerometersList; + return true; +} + +bool YarpSensorBridge::getGyroscopesList(std::vector& gyroscopesList) +{ + if (!m_pimpl->checkValid("[YarpSensorBridge::getGyroscopesList]")) + { + return false; + } + gyroscopesList = m_pimpl->metaData.sensorsList.gyroscopesList; + return true; +} + +bool YarpSensorBridge::getOrientationSensorsList(std::vector& orientationSensorsList) +{ + if (!m_pimpl->checkValid("[YarpSensorBridge::getOrientationSensorsList]")) + { + return false; + } + orientationSensorsList = m_pimpl->metaData.sensorsList.orientationSensorsList; + return true; +} + +bool YarpSensorBridge::getMagnetometersList(std::vector& magnetometersList) +{ + if (!m_pimpl->checkValid("[YarpSensorBridge::getMagnetometersList]")) + { + return false; + } + magnetometersList = m_pimpl->metaData.sensorsList.magnetometersList; + return true; +} + +bool YarpSensorBridge::getSixAxisForceTorqueSensorsList(std::vector& sixAxisForceTorqueSensorsList) +{ + if (!m_pimpl->checkValid("[YarpSensorBridge::getSixAxisForceTorqueSensorsList]")) + { + return false; + } + sixAxisForceTorqueSensorsList = m_pimpl->metaData.sensorsList.sixAxisForceTorqueSensorsList; + return true; +} + +bool YarpSensorBridge::getCartesianWrenchesList(std::vector& cartesianWrenchesList) +{ + if (!m_pimpl->checkValid("[YarpSensorBridge::getCartesianWrenchesList]")) + { + return false; + } + cartesianWrenchesList = m_pimpl->metaData.sensorsList.cartesianWrenchesList; + return true; +} + +bool YarpSensorBridge::getRGBCamerasList(std::vector& rgbCamerasList) +{ + if (!m_pimpl->checkValid("[YarpSensorBridge::getRGBCamerasList]")) + { + return false; + } + rgbCamerasList = m_pimpl->metaData.sensorsList.rgbCamerasList; + return true; +} + +bool YarpSensorBridge::getRGBDCamerasList(std::vector& rgbdCamerasList) +{ + if (!m_pimpl->checkValid("[YarpSensorBridge::getRGBDCamerasList]")) + { + return false; + } + rgbdCamerasList = m_pimpl->metaData.sensorsList.rgbdCamerasList; + return true; +} + +bool YarpSensorBridge::getJointPosition(const std::string& jointName, + double& jointPosition, + double* receiveTimeInSeconds) +{ + int idx; + if (!m_pimpl->getIndexFromVector(m_pimpl->metaData.sensorsList.jointsList, + jointName, idx)) + { + std::cerr << "[YarpSensorBridge::getJointVelocity] " << jointName + << " could not be found in the configured list of joints" << std::endl; + return false; + } + + jointPosition = m_pimpl->controlBoardRemapperMeasures.jointPositions[idx]; + receiveTimeInSeconds = &m_pimpl->controlBoardRemapperMeasures.receivedTimeInSeconds; + + return true; +} + +bool YarpSensorBridge::getJointPositions(Eigen::Ref jointPositions, + double* receiveTimeInSeconds) +{ + + jointPositions = yarp::eigen::toEigen(m_pimpl->controlBoardRemapperMeasures.jointPositions); + receiveTimeInSeconds = &m_pimpl->controlBoardRemapperMeasures.receivedTimeInSeconds; + return true; +} + +bool YarpSensorBridge::getJointVelocity(const std::string& jointName, + double& jointVelocity, + double* receiveTimeInSeconds) +{ + int idx; + if (!m_pimpl->getIndexFromVector(m_pimpl->metaData.sensorsList.jointsList, + jointName, idx)) + { + std::cerr << "[YarpSensorBridge::getJointVelocity] " << jointName + << " could not be found in the configured list of joints" << std::endl; + return false; + } + + jointVelocity = m_pimpl->controlBoardRemapperMeasures.jointVelocities[idx]; + receiveTimeInSeconds = &m_pimpl->controlBoardRemapperMeasures.receivedTimeInSeconds; + + return true; +} + +bool YarpSensorBridge::getJointVelocities(Eigen::Ref jointVelocties, + double* receiveTimeInSeconds) +{ + jointVelocties = yarp::eigen::toEigen(m_pimpl->controlBoardRemapperMeasures.jointVelocities); + receiveTimeInSeconds = &m_pimpl->controlBoardRemapperMeasures.receivedTimeInSeconds; + return true; +} + +bool YarpSensorBridge::getIMUMeasurement(const std::string& imuName, + Eigen::Ref imuMeasurement, + double* receiveTimeInSeconds) +{ + if (!m_pimpl->checkValidSensorMeasure("YarpSensorBridge::getIMUMeasurement ", + m_pimpl->wholeBodyIMUMeasures, imuName)) + { + return false; + } + + auto iter = m_pimpl->wholeBodyIMUMeasures.find(imuName); + imuMeasurement = yarp::eigen::toEigen(iter->second.first); + receiveTimeInSeconds = &iter->second.second; + return true; +} + +bool YarpSensorBridge::getLinearAccelerometerMeasurement(const std::string& accName, + Eigen::Ref accMeasurement, + double* receiveTimeInSeconds) +{ + if (!m_pimpl->checkValidSensorMeasure("YarpSensorBridge::getLinearAccelerometerMeasurement ", + m_pimpl->wholeBodyInertialMeasures, accName)) + { + return false; + } + + auto iter = m_pimpl->wholeBodyInertialMeasures.find(accName); + accMeasurement = yarp::eigen::toEigen(iter->second.first); + receiveTimeInSeconds = &iter->second.second; + return true; +} + +bool YarpSensorBridge::getGyroscopeMeasure(const std::string& gyroName, + Eigen::Ref gyroMeasurement, + double* receiveTimeInSeconds) +{ + if (!m_pimpl->checkValidSensorMeasure("YarpSensorBridge::getGyroscopeMeasure ", + m_pimpl->wholeBodyInertialMeasures, gyroName)) + { + return false; + } + + auto iter = m_pimpl->wholeBodyInertialMeasures.find(gyroName); + gyroMeasurement = yarp::eigen::toEigen(iter->second.first); + receiveTimeInSeconds = &iter->second.second; + return true; +} + + +bool YarpSensorBridge::getOrientationSensorMeasurement(const std::string& rpyName, + Eigen::Ref rpyMeasurement, + double* receiveTimeInSeconds) +{ + if (!m_pimpl->checkValidSensorMeasure("YarpSensorBridge::getOrientationSensorMeasurement ", + m_pimpl->wholeBodyInertialMeasures, rpyName)) + { + return false; + } + + auto iter = m_pimpl->wholeBodyInertialMeasures.find(rpyName); + rpyMeasurement = yarp::eigen::toEigen(iter->second.first); + receiveTimeInSeconds = &iter->second.second; + return true; +} + +bool YarpSensorBridge::getMagnetometerMeasurement(const std::string& magName, + Eigen::Ref magMeasurement, + double* receiveTimeInSeconds) +{ + if (!m_pimpl->checkValidSensorMeasure("YarpSensorBridge::getMagnetometerMeasurement ", + m_pimpl->wholeBodyInertialMeasures, magName)) + { + return false; + } + + auto iter = m_pimpl->wholeBodyInertialMeasures.find(magName); + magMeasurement = yarp::eigen::toEigen(iter->second.first); + receiveTimeInSeconds = &iter->second.second; + return true; +} + +bool YarpSensorBridge::getSixAxisForceTorqueMeasurement(const std::string& ftName, + Eigen::Ref ftMeasurement, + double* receiveTimeInSeconds) +{ + if (!m_pimpl->checkValidSensorMeasure("YarpSensorBridge::getSixAxisForceTorqueMeasurement ", + m_pimpl->wholeBodyFTMeasures, ftName)) + { + return false; + } + + auto iter = m_pimpl->wholeBodyFTMeasures.find(ftName); + ftMeasurement = yarp::eigen::toEigen(iter->second.first); + receiveTimeInSeconds = &iter->second.second; + return true; +} + +bool YarpSensorBridge::getCartesianWrench(const std::string& cartesianWrenchName, + Eigen::Ref cartesianWrenchMeasurement, + double* receiveTimeInSeconds) +{ + if (!m_pimpl->checkValidSensorMeasure("YarpSensorBridge::getCartesianWrench ", + m_pimpl->wholeBodyCartesianWrenchMeasures, cartesianWrenchName)) + { + return false; + } + + auto iter = m_pimpl->wholeBodyCartesianWrenchMeasures.find(cartesianWrenchName); + cartesianWrenchMeasurement = yarp::eigen::toEigen(iter->second.first); + receiveTimeInSeconds = &iter->second.second; + return true; +} + +bool YarpSensorBridge::getColorImage(const std::string& camName, + Eigen::Ref colorImg, + double* receiveTimeInSeconds) +{ + if (!m_pimpl->checkValidSensorMeasure("YarpSensorBridge::getColorImage ", + m_pimpl->wholeBodyCameraRGBImages, camName)) + { + return false; + } + + auto iter = m_pimpl->wholeBodyCameraRGBImages.find(camName); + // TODO understand the conversion from YARP image to Eigen + receiveTimeInSeconds = &iter->second.second; + + std::cerr << "This method is currently unimplemented. Need to implement toEigen for yarp images" << std::endl; + return false; +} + +bool YarpSensorBridge::getDepthImage(const std::string& camName, + Eigen::Ref depthImg, + double* receiveTimeInSeconds) +{ + if (!m_pimpl->checkValidSensorMeasure("YarpSensorBridge::getDepthImage ", + m_pimpl->wholeBodyCameraDepthImages, camName)) + { + return false; + } + + auto iter = m_pimpl->wholeBodyCameraDepthImages.find(camName); + // TODO understand the conversion from YARP image to Eigen + receiveTimeInSeconds = &iter->second.second; + + std::cerr << "This method is currently unimplemented. Need to implement toEigen for yarp images" << std::endl; + return false; +} + +bool YarpSensorBridge::getThreeAxisForceTorqueMeasurement(const std::string& ftName, + Eigen::Ref ftMeasurement, + double* receiveTimeInSeconds) +{ + constexpr std::string_view error = "[YarpSensorBridge::getThreeAxisForceTorqueMeasurement] Currently unimplemented"; + std::cerr << error << std::endl; + return false; +} + +bool YarpSensorBridge::getThreeAxisForceTorqueSensorsList(std::vector& threeAxisForceTorqueSensorsList) +{ + constexpr std::string_view error = "[YarpSensorBridge::getThreeAxisForceTorqueSensorsList] Currently unimplemented"; + std::cerr << error << std::endl; + + return false; +} diff --git a/src/RobotInterface/include/BipedalLocomotion/RobotInterface/ISensorBridge.h b/src/RobotInterface/include/BipedalLocomotion/RobotInterface/ISensorBridge.h index 8b85ceb001..b468a8ba4e 100644 --- a/src/RobotInterface/include/BipedalLocomotion/RobotInterface/ISensorBridge.h +++ b/src/RobotInterface/include/BipedalLocomotion/RobotInterface/ISensorBridge.h @@ -17,7 +17,6 @@ #include -using namespace BipedalLocomotion::ParametersHandler; using Vector12d = Eigen::Matrix; using Vector6d = Eigen::Matrix; @@ -35,6 +34,7 @@ struct SensorBridgeOptions bool isIMUEnabled{false}; /**< flag to connect IMU measurement sources */ bool isLinearAccelerometerEnabled{false}; /**< flag to connect linear accelerometer measurement sources */ bool isGyroscopeEnabled{false}; /**< flag to connect gyroscope measurement sources */ + bool isOrientationSensorEnabled{false}; /**< flag to connect gyroscope measurement sources */ bool isMagnetometerEnabled{false}; /**< flag to connect magnetometer measurement sources */ bool isSixAxisForceTorqueSensorEnabled{false}; /**< flag to connect six axis force torque measurement sources */ bool isThreeAxisForceTorqueSensorEnabled{false}; /**< flag to connect six axis force torque measurement sources */ @@ -42,7 +42,8 @@ struct SensorBridgeOptions bool isCameraEnabled{false}; /**< flag to connect camera sources */ size_t nrJoints{0}; /**< number of joints available through Kinematics stream, to be configured at initialization */ - std::unordered_map > imgDimensions; /**< dimensions of the camera images available through camera streams, to be configured at initialization */ + std::unordered_map > rgbImgDimensions; /**< dimensions of the images available through rgb camera streams, to be configured at initialization */ + std::unordered_map > rgbdImgDimensions; /**< dimensions of the depth images available through rgbd camera streams, to be configured at initialization */ }; /** @@ -60,7 +61,18 @@ struct SensorLists std::vector threeAxisForceTorqueSensorsList; /**< list of three axis force torque sensors attached to the bridge */ std::vector cartesianWrenchesList; /**< list of cartesian wrench streams attached to the bridge */ std::vector rgbCamerasList; /**< list of rgb cameras attached to the bridge */ - std::vector depthCamerasList; /**< list of depth cameras attached to the bridge */ + std::vector rgbdCamerasList; /**< list of RGBD cameras attached to the bridge */ +}; + + +/** + * Meta data struct to hold list of sensors and configured options + * available from the Sensor bridge interface + */ +struct SensorBridgeMetaData +{ + SensorLists sensorsList; + SensorBridgeOptions bridgeOptions; }; /** @@ -79,84 +91,84 @@ class ISensorBridge * Initialize estimator * @param[in] handler Parameters handler */ - virtual bool initialize(std::weak_ptr handler) = 0; + virtual bool initialize(std::weak_ptr handler) = 0; /** * Get joints list * @param[out] jointsList list of joints attached to the bridge * @return true/false in case of success/failure */ - virtual bool getJointsList(std::vector& jointsList) = 0; + virtual bool getJointsList(std::vector& jointsList) { return false; }; /** * Get imu sensors * @param[out] IMUsList list of IMUs attached to the bridge * @return true/false in case of success/failure */ - virtual bool getIMUsList(std::vector& IMUsList) = 0; + virtual bool getIMUsList(std::vector& IMUsList) { return false; }; /** * Get linear accelerometers * @param[out] linearAccelerometersList list of linear accelerometers attached to the bridge * @return true/false in case of success/failure */ - virtual bool getLinearAccelerometersList(std::vector& linearAccelerometersList) = 0; + virtual bool getLinearAccelerometersList(std::vector& linearAccelerometersList) { return false; }; /** * Get gyroscopes * @param[out] gyroscopesList list of gyroscopes attached to the bridge * @return true/false in case of success/failure */ - virtual bool getGyroscopesList(std::vector& gyroscopesList) = 0; + virtual bool getGyroscopesList(std::vector& gyroscopesList) { return false; }; /** * Get orientation sensors * @param[out] orientationSensorsList list of orientation sensors attached to the bridge * @return true/false in case of success/failure */ - virtual bool getOrientationSensorsList(std::vector& orientationSensorsList) = 0; + virtual bool getOrientationSensorsList(std::vector& orientationSensorsList) { return false; }; /** * Get magnetometers sensors * @param[out] magnetometersList list of magnetometers attached to the bridge * @return true/false in case of success/failure */ - virtual bool getMagnetometersList(std::vector& magnetometersList) = 0; + virtual bool getMagnetometersList(std::vector& magnetometersList) { return false; }; /** * Get 6 axis FT sensors * @param[out] sixAxisForceTorqueSensorsList list of 6 axis force torque sensors attached to the bridge * @return true/false in case of success/failure */ - virtual bool getSixAxisForceTorqueSensorsList(std::vector& sixAxisForceTorqueSensorsList) = 0; + virtual bool getSixAxisForceTorqueSensorsList(std::vector& sixAxisForceTorqueSensorsList) { return false; }; /** * Get 6 axis FT sensors * @param[out] threeAxisForceTorqueSensorsList list of 3 axis force torque sensors attached to the bridge * @return true/false in case of success/failure */ - virtual bool getThreeAxisForceTorqueSensorsList(std::vector& threeAxisForceTorqueSensorsList) = 0; + virtual bool getThreeAxisForceTorqueSensorsList(std::vector& threeAxisForceTorqueSensorsList) { return false; }; /** * Get cartesian wrenches * @param[out] cartesianWrenchesList list of cartesian wrenches attached to the bridge * @return true/false in case of success/failure */ - virtual bool getCartesianWrenchesList(std::vector& cartesianWrenchesList) = 0; + virtual bool getCartesianWrenchesList(std::vector& cartesianWrenchesList) { return false; }; /** * Get rgb cameras * @param[out] rgbCamerasList list of rgb cameras attached to the bridge * @return true/false in case of success/failure */ - virtual bool getRGBCamerasList(std::vector& rgbCamerasList) = 0; + virtual bool getRGBCamerasList(std::vector& rgbCamerasList) { return false; }; /** - * Get depth cameras - * @param[out] depthCamerasList list of depth cameras attached to the bridge + * Get RGBD cameras + * @param[out] rgbdCamerasList list of rgbd cameras attached to the bridge * @return true/false in case of success/failure */ - virtual bool getDepthCamerasList(std::vector& depthCamerasList) = 0; + virtual bool getRGBDCamerasList(std::vector& rgbdCamerasList) { return false; }; /** * Get joint position in radians @@ -167,7 +179,7 @@ class ISensorBridge */ virtual bool getJointPosition(const std::string& jointName, double& jointPosition, - double* receiveTimeInSeconds = nullptr) = 0; + double* receiveTimeInSeconds = nullptr) { return false; }; /** * Get all joints' positions in radians @@ -181,7 +193,7 @@ class ISensorBridge * @return true/false in case of success/failure */ virtual bool getJointPositions(Eigen::Ref jointPositions, - double* receiveTimeInSeconds = nullptr) = 0; + double* receiveTimeInSeconds = nullptr) { return false; }; /** * Get joint velocity in rad/s @@ -192,7 +204,7 @@ class ISensorBridge */ virtual bool getJointVelocity(const std::string& jointName, double& jointVelocity, - double* receiveTimeInSeconds = nullptr) = 0; + double* receiveTimeInSeconds = nullptr) { return false; }; /** * Get all joints' velocities in rad/s @@ -206,7 +218,7 @@ class ISensorBridge * @return true/false in case of success/failure */ virtual bool getJointVelocities(Eigen::Ref jointVelocties, - double* receiveTimeInSeconds = nullptr) = 0; + double* receiveTimeInSeconds = nullptr) { return false; }; /** * Get IMU measurement @@ -223,7 +235,7 @@ class ISensorBridge */ virtual bool getIMUMeasurement(const std::string& imuName, Eigen::Ref imuMeasurement, - double* receiveTimeInSeconds = nullptr) = 0; + double* receiveTimeInSeconds = nullptr) { return false; }; /** * Get linear accelerometer measurement in m/s^2 @@ -234,7 +246,7 @@ class ISensorBridge */ virtual bool getLinearAccelerometerMeasurement(const std::string& accName, Eigen::Ref accMeasurement, - double* receiveTimeInSeconds = nullptr) = 0; + double* receiveTimeInSeconds = nullptr) { return false; }; /** * Get gyroscope measurement in rad/s @@ -245,7 +257,7 @@ class ISensorBridge */ virtual bool getGyroscopeMeasure(const std::string& gyroName, Eigen::Ref gyroMeasurement, - double* receiveTimeInSeconds = nullptr) = 0; + double* receiveTimeInSeconds = nullptr) { return false; }; /** * Get orientation sensor measurement in radians as roll pitch yaw Euler angles @@ -256,7 +268,7 @@ class ISensorBridge */ virtual bool getOrientationSensorMeasurement(const std::string& rpyName, Eigen::Ref rpyMeasurement, - double* receiveTimeInSeconds = nullptr) = 0; + double* receiveTimeInSeconds = nullptr) { return false; }; /** * Get magentometer measurement in tesla @@ -267,7 +279,7 @@ class ISensorBridge */ virtual bool getMagnetometerMeasurement(const std::string& magName, Eigen::Ref magMeasurement, - double* receiveTimeInSeconds = nullptr) = 0; + double* receiveTimeInSeconds = nullptr) { return false; }; /** * Get six axis force torque measurement @@ -278,7 +290,7 @@ class ISensorBridge */ virtual bool getSixAxisForceTorqueMeasurement(const std::string& ftName, Eigen::Ref ftMeasurement, - double* receiveTimeInSeconds = nullptr) = 0; + double* receiveTimeInSeconds = nullptr) { return false; }; /** * Get three axis force-torque measurement containing normal force (N) and tangential moments (Nm) @@ -289,7 +301,7 @@ class ISensorBridge */ virtual bool getThreeAxisForceTorqueMeasurement(const std::string& ftName, Eigen::Ref ftMeasurement, - double* receiveTimeInSeconds = nullptr) = 0; + double* receiveTimeInSeconds = nullptr) { return false; }; /** * Get 6D end effector wrenches in N and Nm for forces and torques respectively @@ -300,7 +312,7 @@ class ISensorBridge */ virtual bool getCartesianWrench(const std::string& cartesianWrenchName, Eigen::Ref cartesianWrenchMeasurement, - double* receiveTimeInSeconds = nullptr) = 0; + double* receiveTimeInSeconds = nullptr) { return false; }; /** * Get color image from the camera @@ -316,7 +328,7 @@ class ISensorBridge */ virtual bool getColorImage(const std::string& camName, Eigen::Ref colorImg, - double* receiveTimeInSeconds = nullptr) = 0; + double* receiveTimeInSeconds = nullptr) { return false; }; /** * Get depth image @@ -332,7 +344,7 @@ class ISensorBridge */ virtual bool getDepthImage(const std::string& camName, Eigen::Ref depthImg, - double* receiveTimeInSeconds = nullptr) = 0; + double* receiveTimeInSeconds = nullptr) { return false; }; /** * Destructor @@ -348,7 +360,7 @@ class ISensorBridge * @param[in] handler Parameters handler * @param[in] sensorBridgeOptions SensorBridgeOptions to hold the bridge options for streaming sensor measurements */ - virtual bool populateSensorBridgeOptionsFromConfig(std::weak_ptr handler, + virtual bool populateSensorBridgeOptionsFromConfig(std::weak_ptr handler, SensorBridgeOptions& sensorBridgeOptions) { return true; }; /** @@ -360,10 +372,22 @@ class ISensorBridge * @param[in] sensorBridgeOptions configured object of SensorBridgeOptions * @param[in] sensorLists SensorLists object holding list of connected sensor devices */ - virtual bool populateSensorListsFromConfig(std::weak_ptr handler, + virtual bool populateSensorListsFromConfig(std::weak_ptr handler, const SensorBridgeOptions& sensorBridgeOptions, SensorLists& sensorLists) { return true; }; + /** + * Helper method to maintain SensorBridgeMetaData struct by populating it from the configuration parameters + * @note the user may choose to use/not use this method depending on their requirements for the implementation + * if the user chooses to not use the method, the implementation must simply contain "return true;" + * + * @param[in] handler Parameters handler + * @param[in] sensorBridgeMetaData configured object of SensorBridgeMetadata + * @param[in] sensorLists SensorLists object holding list of connected sensor devices + */ + virtual bool populateSensorBridgeMetaDataFromConfig(std::weak_ptr handler, + SensorBridgeMetaData& sensorBridgeMetaData) { return true; }; + }; } // namespace RobotInterface diff --git a/src/RobotInterface/tests/DummyImplementation/DummySensorBridge.h b/src/RobotInterface/tests/DummyImplementation/DummySensorBridge.h index d79d405149..3f439bbfc2 100644 --- a/src/RobotInterface/tests/DummyImplementation/DummySensorBridge.h +++ b/src/RobotInterface/tests/DummyImplementation/DummySensorBridge.h @@ -20,7 +20,7 @@ namespace RobotInterface class DummySensorBridge : public ISensorBridge { public: - virtual bool initialize(std::weak_ptr handler) override + virtual bool initialize(std::weak_ptr handler) override { if (!populateSensorBridgeOptionsFromConfig(handler, m_options)) { @@ -97,9 +97,9 @@ class DummySensorBridge : public ISensorBridge rgbCamerasList = std::vector{""}; return true; }; - virtual bool getDepthCamerasList(std::vector& depthCamerasList) override + virtual bool getRGBDCamerasList(std::vector& rgbdCamerasList) override { - depthCamerasList = std::vector{""}; + rgbdCamerasList = std::vector{""}; return true; }; @@ -168,7 +168,7 @@ class DummySensorBridge : public ISensorBridge Eigen::Ref depthImg, double* receiveTimeInSeconds = nullptr) override { return true; }; protected: - virtual bool populateSensorBridgeOptionsFromConfig(std::weak_ptr handler, + virtual bool populateSensorBridgeOptionsFromConfig(std::weak_ptr handler, SensorBridgeOptions& sensorBridgeOptions) override { auto handle = handler.lock(); @@ -197,7 +197,7 @@ class DummySensorBridge : public ISensorBridge return true; }; - virtual bool populateSensorListsFromConfig(std::weak_ptr handler, + virtual bool populateSensorListsFromConfig(std::weak_ptr handler, const SensorBridgeOptions& sensorBridgeOptions, SensorLists& sensorLists) override {