From e3cee09473453f8370a50c858f82fd01c5787bb2 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Wed, 24 Apr 2024 23:25:37 +0200 Subject: [PATCH] Delete seldom used classes --- CMakeLists.txt | 1 - cmakemodules/script_duo3d.cmake | 34 - doc/source/doxygen-docs/changelog.md | 9 +- libs/hwdrivers/CMakeLists.txt | 9 - libs/hwdrivers/include/mrpt/hwdrivers.h | 7 - .../include/mrpt/hwdrivers/CBoardENoses.h | 142 --- .../include/mrpt/hwdrivers/CBoardSonars.h | 148 ---- .../include/mrpt/hwdrivers/CCameraSensor.h | 70 +- .../include/mrpt/hwdrivers/CDUO3DCamera.h | 253 ------ .../include/mrpt/hwdrivers/CPtuDPerception.h | 422 --------- .../mrpt/hwdrivers/CRoboticHeadInterface.h | 120 --- .../include/mrpt/hwdrivers/CTuMicos.h | 327 ------- libs/hwdrivers/src/CBoardENoses.cpp | 429 --------- libs/hwdrivers/src/CBoardSonars.cpp | 330 ------- libs/hwdrivers/src/CCameraSensor.cpp | 60 +- libs/hwdrivers/src/CDUO3DCamera.cpp | 606 ------------- libs/hwdrivers/src/CPtuDPerception.cpp | 820 ------------------ libs/hwdrivers/src/CRoboticHeadInterface.cpp | 123 --- libs/hwdrivers/src/CTuMicos.cpp | 819 ----------------- libs/hwdrivers/src/registerAllClasses.cpp | 2 - parse-files/config.h.in | 3 - python/python.conf | 4 - python/src/pymrpt.sources | 2 - .../rawlog-grabber/camera_duo3d.ini | 103 --- .../config_files/rawlog-grabber/ptuHokuyo.ini | 55 -- .../rawlog-grabber/ptuHokuyoCamera.ini | 94 -- .../config_files/rawlog-grabber/tuHokuyo.ini | 55 -- 27 files changed, 13 insertions(+), 5034 deletions(-) delete mode 100644 cmakemodules/script_duo3d.cmake delete mode 100644 libs/hwdrivers/include/mrpt/hwdrivers/CBoardENoses.h delete mode 100644 libs/hwdrivers/include/mrpt/hwdrivers/CBoardSonars.h delete mode 100644 libs/hwdrivers/include/mrpt/hwdrivers/CDUO3DCamera.h delete mode 100644 libs/hwdrivers/include/mrpt/hwdrivers/CPtuDPerception.h delete mode 100644 libs/hwdrivers/include/mrpt/hwdrivers/CRoboticHeadInterface.h delete mode 100644 libs/hwdrivers/include/mrpt/hwdrivers/CTuMicos.h delete mode 100644 libs/hwdrivers/src/CBoardENoses.cpp delete mode 100644 libs/hwdrivers/src/CBoardSonars.cpp delete mode 100644 libs/hwdrivers/src/CDUO3DCamera.cpp delete mode 100644 libs/hwdrivers/src/CPtuDPerception.cpp delete mode 100644 libs/hwdrivers/src/CRoboticHeadInterface.cpp delete mode 100644 libs/hwdrivers/src/CTuMicos.cpp delete mode 100644 share/mrpt/config_files/rawlog-grabber/camera_duo3d.ini delete mode 100644 share/mrpt/config_files/rawlog-grabber/ptuHokuyo.ini delete mode 100644 share/mrpt/config_files/rawlog-grabber/ptuHokuyoCamera.ini delete mode 100644 share/mrpt/config_files/rawlog-grabber/tuHokuyo.ini diff --git a/CMakeLists.txt b/CMakeLists.txt index 1dcb498bac..63504f92eb 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -270,7 +270,6 @@ endif() include(cmakemodules/script_assimp.cmake REQUIRED) # Check for system assimp lib (3D models) include(cmakemodules/script_bfd.cmake REQUIRED) # BFD library for debug symbols for stack backtraces include(cmakemodules/script_clang_tidy.cmake REQUIRED) # Clang tidy -include(cmakemodules/script_duo3d.cmake REQUIRED) # Support for DUO3D Camera include(cmakemodules/script_eigen.cmake REQUIRED) # Eigen3 include(cmakemodules/script_ffmpeg.cmake REQUIRED) # Check for ffmpeg C libraries: libavcodec, libavutil, libavformat, libswscale include(cmakemodules/script_flycapture2.cmake REQUIRED) # Check for PointGreyResearch (PGR) FlyCapture2 library diff --git a/cmakemodules/script_duo3d.cmake b/cmakemodules/script_duo3d.cmake deleted file mode 100644 index 8907cd3b49..0000000000 --- a/cmakemodules/script_duo3d.cmake +++ /dev/null @@ -1,34 +0,0 @@ -# Support for DUO3D stereo camera -# ========================================================================================== -set(CMAKE_MRPT_HAS_DUO3D 0) # Set default value (it cannot be empty!) - -set( MRPT_HAS_DUO3D OFF CACHE BOOL "Build with support for DUO3D libraries?") - -if( MRPT_HAS_DUO3D ) - if(UNIX) - message("Sorry! DUO3D camera is supported only for Windows yet. Set MRPT_HAS_DUO3D to OFF") - else(UNIX) - - set( DUO3D_INCLUDE_DIR "" CACHE PATH "Path to DUO3D include directory" ) - set( DUO3D_LIB_DIR "" CACHE PATH "Path to DUO3D library directory" ) - - # Set to 1, next check for missing things and set to 0 on any error & report message: - set(CMAKE_MRPT_HAS_DUO3D 1) - - # if(NOT EXISTS ${DUO3D_ROOT_DIR}) - # set(CMAKE_MRPT_HAS_DUO3D 0) - # message("The directory 'DUO3D_ROOT_DIR' does not exists. Turn off DUO3D support or provide the correct path.") - # endif(NOT EXISTS ${DUO3D_ROOT_DIR}) - - if(NOT EXISTS ${DUO3D_INCLUDE_DIR}/DUOLib.h) - set(CMAKE_MRPT_HAS_DUO3D 0) - message("The directory 'DUO3D_INCLUDE_DIR' does not contain DUOLib.h. Turn off DUO3D support or provide the correct path.") - endif(NOT EXISTS ${DUO3D_INCLUDE_DIR}/DUOLib.h) - - if(NOT EXISTS ${DUO3D_LIB_DIR}/DUOLib.lib) - set(CMAKE_MRPT_HAS_DUO3D 0) - message("The directory 'DUO3D_LIB_DIR' does not contain DUOLib.lib. Turn off DUO3D support or provide the correct path.") - endif(NOT EXISTS ${DUO3D_LIB_DIR}/DUOLib.lib) - endif(UNIX) - -endif(MRPT_HAS_DUO3D) diff --git a/doc/source/doxygen-docs/changelog.md b/doc/source/doxygen-docs/changelog.md index 63b9589d2b..45e0005eba 100644 --- a/doc/source/doxygen-docs/changelog.md +++ b/doc/source/doxygen-docs/changelog.md @@ -8,7 +8,14 @@ - mrpt::containers::yaml::asSequenceRange() - \ref mrpt_maps_grp - Removed unused method: mrpt::maps::COccupancyGridMap2D::saveAsEMFTwoMapsWithCorrespondences() - - Removed module mrpt-detectors, which was seldom used. If needed, refer to MRPT versions <2.13.0 + - Removed seldom used code (if needed, refer to MRPT versions <2.13.0): + - The whole module mrpt-detectors + - mrpt::hwdrivers::CBoardENoses + - mrpt::hwdrivers::CBoardSonars + - mrpt::hwdrivers::CDUO3DCamera + - mrpt::hwdrivers::CPtuDPerception + - mrpt::hwdrivers::CRoboticHeadInterface + - mrpt::hwdrivers::CTuMicos # Version 2.12.2: Released April 23rd, 2024 - Changes in libraries: diff --git a/libs/hwdrivers/CMakeLists.txt b/libs/hwdrivers/CMakeLists.txt index 6c56a67519..8e1a80edb0 100644 --- a/libs/hwdrivers/CMakeLists.txt +++ b/libs/hwdrivers/CMakeLists.txt @@ -31,11 +31,6 @@ if(CMAKE_MRPT_HAS_LIBDC1394_2) endif() -if(CMAKE_MRPT_HAS_DUO3D) - include_directories("${DUO3D_INCLUDE_DIR}") - link_directories("${DUO3D_LIB_DIR}") -endif() - if(CMAKE_MRPT_HAS_SWISSRANGE) include_directories("${SWR_LIBMESASR_DIR}") endif() @@ -112,10 +107,6 @@ if(CMAKE_MRPT_HAS_SWISSRANGE) target_link_libraries(hwdrivers PRIVATE ${MRPT_SWR_LIBS}) endif() -if(CMAKE_MRPT_HAS_DUO3D) - target_link_libraries(hwdrivers PRIVATE "${DUO3D_LIB_DIR}/DUOLib.lib" "${DUO3D_LIB_DIR}/Dense3D.lib") -endif() - # Seems to be required to link against prebuilt libavcodec, etc. if (MSVC) #set_target_properties() diff --git a/libs/hwdrivers/include/mrpt/hwdrivers.h b/libs/hwdrivers/include/mrpt/hwdrivers.h index 3ba1e528f9..2f759a2377 100644 --- a/libs/hwdrivers/include/mrpt/hwdrivers.h +++ b/libs/hwdrivers/include/mrpt/hwdrivers.h @@ -24,11 +24,8 @@ MRPT_WARNING( // All HWDRIVERS classes: #include -#include -#include #include #include -#include #include #include #include @@ -55,12 +52,9 @@ MRPT_WARNING( #include #include #include -#include #include #include -#include #include -#include #include #include #include @@ -68,6 +62,5 @@ MRPT_WARNING( #include #include #include -#include #include #include diff --git a/libs/hwdrivers/include/mrpt/hwdrivers/CBoardENoses.h b/libs/hwdrivers/include/mrpt/hwdrivers/CBoardENoses.h deleted file mode 100644 index bfcab80a72..0000000000 --- a/libs/hwdrivers/include/mrpt/hwdrivers/CBoardENoses.h +++ /dev/null @@ -1,142 +0,0 @@ -/* +------------------------------------------------------------------------+ - | Mobile Robot Programming Toolkit (MRPT) | - | https://www.mrpt.org/ | - | | - | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | - | See: https://www.mrpt.org/Authors - All rights reserved. | - | Released under BSD License. See: https://www.mrpt.org/License | - +------------------------------------------------------------------------+ */ - -#pragma once - -#include -#include -#include -#include -#include - -#include // unique_ptr - -namespace mrpt::hwdrivers -{ -/** A class for interfacing an e-Noses via a FTDI USB link. - * Implemented for the board v1.0 designed by 2007 @ ISA (University of - * Malaga). - * - * \code - * PARAMETERS IN THE ".INI"-LIKE CONFIGURATION STRINGS: - * ------------------------------------------------------- - * [supplied_section_name] - * USB_serialname=ENOSE001 // USB FTDI pipe: will open only if COM_port_* - * are not set or empty - * - * COM_port_WIN = COM1 // Serial port to connect to. - * COM_port_LIN = ttyS0 - * - * COM_baudRate = 115200 - * - * ; 3D position (in meters) of the master +slave eNoses - * enose_poses_x= ... - * enose_poses_y= ... - * enose_poses_z= ... - * - * ; 3D pose angles (in degrees) of the master +slave eNoses - * enose_poses_yaw= ... - * enose_poses_pitch= ... - * enose_poses_roll= ... - * - * \endcode - * - * \ingroup mrpt_hwdrivers_grp - */ -class CBoardENoses : public mrpt::hwdrivers::CGenericSensor -{ - DEFINE_GENERIC_SENSOR(CBoardENoses) - - protected: - /** A copy of the device serial number (to open the USB FTDI chip) - */ - std::string m_usbSerialNumber; - mrpt::Clock::duration initial_timestamp; - bool first_reading; - - /** If not an empty string (default), will open that serial port, otherwise - * will try to open USB FTDI device "m_usbSerialNumber" */ - std::string m_COM_port; - /** Default=115200 */ - unsigned int m_COM_baud{115200}; - - // Only one of these two streams will be !=nullptr and open for each - // specific eNose board! - /** FTDI comms pipe (when not in serial port mode) */ - std::unique_ptr m_stream_FTDI; - /** Serial port comms */ - std::unique_ptr m_stream_SERIAL; - - /** The 3D pose of the master + N slave eNoses on the robot (meters & - * radians) */ - std::vector enose_poses_x, enose_poses_y, enose_poses_z, - enose_poses_yaw, enose_poses_pitch, enose_poses_roll; - - /** Tries to connect to the USB device (if disconnected). - * \return nullptr on error, otherwise a stream to be used for comms. - */ - mrpt::io::CStream* checkConnectionAndConnect(); - - /** See the class documentation at the top for expected parameters */ - void loadConfig_sensorSpecific( - const mrpt::config::CConfigFileBase& configSource, - const std::string& section) override; - - public: - /** Constructor - * \param serialNumberUSBdevice The serial number (text) of the device to - * open. - * The constructor will try to open the device. You can check if it failed - * calling "isOpen()". - */ - CBoardENoses(); - - /** Set the active chamber (afected by poluted air) on the device - * \return true on success, false on communications errors or device not - * found. - */ - bool setActiveChamber(unsigned char chamber); - - /** Query the firmware version on the device (can be used to test - * communications). - * \return true on success, false on communications errors or device not - * found. - */ - bool queryFirmwareVersion(std::string& out_firmwareVersion); - - /** Request the master eNose the latest readings from all the eNoses. - * The output observation contains a valid timestamp and 3D positions if - * "loadConfig" has been called previously. - * \return true if OK, false if there were any error. - */ - bool getObservation(mrpt::obs::CObservationGasSensors& outObservation); - - // See docs in parent class - void doProcess() override; - - /** Tries to open the camera, after setting all the parameters with a call - * to loadConfig. - * \exception This method must throw an exception with a descriptive - * message if some critical error is found. - */ - void initialize() override; - - /** If not an empty string, will open that serial port, otherwise will try - * to open USB FTDI device "m_usbSerialNumber" - * The default is an empty string. Example strings: "COM1", "ttyUSB0", ... - */ - inline void setSerialPort(const std::string& port) { m_COM_port = port; } - inline std::string getSerialPort() const { return m_COM_port; } - /** Set the serial port baud rate (default: 115200) */ - inline void setSerialPortBaud(unsigned int baud) { m_COM_baud = baud; } - inline unsigned int getSerialPortBaud() const { return m_COM_baud; } -}; // end of class -} // namespace mrpt::hwdrivers diff --git a/libs/hwdrivers/include/mrpt/hwdrivers/CBoardSonars.h b/libs/hwdrivers/include/mrpt/hwdrivers/CBoardSonars.h deleted file mode 100644 index 8bf1355101..0000000000 --- a/libs/hwdrivers/include/mrpt/hwdrivers/CBoardSonars.h +++ /dev/null @@ -1,148 +0,0 @@ -/* +------------------------------------------------------------------------+ - | Mobile Robot Programming Toolkit (MRPT) | - | https://www.mrpt.org/ | - | | - | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | - | See: https://www.mrpt.org/Authors - All rights reserved. | - | Released under BSD License. See: https://www.mrpt.org/License | - +------------------------------------------------------------------------+ */ -#pragma once - -#include -#include -#include -#include - -namespace mrpt::hwdrivers -{ -/** This "software driver" implements the communication protocol for - * interfacing a Ultrasonic range finder SRF10 through a custom USB board. - * - * In this class the "bind" is ignored since it is designed for USB - * connections only, thus it internally generate the required object for - * simplicity of use. - * The serial number of the USB device is used to open it on the first call - * to "doProcess", thus you must call "loadConfig" before this, or manually - * call "setDeviceSerialNumber". The default serial number is "SONAR001" - * - * Warning: Avoid defining an object of this class in a global scope if you - * want to catch all potential - * exceptions during the constructors (like USB interface DLL not found, - * etc...) - * - * \code - * PARAMETERS IN THE ".INI"-LIKE CONFIGURATION STRINGS: - * ------------------------------------------------------- - * [supplied_section_name] - * USB_serialNumber=SONAR001 - * gain=6 ; Value between 0 and 16, for analog gains between 40 - * and 700. - * maxRange=4.0 ; In meters, used for device internal timer. - * minTimeBetweenPings=0.3 ; In seconds - * - * ; The order in which sonars will be fired, indexed by their I2C addresses - * [0,15] - * ; Up to 16 devices, but you can put any number of devices (from 1 to 16). - * firingOrder=0 1 2 3 - * - * - * \endcode - * - * \ingroup mrpt_hwdrivers_grp - */ -class CBoardSonars : public mrpt::comms::CInterfaceFTDI, public CGenericSensor -{ - DEFINE_GENERIC_SENSOR(CBoardSonars) - - public: - /** Constructor - */ - CBoardSonars(); - - /** Destructor - */ - ~CBoardSonars() override = default; - /** Query the firmware version on the device (can be used to test - * communications). - * \return true on success, false on communications errors or device not - * found. - */ - bool queryFirmwareVersion(std::string& out_firmwareVersion); - - /** Request the latest range measurements. - * \return true on success, false on communications errors or device not - * found. - */ - bool getObservation(mrpt::obs::CObservationRange& obs); - - /** Requests a command of "change address" for a given SRF10 device. - * currentAddress and newAddress are the I2C addresses in the range 0 to - * 15 (mapped to 0xE0 to 0xFE internally). - * \return true on success, false on communications errors or device not - * found. - */ - bool programI2CAddress(uint8_t currentAddress, uint8_t newAddress); - - // See docs in parent class - void doProcess() override; - - protected: - /** A copy of the device serial number (to open the USB FTDI chip) - */ - std::string m_usbSerialNumber; - - /** A value between 0 and 16, for gains between 40 and 700 (not linear). - */ - uint8_t m_gain; - - /** The maximum range in meters, used for the internal device timer (value - * between 4cm and 11m). - */ - float m_maxRange; - - /** The order in which sonars will be fired, indexed by their I2C addresses - * [0,15]. - * Up to 16 devices, but you can put any number of devices (from 1 to 16). - */ - std::vector m_firingOrder; - - /** The individual gains of the sonars, indexed by their I2C addresses - * [0,15]. - * Up to 16 devices, but you can put any number of devices (from 1 to 16). - */ - std::map m_sonarGains; - - /** The poses of the sonars: x[m] y[m] z[m] yaw[deg] pitch[deg] roll[deg] - * Up to 16 devices, but you can put any number of devices (from 1 to 16). - */ - std::map m_sonarPoses; - - /** The minimum time between sonar pings (in seconds). - */ - float m_minTimeBetweenPings; - - /** Tries to connect to the USB device (if disconnected). - * \return True on connection OK, false on error. - */ - bool checkConnectionAndConnect(); - - /** Sends the configuration (max range, gain,...) to the USB board. Used - * internally after a successfull connection. - * \return true on success, false on communications errors or device not - * found. - */ - bool sendConfigCommands(); - - /** Loads specific configuration for the device from a given source of - * configuration parameters, for example, an ".ini" file, - * loading from the section "[iniSection]" (see config::CConfigFileBase - * and - * derived classes) - * See hwdrivers::CBoardSonars for the possible parameters - */ - void loadConfig_sensorSpecific( - const mrpt::config::CConfigFileBase& configSource, - const std::string& iniSection) override; - -}; // End of class -} // namespace mrpt::hwdrivers diff --git a/libs/hwdrivers/include/mrpt/hwdrivers/CCameraSensor.h b/libs/hwdrivers/include/mrpt/hwdrivers/CCameraSensor.h index 3e394595cb..2273c34681 100644 --- a/libs/hwdrivers/include/mrpt/hwdrivers/CCameraSensor.h +++ b/libs/hwdrivers/include/mrpt/hwdrivers/CCameraSensor.h @@ -11,7 +11,6 @@ #include #include -#include #include #include #include @@ -84,8 +83,7 @@ namespace mrpt::hwdrivers * [supplied_section_name] * # Select one of the grabber implementations ----------------------- * grabber_type = opencv | dc1394 | bumblebee_dc1394 | ffmpeg | rawlog - * | swissranger | svs | kinect | flycap | flycap_stereo | image_dir | duo3d | - * myntd + * | swissranger | svs | kinect | flycap | flycap_stereo | image_dir | myntd * * # Options for any grabber_type ------------------------------------ * preview_decimation = 0 // N<=0 (or not present): No preview; N>0, @@ -268,67 +266,6 @@ namespace mrpt::hwdrivers * # Options for grabber_type= myntd ------------------------------------ * myntd_xxx = * - * # Options for grabber_type= duo3d - * Create a section like this: - * [DUO3DOptions] - * rawlog-grabber-ignore = true // Instructs rawlog-grabber to ignore - * this section (it is not a separate device!) - * - * image_width = 640 // [int] x Resolution - * image_height = 480 // [int] y Resolution - * fps = 30 // [int] Frames per second - * (<= 30) - * exposure = 50 // [int] Exposure value (1..100) - * led = 0 // [int] Led intensity - * (only for some device models) (1..100). - * gain = 50 // [int] Camera gain (1..100) - * capture_rectified = false // [bool] Rectify - * captured images - * capture_imu = true // [bool] Capture IMU data - * from DUO3D device (if available) - * calibration_from_file = true // [bool] Use YML - * calibration files provided by calibration application supplied with DUO3D - * device - * intrinsic_filename = "" // [string] Intrinsic - * parameters file. This filename should contain a substring _RWWWxHHH_ with WWW - * being the image width and HHH the image height, as provided by the - * calibration application. - * extrinsic_filename = "" // [string] Extrinsic - * parameters file. This filename should contain a substring _RWWWxHHH_ with WWW - * being the image width and HHH the image height, as provided by the - * calibration application. - * rectify_map_filename = "" // [string] Rectification map - * file. This filename should contain a substring _RWWWxHHH_ with WWW being the - * image width and HHH the image height, as provided by the calibration - * application. - * - * // if 'calibration_from_file' = false, three more sections containing the - * calibration must be provided: - * [DUO3D_LEFT] - * rawlog-grabber-ignore = true // Instructs rawlog-grabber to ignore - * this section (it is not a separate device!) - * resolution = [640 480] - * cx = 320 - * cy = 240 - * fx = 700 - * fy = 700 - * dist = [0 0 0 0 0] - * - * [DUO3D_RIGHT] - * rawlog-grabber-ignore = true // Instructs rawlog-grabber to ignore - * this section (it is not a separate device!) - * resolution = [640 480] - * cx = 320 - * cy = 240 - * fx = 700 - * fy = 700 - * dist = [0 0 0 0 0] - * - * [DUO3D_LEFT2RIGHT_POSE] - * rawlog-grabber-ignore = true // Instructs rawlog-grabber to ignore - * this section (it is not a separate device!) - * pose_quaternion = [0.12 0 0 1 0 0 0] - * * \endcode * * \note The execution rate, in rawlog-grabber or the user code calling @@ -505,9 +442,6 @@ class CCameraSensor : public mrpt::system::COutputLogger, public CGenericSensor bool m_img_dir_is_stereo{true}; int m_img_dir_counter{0}; - // Options for grabber type= duo3d - TCaptureOptions_DUO3D m_duo3d_options; - // Other options: /** Whether to launch independent thread */ bool m_external_images_own_thread{false}; @@ -543,8 +477,6 @@ class CCameraSensor : public mrpt::system::COutputLogger, public CGenericSensor std::unique_ptr m_cap_openni2; /** Read images from directory */ std::unique_ptr m_cap_image_dir; - /** The DUO3D capture object */ - std::unique_ptr m_cap_duo3d; /** The MYNT EYE capture object */ std::unique_ptr m_myntd; // ========================= diff --git a/libs/hwdrivers/include/mrpt/hwdrivers/CDUO3DCamera.h b/libs/hwdrivers/include/mrpt/hwdrivers/CDUO3DCamera.h deleted file mode 100644 index 0e4d1f9523..0000000000 --- a/libs/hwdrivers/include/mrpt/hwdrivers/CDUO3DCamera.h +++ /dev/null @@ -1,253 +0,0 @@ -/* +------------------------------------------------------------------------+ - | Mobile Robot Programming Toolkit (MRPT) | - | https://www.mrpt.org/ | - | | - | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | - | See: https://www.mrpt.org/Authors - All rights reserved. | - | Released under BSD License. See: https://www.mrpt.org/License | - +------------------------------------------------------------------------+ */ - -#pragma once - -#include -#include -#include - -namespace mrpt::hwdrivers -{ -/** Options used when creating a camera capture object of type - * CImageGrabber_FlyCapture2 \ingroup mrpt_hwdrivers_grp */ -struct TCaptureOptions_DUO3D -{ - enum TYMLReadResult - { - yrr_NAME_NON_CONSISTENT, - yrr_EMPTY, - yrr_OK - }; - - TCaptureOptions_DUO3D(); - ~TCaptureOptions_DUO3D(); - - /** @name Image settings - * @{ */ - /** (Default = 640) Width of the captured image. */ - int m_img_width{640}; - /** (Default = 480) Height of the captured image. */ - int m_img_height{480}; - /** (Default = 30) Frames per second <= 30. */ - float m_fps{30}; - /** (Default = 50) Exposure value. */ - float m_exposure{50}; - /** (Default = 25) Led intensity (some device models). */ - float m_led{25}; - /** (Default = 10) Camera gain. */ - float m_gain{0}; - /** @} */ - - /** @name Behaviour selection - * @{ */ - /** (Default = false) Capture IMU data. */ - bool m_capture_imu{false}; - /** (Default = true) Rectify images. Rectification map must be provided \sa - * m_rectify_map_filename. */ - bool m_capture_rectified{false}; - /** (Default = true) Get calibration information from files provided by - * DUO3D Calibration App. */ - bool m_calibration_from_file{true}; - /** @} */ - - /** @name Files specification - * @{ */ - /** Rectification map file provided by DUO3D Calibration App (YML format). - */ - std::string m_rectify_map_filename; - /** Intrinsic parameters file provided by DUO3D Calibration App (YML - * format). */ - std::string m_intrinsic_filename; - /** Extrinsic parameters file provided by DUO3D Calibration App (YML - * format). */ - std::string m_extrinsic_filename; - /** @} */ - - /** @name Others - * @{ */ - mrpt::img::TStereoCamera m_stereo_camera; - /** @} */ - - // clang-format off - /** Loads all the options from a config file. - * Expected format: - * - * \code - * [sectionName] - * image_width = 640 // [int] x Resolution - * image_height = 480 // [int] y Resolution - * fps = 30 // [int] Frames per second (<= *30) - * exposure = 50 // [int] Exposure value (1..100) - * led = 0 // [int] Led intensity (only for some device models) (1..100). - * gain = 50 // [int] Camera gain (1..100) - * capture_rectified = false // [bool] Rectify captured images - * capture_imu = true // [bool] Capture IMU data from DUO3D device (if available) - * calibration_from_file = true // [bool] Use YML calibration files provided by calibration application supplied with DUO3D device - * intrinsic_filename = "" // [string] Intrinsic parameters file. This filename should contain a substring _RWWWxHHH_ with WWW being the image width and HHH the image height, as provided by the calibration application. - * extrinsic_filename = "" // [string] Extrinsic parameters file. This filename should contain a substring _RWWWxHHH_ with WWW being the image width and HHH the image height, as provided by the calibration application. - * rectify_map_filename = "" // [string] Rectification map file. This filename should contain a substring _RWWWxHHH_ with WWW being the image width and HHH the image height, as provided by the calibration application. - * - * \endcode - * \note All parameter names may have an optional prefix, set with the - *"prefix" parameter. - * For example, if prefix="LEFT_", the expected variable name - *"camera_index" in the config section will be "LEFT_camera_index", and so - *on. - */ - void loadOptionsFrom( - const mrpt::config::CConfigFileBase& configSource, - const std::string& sectionName, - const std::string& prefix = std::string()); - // clang-format on - - TYMLReadResult m_camera_int_params_from_yml( - const std::string& _file_name = std::string()); - TYMLReadResult m_camera_ext_params_from_yml( - const std::string& _file_name = std::string()); - TYMLReadResult m_rectify_map_from_yml( - const std::string& _file_name = std::string()); - -}; // end-TCaptureOptions_DUO3D - -// clang-format off -/** This "software driver" implements the communication protocol for interfacing - *a DUO3D Stereo Camera - * - * See also the example configuration file for rawlog-grabber in - *"share/mrpt/config_files/rawlog-grabber". - * - * \code - * PARAMETERS IN THE ".INI"-LIKE CONFIGURATION STRINGS: - * ------------------------------------------------------- - * [supplied_section_name] - * image_width = 640 // [int] x Resolution - * image_height = 480 // [int] y Resolution - * fps = 30 // [int] Frames per second (<=30) - * exposure = 50 // [int] Exposure value (1..100) - * led = 0 // [int] Led intensity (only for some device models) (1..100). - * gain = 50 // [int] Camera gain (1..100) - * capture_rectified = false // [bool] Rectify captured images - * capture_imu = true // [bool] Capture IMU data from DUO3D device (if available) - * calibration_from_file = true // [bool] Use YML calibration files provided by calibration application supplied with DUO3D device - * intrinsic_filename = "" // [string] Intrinsic parameters file. This filename should contain a substring _RWWWxHHH_ with WWW being the image width and HHH the image height, as provided by the calibration application. - * extrinsic_filename = "" // [string] Extrinsic parameters file. This filename should contain a substring _RWWWxHHH_ with WWW being the image width and HHH the image height, as provided by the calibration application. - * rectify_map_filename = "" // [string] Rectification map file. This filename should contain a substring _RWWWxHHH_ with WWW being the image width and HHH the image height, as provided by the calibration application. - * - * pose_x=0.21 // camera 3D position in the robot (meters) - * pose_y=0 - * pose_z=0.34 - * pose_yaw=0 // Angles in degrees - * pose_pitch=0 - * pose_roll=0 - * \endcode - * - * \ingroup mrpt_hwdrivers_grp - */ // clang-format on -class CDUO3DCamera -{ - protected: - // members - // [USER-DEFINED] - TCaptureOptions_DUO3D m_options; - - // [INTERNAL] - mrpt::vision::CStereoRectifyMap m_rectify_map; - - /** Opaque pointer to DUO's DUOInstance */ - void* m_duo{nullptr}; - /** Pointer, to be reinterpreted as "PDUOFrame" */ - void* m_pframe_data; - /** DUO's HANDLE */ - void* m_evFrame; - - public: - /** Default Constructor (does not open the camera) */ - CDUO3DCamera(); - - /** Constructor: tries to open the camera with the given options. Raises an - * exception on error. \sa open() */ - CDUO3DCamera(const TCaptureOptions_DUO3D& options); - - CDUO3DCamera(const CDUO3DCamera&) = delete; - CDUO3DCamera& operator=(const CDUO3DCamera&) = delete; - - /** Destructor */ - virtual ~CDUO3DCamera(); - - /** Returns the current settings of the camera */ - const TCaptureOptions_DUO3D& getCameraOptions() const { return m_options; } - /** Tries to open the camera with the given options, and starts capturing. - * Raises an exception on error. - * \param[in] startCapture If set to false, the camera is only opened and - * configured, but a posterior call to startCapture() is required to start - * grabbing data. - * \sa close(), startCapture() - */ - void open( - const TCaptureOptions_DUO3D& options, const bool startCapture = true); - - /** Start the actual data capture of the camera. Must be called after - * open(), only when "startCapture" was set to false. - */ - void startCapture(); - - /** Stop capture. */ - void stopCapture(); - - /** Stop capture and closes the opened camera, if any. Called automatically - * on object destruction. */ - void close(); - - /** Specific laser scanner "software drivers" must process here new data - * from the I/O stream, and, if a whole scan has arrived, return it. - * This method will be typically called in a different thread than other - * methods, and will be called in a timely fashion. - */ - void getObservations( - mrpt::obs::CObservationStereoImages& outObservation_img, - mrpt::obs::CObservationIMU& outObservation_imu, bool& there_is_img, - bool& there_is_imu); - - /** Indicates if the camera is grabbing IMU data */ - inline bool captureIMUIsSet() { return m_options.m_capture_imu; } - /** Returned pointer to be reinterpreted as DUO3D's "HANDLE" */ - inline void* getEvent() { return this->m_evFrame; } - /** frame is a reinterpreted PDUOFrame */ - inline void setDataFrame(void* frame) { this->m_pframe_data = frame; } - - protected: - /** Queries the DUO3D Camera firmware version */ - bool queryVersion(std::string version, bool printOutVersion = false); - - /** Gets a stereo frame from the DUO3D Camera (void* to be reinterpreted as - * PDUOFrame) */ - void* m_get_duo_frame(); - - /** Opens DUO3D camera */ - bool m_open_duo_camera(int width, int height, float fps); - - /** Closes DUO3D camera */ - void m_close_duo_camera(); - - /** Sets DUO3D camera Exposure setting */ - void m_set_exposure(float value); - - /** Sets DUO3D camera Gain setting */ - void m_set_gain(float value); - - /** Sets DUO3D camera LED setting */ - void m_set_led(float value); - - public: -}; // End of class - -static_assert(!std::is_copy_constructible_v, "Copy Check"); -static_assert(!std::is_copy_assignable_v, "Assign Check"); -} // namespace mrpt::hwdrivers diff --git a/libs/hwdrivers/include/mrpt/hwdrivers/CPtuDPerception.h b/libs/hwdrivers/include/mrpt/hwdrivers/CPtuDPerception.h deleted file mode 100644 index e2e6810d2a..0000000000 --- a/libs/hwdrivers/include/mrpt/hwdrivers/CPtuDPerception.h +++ /dev/null @@ -1,422 +0,0 @@ -/* +------------------------------------------------------------------------+ - | Mobile Robot Programming Toolkit (MRPT) | - | https://www.mrpt.org/ | - | | - | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | - | See: https://www.mrpt.org/Authors - All rights reserved. | - | Released under BSD License. See: https://www.mrpt.org/License | - +------------------------------------------------------------------------+ */ - -#pragma once - -#include -#include - -namespace mrpt::hwdrivers -{ -/** This class implements initialization and communication methods to - * control a Pan and Tilt Unit model PTU-46-17.5, working in radians . - * \ingroup mrpt_hwdrivers_grp - */ -class CPtuDPerception : public CPtuBase -{ - public: - /** Default constructor */ - - CPtuDPerception() = default; - - /** Destructor */ - - ~CPtuDPerception() override { close(); } - /*************************** Commands ***************************/ - - public: - /** Search limit forward */ - - bool rangeMeasure() override; - - /** Specification of positions in absolute terms */ - - bool moveToAbsPos(char axis, double nRad) override; - - /** Query position in absolute terms */ - - bool absPosQ(char axis, double& nRad) override; - - /** Specify desired axis position as an offset from the current position. \n - * This method recives the number of radians to move. - * \code - * Example of use: - * TT-500 * - * A * - * TO * Current Tilt position is -500 - * TO500 * - * A * - * TT * Current Pan position is 1000 - * \endcode - */ - - bool moveToOffPos(char axis, double nRad) override; - - /** Query position in relative terms */ - - bool offPosQ(char axis, double& nRad) override; - - /** Query max movement limit of a axis in absolute terms */ - - bool maxPosQ(char axis, double& nRad) override; - - /** Query min movement limit of a axis in absolute terms */ - - bool minPosQ(char axis, double& nRad) override; - - /** Query if exist movement limits */ - - bool enableLimitsQ(bool& enable) override; // Query if exist some limit - - /** Enable/Disable movement limits */ - - bool enableLimits(bool set) override; - - /** With I mode (default) instructs pan-tilt unit to immediately - * execute positional commands. \n - * In S mode instructs pan-tilt unit to execute positional commands - * only when an Await Position Command Completion command is executed - * or when put into Immediate Execution Mode. \n - * \code - * Example of use of S mode: - * DR * - * S * - * PP1500 * - * TP-900 * - * PP * Current Pan position is 0 - * TP * Current Tilt position is 0 - * A * - * PP * Current Pan position is 1500 - * TP * Current Tilt position is -900 - * \endcode - */ - - bool inmediateExecution(bool set) override; - - /** Wait the finish of the last position command to - * continue accept commands - */ - - bool aWait() override; - - /** Inmediately stop all */ - - bool haltAll() override; - - /** Inmediately stop */ - - bool halt(char axis) override; - - /** Specification of turn speed */ - - bool speed(char axis, double radSec) override; - - /** Query turn speed */ - - bool speedQ(char axis, double& radSec) override; - - /** Specification (de/a)celeration in turn */ - - bool aceleration(char axis, double radSec2) override; - - /** Query (de/a)celeration in turn */ - - bool acelerationQ(char axis, double& radSec2) override; - - /** Specification of velocity to which start and finish - * the (de/a)celeration - */ - - bool baseSpeed(char axis, double radSec) override; - - /** Query velocity to which start and finish - * the (de/a)celeration - */ - - bool baseSpeedQ(char axis, double& radSec) override; - - /** Specification of velocity upper limit */ - - bool upperSpeed(char axis, double radSec) override; - - /** Query velocity upper limit */ - - bool upperSpeedQ(char axis, double& radSec) override; - - /** Specification of velocity lower limit */ - - bool lowerSpeed(char axis, double radSec) override; - - /** Query velocity lower limit */ - - bool lowerSpeedQ(char axis, double& radSec) override; - - /** Reset PTU to initial state */ - - bool reset() override; - - /** Save or restart default values */ - - bool save() override; - - /** Restore default values */ - - bool restoreDefaults() override; - - /** Restore factory default values */ - - bool restoreFactoryDefaults() override; - - /** Version and CopyRights */ - - bool version(char* nVersion) override; - - /** Number of version */ - - void nversion(double& nVersion) override; - - /** Query power mode */ - - bool powerModeQ(bool transit, char& mode) override; - - /** Specification of power mode */ - - bool powerMode(bool transit, char mode) override; - - /** Check if ptu is moving */ - - double status([[maybe_unused]] double& rad) override { return 1; } - - /** Set limits of movement */ - - bool setLimits(char axis, double& l, double& u) override; - - /* Change motion direction */ - - bool changeMotionDir() override; - - /**************************** State Queries ********************/ - - /** Check errors, returns 0 if there are not errors or error code in - *otherwise - * Error codes: - * \code - * 1: Com error - * 2: Time out error - * 3: Init error - * 4: Pan tilt hit error - * 5: Pan hit error - * 6: Tilt hit error - * 7: Max limit error - * 8: Min limit error - * 9: Out of range - * 10: Illegal command error - * 11: Unexpected error - * \endcode - **/ - - int checkErrors() override; - - inline bool noError() { return nError == 1; } - inline bool comError() { return (nError % CPtuDPerception::ComError) == 0; } - inline bool timeoutError() - { - return (nError % CPtuDPerception::TimeoutError) == 0; - } - inline bool initError() - { - return (nError % CPtuDPerception::InitError) == 0; - } - inline bool panTiltHitError() - { - return (nError % CPtuDPerception::PanTiltHitError) == 0; - } - inline bool panHitError() - { - return (nError % CPtuDPerception::PanHitError) == 0; - } - inline bool tiltHitError() - { - return (nError % CPtuDPerception::TiltHitError) == 0; - } - inline bool maxLimitError() - { - return (nError % CPtuDPerception::MaxLimitError) == 0; - } - inline bool minLimitError() - { - return (nError % CPtuDPerception::MinLimitError) == 0; - } - inline bool outOfRange() - { - return (nError % CPtuDPerception::OutOfRange) == 0; - } - inline bool illegalCommandError() - { - return (nError % CPtuDPerception::IllegalCommandError) == 0; - } - inline bool unExpectedError() - { - return (nError % CPtuDPerception::UnExpectedError) == 0; - } - - /** Clear errors **/ - - void clearErrors() override { nError = NoError; } - /*************************** Other member methods *****************/ - - public: - /** PTU and serial port initialization */ - - bool init(const std::string& port) override; - - /** Close Connection with serial port */ - - void close() override; - - /** To obtains the mistake for use discrete values when the movement - * is expressed in radians. Parameters are the absolute position in - * radians and the axis desired - */ - - double radError(char axis, double nRadMoved) override; - - /** To obtain the discrete value for a number of radians */ - - long radToPos(char axis, double nRad) override; - - /** To obtain the number of radians for a discrete value */ - - double posToRad(char axis, long nPos) override; - - /** Performs a scan in the axis indicated and whit the precision desired. \n - * \param {Pan or Till} \n - * \param {Wait time betwen commands} \n - * \param {initial position} - * \param {final position} - * \param {radians precision for the scan} - */ - - bool scan(char axis, int wait, float initial, float final, double radPre) - override; - - /** Query verbose mode */ - - bool verboseQ(bool& modo) override; - - /** Set verbose. \n - * \conde - * Example of response with FV (verbose) active: - * FV * - * PP * Current pan position is 0 - * Example of response with FT (terse) active: - * FT * - * PP * 0 - * \endcode - */ - - bool verbose(bool set) override; - - /** Query echo mode */ - - bool echoModeQ(bool& mode) override; - - /** Enable/Disable echo response with command. \n - * \code - * Example of use (EE supposed): - * PP * 22 - * ED * - * * 22 - * \endcode - */ - - bool echoMode(bool mode) override; - - /** Query the pan and tilt resolution per position moved - * and initialize local atributes - */ - - bool resolution() override; - - /*************************** Methods for internal use ****************/ - - private: - /** To transmition commands to the PTU */ - - bool transmit(const char* command) override; - - /** To receive the responseof the PTU */ - - bool receive(const char* command, char* response) override; - - /** Used to obtains a number of radians */ - - bool radQuerry(char axis, char command, double& nRad) override; - - /** Method used for asign a number of radians with a command */ - - bool radAsign(char axis, char command, double nRad) override; - - /** Convert string to double */ - - virtual double convertToDouble(char* sDouble); - - /** Convert string to long */ - - virtual long convertToLong(char* sLong); - - /**************************** Atributes ********************/ - - public: - enum - { - NoError = 1, - ComError = 2, - TimeoutError = 3, - InitError = 5, - PanHitError = 7, - TiltHitError = 11, - PanTiltHitError = 13, - MaxLimitError = 17, - MinLimitError = 19, - OutOfRange = 23, - IllegalCommandError = 29, - UnExpectedError = 31 - }; - - /** TimeoutError: Only occurs if the communication is cut with PTU - * so it is advisable to check the connection and initialize - * again the communication. - */ - - int nError; - - enum - { - Pan = 'P', - Tilt = 'T' - }; - enum - { - Regular = 'R', - High = 'H', - Low = 'L', - Off = 'O' - }; - enum - { - Com1 = 1, - Com2 = 2, - Com3 = 3, - Com4 = 4 - }; - -}; // End of class - -} // namespace mrpt::hwdrivers diff --git a/libs/hwdrivers/include/mrpt/hwdrivers/CRoboticHeadInterface.h b/libs/hwdrivers/include/mrpt/hwdrivers/CRoboticHeadInterface.h deleted file mode 100644 index 6e30c6bb49..0000000000 --- a/libs/hwdrivers/include/mrpt/hwdrivers/CRoboticHeadInterface.h +++ /dev/null @@ -1,120 +0,0 @@ -/* +------------------------------------------------------------------------+ - | Mobile Robot Programming Toolkit (MRPT) | - | https://www.mrpt.org/ | - | | - | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | - | See: https://www.mrpt.org/Authors - All rights reserved. | - | Released under BSD License. See: https://www.mrpt.org/License | - +------------------------------------------------------------------------+ */ - -#pragma once - -#include -#include -#include -#include -#include -#include - -namespace mrpt::hwdrivers -{ -/** This "software driver" implements the communication protocol for - * interfacing a Robotic Head Board through a custom - * USB RS-422 interface board. - * In this class the "bind" is ignored since it is designed for USB - * connections only, thus it internally generate the required object for - * simplicity of use. - * The default serial number is "OREJA001" - * - * Warning: Avoid defining an object of this class in a global scope if you - * want to catch all potential - * exceptions during the constructors (like USB interface DLL not found, - * etc...) - * - * \code - * PARAMETERS IN THE ".INI"-LIKE CONFIGURATION STRINGS: - * ------------------------------------------------------- - * [supplied_section_name] - * HEAD_serialNumber=OREJA001 - * HEAD_gain=127,127,127 - * HEAD_yaw=0 // initial yaw value - * HEAD_pitch=0 // initial tilt - * \endcode - * \ingroup mrpt_hwdrivers_grp - */ -class CRoboticHeadInterface : public mrpt::system::COutputLogger -{ - private: - mrpt::comms::CInterfaceFTDI m_usbConnection; - mrpt::serialization::CMessage msg; - std::string m_serialNumber; - std::vector gain; - int head_yaw, head_pitch; - - // bool checkControllerIsConnected(); - - protected: - /** Loads specific configuration for the device from a given source of - * configuration parameters, for example, an ".ini" file, - * loading from the section "[iniSection]" (see config::CConfigFileBase - * and - * derived classes) - * See hwdrivers::CSonarSRF10 for the possible parameters - */ - void loadConfig_sensorSpecific( - const mrpt::config::CConfigFileBase* configSource, - const std::string& iniSection); - - public: - /** Constructor - */ - CRoboticHeadInterface(); - - /** Destructor - */ - ~CRoboticHeadInterface() override = default; - /** Changes the serial number of the device to open - */ - void setDeviceSerialNumber(const std::string& deviceSerialNumber) - { - m_serialNumber = deviceSerialNumber; - } - - /** Read the gain for the amplifier of the ear "channel", where channel is - * 0, 1 or 2. - */ - void GetGain(int& _gain, int& channel); - - /** Set the gain for the amplifier each ear. The value range is [0x00(min) - * .. 0x7F(max)]. The value 0x80 set the resistor - * in high impedance state, DON'T USE IT!!! - */ - bool SetGain(int& new_gain, int& channel); - - /** This function return the angle where last sound where detected. This - * angle is related to the robot pose, NOT head pose. - * \code - * angle > 0deg --> Sound detected in the left - * angle = 0deg --> Sound detected in front of the head - * angle < 0deg --> Sound detected in the right - * \endcode - */ - void GetSoundLocation(int& ang); - - /** Debug only!!! This function return the last 500 acquired samples for - * each sound channel. - * - */ - void Get3SoundBuffer(mrpt::math::CMatrixDynamic& buf); - - /** Move the head in: - \code - * elevation = 'yaw' degrees - * orientation = 'pitch' degrees - * \endcode - */ - void SetHeadPose(int& yaw, int& pitch); - -}; // End of class - -} // namespace mrpt::hwdrivers diff --git a/libs/hwdrivers/include/mrpt/hwdrivers/CTuMicos.h b/libs/hwdrivers/include/mrpt/hwdrivers/CTuMicos.h deleted file mode 100644 index fce4fdfabc..0000000000 --- a/libs/hwdrivers/include/mrpt/hwdrivers/CTuMicos.h +++ /dev/null @@ -1,327 +0,0 @@ -/* +------------------------------------------------------------------------+ - | Mobile Robot Programming Toolkit (MRPT) | - | https://www.mrpt.org/ | - | | - | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | - | See: https://www.mrpt.org/Authors - All rights reserved. | - | Released under BSD License. See: https://www.mrpt.org/License | - +------------------------------------------------------------------------+ */ - -#pragma once - -#include -#include -#include - -namespace mrpt::hwdrivers -{ -/** This class implements initialization and communication methods to - * control a Tilt Unit model DT-80, working in radians . - * \ingroup mrpt_hwdrivers_grp - */ -class CTuMicos : public CPtuBase, public mrpt::system::COutputLogger -{ - public: - /** Default constructor */ - - CTuMicos() = default; - - /** Destructor */ - - ~CTuMicos() override { close(); } - /*************************** Commands ***************************/ - - public: - /** Search limit forward */ - - bool rangeMeasure() override; - - /** Specification of positions in absolute terms */ - - bool moveToAbsPos(char axis, double nRad) override; - - /** Query position in absolute terms */ - - bool absPosQ(char axis, double& nRad) override; - - /** Specify desired axis position as an offset from the current position. \n - * This method recives the number of radians to move. - * \code - * Example of use: - * TT-500 * - * A * - * TO * Current Tilt position is -500 - * TO500 * - * A * - * TT * Current Pan position is 1000 - * \endcode - */ - - bool moveToOffPos(char axis, double nRad) override; - - /** Query position in relative terms */ - - bool offPosQ(char axis, double& nRad) override; - - /** Query max movement limit of a axis in absolute terms */ - - bool maxPosQ(char axis, double& nRad) override; - - /** Query min movement limit of a axis in absolute terms */ - - bool minPosQ(char axis, double& nRad) override; - - /** Query if exist movement limits */ - - bool enableLimitsQ(bool& enable) override; // Query if exist some limit - - /** Enable/Disable movement limits */ - - bool enableLimits(bool set) override; - - /** With I mode (default) instructs pan-tilt unit to immediately - * execute positional commands. \n - * In S mode instructs pan-tilt unit to execute positional commands - * only when an Await Position Command Completion command is executed - * or when put into Immediate Execution Mode. \n - * \code - * Example of use of S mode: - * DR * - * S * - * PP1500 * - * TP-900 * - * PP * Current Pan position is 0 - * TP * Current Tilt position is 0 - * A * - * PP * Current Pan position is 1500 - * TP * Current Tilt position is -900 - * \endcode - */ - - bool inmediateExecution(bool set) override; - - /** Wait the finish of the last position command to - * continue accept commands - */ - - bool aWait() override; - - /** Inmediately stop all */ - - bool haltAll() override; - - /** Inmediately stop */ - - bool halt(char axis) override; - - /** Specification of turn speed */ - - bool speed(char axis, double radSec) override; - - /** Query turn speed */ - - bool speedQ(char axis, double& radSec) override; - - /** Specification (de/a)celeration in turn */ - - bool aceleration(char axis, double radSec2) override; - - /** Query (de/a)celeration in turn */ - - bool acelerationQ(char axis, double& radSec2) override; - - /** Specification of velocity to which start and finish - * the (de/a)celeration - */ - - bool baseSpeed(char axis, double radSec) override; - - /** Query velocity to which start and finish - * the (de/a)celeration - */ - - bool baseSpeedQ(char axis, double& radSec) override; - - /** Specification of velocity upper limit */ - - bool upperSpeed(char axis, double radSec) override; - - /** Query velocity upper limit */ - - bool upperSpeedQ(char axis, double& radSec) override; - - /** Specification of velocity lower limit */ - - bool lowerSpeed(char axis, double radSec) override; - - /** Query velocity lower limit */ - - bool lowerSpeedQ(char axis, double& radSec) override; - - /** Reset PTU to initial state */ - - bool reset() override; - - /** Save or restart default values */ - - bool save() override; - - /** Restore default values */ - - bool restoreDefaults() override; - - /** Restore factory default values */ - - bool restoreFactoryDefaults() override; - - /** Version and CopyRights */ - - bool version(char* nVersion) override; - - /** Number of version */ - - void nversion(double& nVersion) override; - - /** Query power mode */ - - bool powerModeQ(bool transit, char& mode) override; - - /** Specification of power mode */ - - bool powerMode(bool transit, char mode) override; - - /** Clear controller internal stack */ - - bool clear(); - - /** Set limits of movement */ - - bool setLimits(char axis, double& l, double& u) override; - - /* Change motion direction */ - - bool changeMotionDir() override; - - /**************************** State Queries ********************/ - - int checkErrors() override; - - /** Clear errors **/ - - void clearErrors() override {} - /*************************** Other member methods *****************/ - - public: - /** PTU and serial port initialization */ - - bool init(const std::string& port) override; - - /** Close Connection with serial port */ - - void close() override; - - /** To obtains the mistake for use discrete values when the movement - * is expressed in radians. Parameters are the absolute position in - * radians and the axis desired - */ - - double radError(char axis, double nRadMoved) override; - - /** To obtain the discrete value for a number of radians */ - - long radToPos(char axis, double nRad) override; - - /** To obtain the number of radians for a discrete value */ - - double posToRad(char axis, long nPos) override; - - /** Performs a scan in the axis indicated and whit the precision desired. \n - * \param {Pan or Till} \n - * \param {Wait time betwen commands} \n - * \param {initial position} - * \param {final position} - * \param {radians precision for the scan} - */ - - bool scan(char axis, int wait, float initial, float final, double radPre) - override; - - /** Query verbose mode */ - - bool verboseQ(bool& modo) override; - - /** Set verbose. \n - * \conde - * Example of response with FV (verbose) active: - * FV * - * PP * Current pan position is 0 - * Example of response with FT (terse) active: - * FT * - * PP * 0 - * \endcode - */ - - bool verbose(bool set) override; - - /** Query echo mode */ - - bool echoModeQ(bool& mode) override; - - /** Enable/Disable echo response with command. \n - * \code - * Example of use (EE supposed): - * PP * 22 - * ED * - * * 22 - * \endcode - */ - - bool echoMode(bool mode) override; - - /** Query the pan and tilt resolution per position moved - * and initialize local atributes - */ - - bool resolution() override; - - /** Check if ptu is moving */ - - double status(double& rad) override; - - /*************************** Methods for internal use ****************/ - - private: - /** To transmition commands to the PTU */ - - bool transmit(const char* command) override; - - /** To receive the responseof the PTU */ - - bool receive(const char* command, char* response) override; - - /** Used to obtains a number of radians */ - - bool radQuerry(char axis, char command, double& nRad) override; - - /** Method used for asign a number of radians with a command */ - - bool radAsign(char axis, char command, double nRad) override; - - /** Convert string to double */ - - static double convertToDouble(char* sDouble); - - /** Convert string to long */ - - static long convertToLong(char* sLong); - - /**************************** Atributes ********************/ - - public: - /* Index of the Tilt axis in use */ - - int axis_index; - -}; // End of class - -} // namespace mrpt::hwdrivers diff --git a/libs/hwdrivers/src/CBoardENoses.cpp b/libs/hwdrivers/src/CBoardENoses.cpp deleted file mode 100644 index 29854ef8d7..0000000000 --- a/libs/hwdrivers/src/CBoardENoses.cpp +++ /dev/null @@ -1,429 +0,0 @@ -/* +------------------------------------------------------------------------+ - | Mobile Robot Programming Toolkit (MRPT) | - | https://www.mrpt.org/ | - | | - | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | - | See: https://www.mrpt.org/Authors - All rights reserved. | - | Released under BSD License. See: https://www.mrpt.org/License | - +------------------------------------------------------------------------+ */ - -#include "hwdrivers-precomp.h" // Precompiled headers -// -#include -#include -#include -#include - -#include -#include -#include - -using namespace mrpt::math; -using namespace mrpt::obs; -using namespace mrpt::poses; -using namespace mrpt::hwdrivers; -using namespace mrpt::io; -using namespace std; - -IMPLEMENTS_GENERIC_SENSOR(CBoardENoses, mrpt::hwdrivers) - -/*------------------------------------------------------------- - CBoardENoses --------------------------------------------------------------*/ -CBoardENoses::CBoardENoses() : m_usbSerialNumber("ENOSE001"), m_COM_port() -{ - m_sensorLabel = "ENOSE"; - first_reading = true; -} - -/*------------------------------------------------------------- - loadConfig_sensorSpecific --------------------------------------------------------------*/ -void CBoardENoses::loadConfig_sensorSpecific( - const mrpt::config::CConfigFileBase& configSource, - const std::string& iniSection) -{ - MRPT_START - - m_usbSerialNumber = - configSource.read_string(iniSection, "USB_serialname", "", false); - -#ifdef _WIN32 - m_COM_port = - configSource.read_string(iniSection, "COM_port_WIN", m_COM_port); -#else - m_COM_port = - configSource.read_string(iniSection, "COM_port_LIN", m_COM_port); -#endif - m_COM_baud = - configSource.read_uint64_t(iniSection, "COM_baudRate", m_COM_baud); - - configSource.read_vector( - iniSection, "enose_poses_x", vector(0), enose_poses_x, true); - configSource.read_vector( - iniSection, "enose_poses_y", vector(0), enose_poses_y, true); - configSource.read_vector( - iniSection, "enose_poses_z", vector(0), enose_poses_z, true); - - configSource.read_vector( - iniSection, "enose_poses_yaw", vector(0), enose_poses_yaw, true); - configSource.read_vector( - iniSection, "enose_poses_pitch", vector(0), enose_poses_pitch, - true); - configSource.read_vector( - iniSection, "enose_poses_roll", vector(0), enose_poses_roll, - true); - - ASSERT_(enose_poses_x.size() == enose_poses_y.size()); - ASSERT_(enose_poses_x.size() == enose_poses_z.size()); - ASSERT_(enose_poses_x.size() == enose_poses_yaw.size()); - ASSERT_(enose_poses_x.size() == enose_poses_pitch.size()); - ASSERT_(enose_poses_x.size() == enose_poses_roll.size()); - - // Pass angles to radians: - enose_poses_yaw *= M_PIf / 180.0f; - enose_poses_pitch *= M_PIf / 180.0f; - enose_poses_roll *= M_PIf / 180.0f; - - MRPT_END -} - -/*------------------------------------------------------------- - queryFirmwareVersion --------------------------------------------------------------*/ -bool CBoardENoses::queryFirmwareVersion(string& out_firmwareVersion) -{ - try - { - mrpt::serialization::CMessage msg, msgRx; - - // Try to connect to the device: - CStream* comms = checkConnectionAndConnect(); - if (!comms) return false; - auto arch = mrpt::serialization::archiveFrom(*comms); - - msg.type = 0x10; - arch.sendMessage(msg); - - if (arch.receiveMessage(msgRx)) - { - msgRx.getContentAsString(out_firmwareVersion); - return true; - } - else - return false; - } - catch (...) - { - // Close everything and start again: - m_stream_SERIAL.reset(); - m_stream_FTDI.reset(); - return false; - } -} - -/*------------------------------------------------------------- - checkConnectionAndConnect --------------------------------------------------------------*/ -CStream* CBoardENoses::checkConnectionAndConnect() -{ - // Make sure one of the two possible pipes is open: - if (!m_stream_FTDI && !m_stream_SERIAL) - { - if (!m_COM_port.empty()) - m_stream_SERIAL = std::make_unique(); - else - m_stream_FTDI = std::make_unique(); - } - - if (m_stream_FTDI) - { // FTDI pipe ================== - if (m_stream_FTDI->isOpen()) return m_stream_FTDI.get(); - try - { - m_stream_FTDI->OpenBySerialNumber(m_usbSerialNumber); - std::this_thread::sleep_for(10ms); - m_stream_FTDI->Purge(); - std::this_thread::sleep_for(10ms); - m_stream_FTDI->SetLatencyTimer(1); - m_stream_FTDI->SetTimeouts(10, 100); - return m_stream_FTDI.get(); - } - catch (...) - { // Error opening device: - m_stream_FTDI->Close(); - return nullptr; - } - } - else - { // Serial pipe ================== - ASSERT_(m_stream_SERIAL); - if (m_stream_SERIAL->isOpen()) return m_stream_SERIAL.get(); - try - { - m_stream_SERIAL->open(m_COM_port); - m_stream_SERIAL->setConfig(m_COM_baud); - std::this_thread::sleep_for(10ms); - m_stream_SERIAL->purgeBuffers(); - std::this_thread::sleep_for(10ms); - // m_stream_SERIAL->setTimeouts(25,1,100, 1,20); - m_stream_SERIAL->setTimeouts(50, 1, 100, 1, 20); - return m_stream_SERIAL.get(); - } - catch (...) - { // Error opening device: - m_stream_SERIAL->close(); - return nullptr; - } - } -} - -/*------------------------------------------------------------- - getObservation --------------------------------------------------------------*/ -bool CBoardENoses::getObservation(mrpt::obs::CObservationGasSensors& obs) -{ - try - { - // Connected? - CStream* comms = checkConnectionAndConnect(); - - if (!comms) return false; - - mrpt::serialization::CMessage msg; - CObservationGasSensors::TObservationENose newRead; - - obs.m_readings.clear(); - - //// Send request: - // msg.type = 0x11; - // msg.content.clear(); - // comms->sendMessage( msg ); - - //----------------------------MCE-nose - // FRAME-------------------------------------------------- - // Wait for e-nose frame: <0x69><0x91><0x96> "Bytes" - // Where = [Numchamber, Activechamber, N sensors*M chambers*2, 2 - // timestamp] of uint16_t - // MCE-nose provides a 136B body lenght which makes 140B total frame - // lenght - - auto arch = mrpt::serialization::archiveFrom(*comms); - if (!arch.receiveMessage(msg)) { return false; } - - // m_state = ssWorking; - - // Copy to "uint16_t": - ASSERT_((msg.content.size() % 2) == 0); - - vector readings( - msg.content.size() / - 2); // divide by 2 to pass from byte to word. 136B/2 = 68 Words - - if (msg.content.size() > 0) - { - // Copy to a vector of 16bit integers: - memcpy( - &readings[0], &msg.content[0], - msg.content.size() * sizeof(msg.content[0])); - - // HEADER Frame [ Nº of chambers/enoses (16b) , Active Chamber - // (16b)] - auto NumberOfChambers = (size_t)readings[0]; - auto ActiveChamber = (size_t)readings[1]; - - // Sensors readings info - ASSERT_(((readings.size() - 4) % NumberOfChambers) == 0); - size_t wordsPereNose = (readings.size() - 4) / NumberOfChambers; - - // Process each chamber - for (size_t i = 0; i < NumberOfChambers; i++) - { - // ---------------------------------------------------------------------- - // Each "i" comprises a complete Enose reading: Gas sensors + - // temperature - // ---------------------------------------------------------------------- - - // Do we have the sensor position? - if (i < enose_poses_x.size()) - { - newRead.eNosePoseOnTheRobot = TPose3D( - enose_poses_x[i], enose_poses_y[i], enose_poses_z[i], - enose_poses_yaw[i], enose_poses_pitch[i], - enose_poses_roll[i]); - } - else - newRead.eNosePoseOnTheRobot = TPose3D(0, 0, 0, 0, 0, 0); - - // Process the sensor codes: - newRead.sensorTypes.clear(); - newRead.readingsVoltage.clear(); - newRead.hasTemperature = false; - newRead.isActive = false; - - // check if active chamber - if (i == (ActiveChamber)) newRead.isActive = true; - - // process each sensor on this chamber "i" - for (size_t idx = 0; idx < wordsPereNose / 2; idx++) - { - if (readings[i * wordsPereNose + 2 * idx + 2] != - 0x0000) // not empty slot - { - // Is temperature? - if (readings[i * wordsPereNose + 2 * idx + 2] == 0xFFFF) - { - newRead.hasTemperature = true; - newRead.temperature = - ((int16_t)readings - [i * wordsPereNose + 2 * idx + 3]) / - 32.0f; - } - else // Is a gas sensors - { - // It is not a null code: There is a valid measure: - newRead.sensorTypes.push_back( - readings[i * wordsPereNose + 2 * idx + 2]); - - // Pass from ADC value[12bits] to [0-2.5] volt - // range: - newRead.readingsVoltage.push_back( - (readings[i * wordsPereNose + 2 * idx + 3] * - 5.0f) / - 4096.0f); - } - } - } // end for each sensor on this eNose - - // Add to observations: - if (!newRead.sensorTypes.empty()) - obs.m_readings.push_back(newRead); - - } // end for each i'th eNose - - obs.sensorLabel = m_sensorLabel; - - // Set Timestamp - auto* p = - (uint16_t*)&readings[readings.size() - 2]; // Get readings time - // from frame - // (always last 2 - // words) - obs.timestamp = mrpt::Clock::fromDouble(((double)*p) / 1000); - - if (first_reading) - { - initial_timestamp = mrpt::Clock::now() - obs.timestamp; - first_reading = false; - } - obs.timestamp = obs.timestamp + initial_timestamp; - - } // end if message has data - - // CONTROL - bool correct = true; - - if (obs.m_readings.size() != 4) correct = false; - else - { - for (auto& m_reading : obs.m_readings) - { - if ((m_reading.sensorTypes.size() != 7) || - (m_reading.readingsVoltage.size() != 7)) - correct = false; - else - { - } - } - } - - if (!correct) printf("Error en la observacion"); // For debug - - return !obs.m_readings.empty(); // Done OK! - } - catch (exception& e) - { - cerr << "[CBoardENoses::getObservation] Returning false due to " - "exception: " - << endl; - cerr << e.what() << endl; - return false; - } - catch (...) - { - return false; - } -} - -/*------------------------------------------------------------- - doProcess --------------------------------------------------------------*/ -/** This method should be called periodically (at least at 1Hz to capture ALL - * the real-time data) - * It is thread safe, i.e. you can call this from one thread, then to other - * methods from other threads. - */ -void CBoardENoses::doProcess() -{ - CObservationGasSensors::Ptr obs = - std::make_shared(); - - if (getObservation(*obs)) - { - m_state = ssWorking; - appendObservation(obs); - } - else - { - m_state = ssError; - // THROW_EXCEPTION("No observation received from the USB board!"); - } -} - -/*------------------------------------------------------------- - initialize --------------------------------------------------------------*/ -/** Tries to open the camera, after setting all the parameters with a call to - * loadConfig. - * \exception This method must throw an exception with a descriptive message - * if some critical error is found. - */ -void CBoardENoses::initialize() -{ - // We'll rather try it in doProcess() since it's quite usual that it fails - // on a first try, then works on the next ones. - /* - if (!checkConnectionAndConnect()) - THROW_EXCEPTION("Couldn't connect to the eNose board"); - */ -} - -/*------------------------------------------------------------- - setActiveChamber --------------------------------------------------------------*/ -/** Send to the MCE-nose the next Active Chamber */ - -bool CBoardENoses::setActiveChamber(unsigned char chamber) -{ - try - { - // Try to connect to the device: - CStream* comms = checkConnectionAndConnect(); - if (!comms) return false; - - // Send a byte to set the Active chamber on device. - // by default: Byte_to_send = 10_ _ _ _10 - unsigned char buf[1]; - buf[0] = ((chamber & 15) << 2) | 130; // 130= 10 0000 10 - - comms->Write(buf, 1); // Exceptions will be raised on errors here - return true; - } - catch (...) - { - // Close everything and start again: - m_stream_SERIAL.reset(); - m_stream_FTDI.reset(); - return false; - } -} diff --git a/libs/hwdrivers/src/CBoardSonars.cpp b/libs/hwdrivers/src/CBoardSonars.cpp deleted file mode 100644 index f0d10f8169..0000000000 --- a/libs/hwdrivers/src/CBoardSonars.cpp +++ /dev/null @@ -1,330 +0,0 @@ -/* +------------------------------------------------------------------------+ - | Mobile Robot Programming Toolkit (MRPT) | - | https://www.mrpt.org/ | - | | - | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | - | See: https://www.mrpt.org/Authors - All rights reserved. | - | Released under BSD License. See: https://www.mrpt.org/License | - +------------------------------------------------------------------------+ */ - -#include "hwdrivers-precomp.h" // Precompiled headers -// -#include -#include -#include -#include - -#include -#include - -using namespace mrpt::hwdrivers; -using namespace mrpt::serialization; -using namespace std; - -IMPLEMENTS_GENERIC_SENSOR(CBoardSonars, mrpt::hwdrivers) - -/*------------------------------------------------------------- - CBoardSonars --------------------------------------------------------------*/ -CBoardSonars::CBoardSonars() -{ - MRPT_START - m_usbSerialNumber = "SONAR001"; - - m_sensorLabel = "SONAR1"; - - m_gain = 6; - m_maxRange = 4.0f; - - MRPT_END -} - -/*------------------------------------------------------------- - loadConfig_sensorSpecific --------------------------------------------------------------*/ -void CBoardSonars::loadConfig_sensorSpecific( - const mrpt::config::CConfigFileBase& configSource, - const std::string& iniSection) -{ - MRPT_START - - std::vector aux; // Auxiliar vector - - // Some parameters ... - m_usbSerialNumber = configSource.read_string( - iniSection, "USB_serialNumber", m_usbSerialNumber, true); - m_gain = configSource.read_int(iniSection, "gain", m_gain, true); - m_maxRange = - configSource.read_float(iniSection, "maxRange", m_maxRange, true); - m_minTimeBetweenPings = configSource.read_float( - iniSection, "minTimeBetweenPings", m_minTimeBetweenPings, true); - // ---------------------------------------------------------------------------------------------------------------------- - ASSERT_(m_maxRange > 0 && m_maxRange <= 11); - ASSERT_(m_gain <= 16); - - // Sonar firing order ... - configSource.read_vector( - iniSection, "firingOrder", m_firingOrder, m_firingOrder, true); - - // ---------------------------------------------------------------------------------------------------------------------- - - // Individual sonar gains ... - configSource.read_vector(iniSection, "sonarGains", aux, aux, true); - - std::vector::iterator itSonar; - std::vector::iterator itAux; - for (itSonar = m_firingOrder.begin(), itAux = aux.begin(); - itSonar != m_firingOrder.end(); ++itSonar, ++itAux) - m_sonarGains[*itSonar] = *itAux; - // ---------------------------------------------------------------------------------------------------------------------- - ASSERT_(aux.size() == m_firingOrder.size()); - - // Individual sonar poses - aux.clear(); - for (itSonar = m_firingOrder.begin(); itSonar != m_firingOrder.end(); - ++itSonar) - { - configSource.read_vector( - iniSection, format("pose%i", *itSonar), aux, aux, - true); // Get the sonar poses - m_sonarPoses[*itSonar] = mrpt::math::TPose3D( - aux[0], aux[1], aux[2], DEG2RAD((float)aux[3]), - DEG2RAD((float)aux[4]), DEG2RAD((float)aux[5])); - } - // ---------------------------------------------------------------------------------------------------------------------- - ASSERT_(m_sonarGains.size() == m_firingOrder.size()); - - MRPT_END -} - -/*------------------------------------------------------------- - queryFirmwareVersion --------------------------------------------------------------*/ -bool CBoardSonars::queryFirmwareVersion(string& out_firmwareVersion) -{ - try - { - CMessage msg, msgRx; - - // Try to connect to the device: - if (!checkConnectionAndConnect()) return false; - - msg.type = 0x10; - auto arch = mrpt::serialization::archiveFrom(*this); - arch.sendMessage(msg); - - if (arch.receiveMessage(msgRx)) - { - msgRx.getContentAsString(out_firmwareVersion); - return true; - } - else - return false; - } - catch (...) - { - Close(); - return false; - } -} - -/*------------------------------------------------------------- - checkConnectionAndConnect --------------------------------------------------------------*/ -bool CBoardSonars::sendConfigCommands() -{ - try - { - if (!isOpen()) return false; - mrpt::serialization::CMessage msg, msgRx; - size_t i; - - // Send cmd for firing order: - // ---------------------------- - msg.type = 0x12; - msg.content.resize(16); - for (i = 0; i < 16; i++) - { - if (i < m_firingOrder.size()) msg.content[i] = m_firingOrder[i]; - else - msg.content[i] = 0xFF; - } - auto arch = mrpt::serialization::archiveFrom(*this); - arch.sendMessage(msg); - if (!arch.receiveMessage(msgRx)) return false; // Error - - // Send cmd for gain: - // ---------------------------- - // msg.type = 0x13; - // msg.content.resize(1); - // msg.content[0] = m_gain; - // sendMessage(msg); - // if (!receiveMessage(msgRx) ) return false; // Error - - // Send cmd for set of gains: - // ---------------------------- - msg.type = 0x16; - msg.content.resize(16); - for (i = 0; i < 16; i++) - { - if (m_sonarGains.find(i) != m_sonarGains.end()) - msg.content[i] = m_sonarGains[i]; - else - msg.content[i] = 0xFF; - } - arch.sendMessage(msg); - if (!arch.receiveMessage(msgRx)) return false; // Error - - // Send cmd for max range: - // ---------------------------- - msg.type = 0x14; - msg.content.resize(1); - msg.content[0] = (int)((m_maxRange / 0.043f) - 1); - arch.sendMessage(msg); - if (!arch.receiveMessage(msgRx)) return false; // Error - - // Send cmd for max range: - // ---------------------------- - msg.type = 0x15; - msg.content.resize(2); - auto T = (uint16_t)(m_minTimeBetweenPings * 1000.0f); - msg.content[0] = T >> 8; - msg.content[1] = T & 0x00FF; - arch.sendMessage(msg); - if (!arch.receiveMessage(msgRx)) return false; // Error - - return true; - } - catch (...) - { - // Error opening device: - Close(); - return false; - } -} - -/*------------------------------------------------------------- - getObservation --------------------------------------------------------------*/ -bool CBoardSonars::getObservation(mrpt::obs::CObservationRange& obs) -{ - try - { - obs.sensorLabel = m_sensorLabel; - obs.timestamp = mrpt::Clock::now(); - obs.minSensorDistance = 0.04f; - obs.maxSensorDistance = m_maxRange; - obs.sensorConeAperture = DEG2RAD(30.0f); - obs.sensedData.clear(); - mrpt::obs::CObservationRange::TMeasurement obsRange; - - mrpt::serialization::CMessage msg, msgRx; - - // Try to connect to the device: - if (!checkConnectionAndConnect()) return false; - - auto arch = mrpt::serialization::archiveFrom(*this); - - msg.type = 0x11; - arch.sendMessage(msg); - - if (arch.receiveMessage(msgRx)) - { - if (msgRx.content.empty()) return false; - - // For each sensor: - ASSERT_((msgRx.content.size() % 2) == 0); - vector data(msgRx.content.size() / 2); - memcpy(&data[0], &msgRx.content[0], msgRx.content.size()); - - for (size_t i = 0; i < data.size() / 2; i++) - { - uint16_t sonar_idx = data[2 * i + 0]; - uint16_t sonar_range_cm = data[2 * i + 1]; - if (sonar_range_cm != 0xFFFF && sonar_idx < 16) - { - obsRange.sensorID = sonar_idx; - obsRange.sensorPose = - m_sonarPoses[sonar_idx]; // mrpt::poses::CPose3D(); // - // sonar_idx - obsRange.sensedDistance = sonar_range_cm * 0.01f; - obs.sensedData.push_back(obsRange); - } - } - return true; - } - else - return false; - } - catch (...) - { - Close(); - return false; - } -} - -/*------------------------------------------------------------- - programI2CAddress --------------------------------------------------------------*/ -bool CBoardSonars::programI2CAddress(uint8_t currentAddress, uint8_t newAddress) -{ - try - { - mrpt::serialization::CMessage msg, msgRx; - - // Try to connect to the device: - if (!checkConnectionAndConnect()) return false; - auto arch = mrpt::serialization::archiveFrom(*this); - - msg.type = 0x20; - msg.content.resize(2); - msg.content[0] = currentAddress; - msg.content[1] = newAddress; - arch.sendMessage(msg); - - std::this_thread::sleep_for(10ms); - - return arch.receiveMessage(msgRx); - } - catch (...) - { - Close(); - return false; - } -} - -/*------------------------------------------------------------- - checkConnectionAndConnect --------------------------------------------------------------*/ -bool CBoardSonars::checkConnectionAndConnect() -{ - if (isOpen()) return true; - - try - { - OpenBySerialNumber(m_usbSerialNumber); - std::this_thread::sleep_for(10ms); - Purge(); - std::this_thread::sleep_for(10ms); - SetLatencyTimer(1); - SetTimeouts(300, 100); - - return sendConfigCommands(); - } - catch (...) - { - // Error opening device: - Close(); - return false; - } -} - -/*------------------------------------------------------------- - doProcess --------------------------------------------------------------*/ -void CBoardSonars::doProcess() -{ - mrpt::obs::CObservationRange::Ptr obs = - mrpt::obs::CObservationRange::Create(); - if (getObservation(*obs)) appendObservation(obs); -} diff --git a/libs/hwdrivers/src/CCameraSensor.cpp b/libs/hwdrivers/src/CCameraSensor.cpp index 4b43fad5db..ff92868ba0 100644 --- a/libs/hwdrivers/src/CCameraSensor.cpp +++ b/libs/hwdrivers/src/CCameraSensor.cpp @@ -331,22 +331,6 @@ void CCameraSensor::initialize() throw; } } - else if (m_grabber_type == "duo3d") - { - // m_cap_duo3D - cout << "[CCameraSensor::initialize] DUO3D stereo camera ...\n"; - - // Open it: - try - { - m_cap_duo3d = std::make_unique(m_duo3d_options); - } - catch (const std::exception& e) - { - m_state = CGenericSensor::ssError; - throw e; - } - } else if (m_grabber_type == "myntd") { cout << "[CCameraSensor::initialize] MYNTEYE-D camera ...\n"; @@ -409,7 +393,6 @@ void CCameraSensor::close() m_cap_kinect.reset(); m_cap_svs.reset(); m_cap_image_dir.reset(); - m_cap_duo3d.reset(); m_state = CGenericSensor::ssInitializing; @@ -611,9 +594,6 @@ void CCameraSensor::loadConfig_sensorSpecific( m_img_dir_is_stereo = !m_img_dir_right_format.empty(); m_img_dir_counter = m_img_dir_start_index; - // DUO3D Camera options: - m_duo3d_options.loadOptionsFrom(configSource, "DUO3DOptions"); - // SwissRanger options: m_sr_open_from_usb = configSource.read_bool(iniSection, "sr_use_usb", m_sr_open_from_usb); @@ -856,10 +836,7 @@ void CCameraSensor::getNextFrame(vector& out_obs) m_state = CGenericSensor::ssError; THROW_EXCEPTION("Error grabbing stereo images"); } - else - { - capture_ok = true; - } + else { capture_ok = true; } } else if (m_cap_svs) { @@ -1117,25 +1094,6 @@ void CCameraSensor::getNextFrame(vector& out_obs) capture_ok = true; } } - else if (m_cap_duo3d) - { - stObs = std::make_shared(); - obsIMU = std::make_shared(); - - bool thereIsIMG, thereIsIMU; - m_cap_duo3d->getObservations(*stObs, *obsIMU, thereIsIMG, thereIsIMU); - if (!thereIsIMG) - { - m_state = CGenericSensor::ssError; - THROW_EXCEPTION("Error getting observations from DUO3D camera."); - } - else if (m_cap_duo3d->captureIMUIsSet() && !thereIsIMU) - { - cout << "[CCamera, duo3d] Warning: There are no IMU data from the " - "device. Only images are being grabbed."; - } - capture_ok = true; - } else if (m_myntd) { obs3D = std::make_shared(); @@ -1153,10 +1111,7 @@ void CCameraSensor::getNextFrame(vector& out_obs) "Error getting observations from MYNTEYE-D camera."); } } - else - { - noObsCnt = 0; - } + else { noObsCnt = 0; } capture_ok = true; } else @@ -1190,9 +1145,7 @@ void CCameraSensor::getNextFrame(vector& out_obs) } else if (stObs) { - stObs->sensorLabel = (m_cap_duo3d && m_cap_duo3d->captureIMUIsSet()) - ? m_sensorLabel + "_IMG" - : m_sensorLabel; + stObs->sensorLabel = m_sensorLabel; stObs->setSensorPose(m_sensorPose); } else if (obs3D) @@ -1443,12 +1396,7 @@ void CCameraSensor::getNextFrame(vector& out_obs) } } // end show preview - if (delayed_insertion_in_obs_queue) - { - if (m_cap_duo3d && m_cap_duo3d->captureIMUIsSet() && obsIMU) - out_obs.push_back(CObservation::Ptr(obsIMU)); - } - else + if (!delayed_insertion_in_obs_queue) { if (stObs) out_obs.push_back(CObservation::Ptr(stObs)); if (obs) out_obs.push_back(CObservation::Ptr(obs)); diff --git a/libs/hwdrivers/src/CDUO3DCamera.cpp b/libs/hwdrivers/src/CDUO3DCamera.cpp deleted file mode 100644 index 1a48cb3ac3..0000000000 --- a/libs/hwdrivers/src/CDUO3DCamera.cpp +++ /dev/null @@ -1,606 +0,0 @@ -/* +------------------------------------------------------------------------+ - | Mobile Robot Programming Toolkit (MRPT) | - | https://www.mrpt.org/ | - | | - | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | - | See: https://www.mrpt.org/Authors - All rights reserved. | - | Released under BSD License. See: https://www.mrpt.org/License | - +------------------------------------------------------------------------+ */ - -#include "hwdrivers-precomp.h" // Precompiled headers -// -#include -#include -#include -#include - -#include - -// We must define & store OpenCV-specific data like this in the .cpp, we don't -// want to force users to need opencv headers: -struct TDUOParams -{ -#if MRPT_HAS_OPENCV - cv::Mat m_rectify_map_left_x; - cv::Mat m_rectify_map_left_y; - cv::Mat m_rectify_map_right_x; - cv::Mat m_rectify_map_right_y; -#endif -}; -std::map duo_params; - -// duo3d header files -#if MRPT_HAS_DUO3D -#include -#endif - -// m_duo: Opaque pointer to DUO3D's "DUOInstance": -#define M_DUO_PTR (reinterpret_cast(m_duo)) -#define M_DUO_VALUE (*M_DUO_PTR) - -using namespace std; -using namespace mrpt; -using namespace mrpt::math; -using namespace mrpt::poses; -using namespace mrpt::img; -using namespace mrpt::obs; -using namespace mrpt::hwdrivers; - -// opencv header files and namespaces -#if MRPT_HAS_OPENCV -using namespace cv; -#endif - -TCaptureOptions_DUO3D::TCaptureOptions_DUO3D() - : m_rectify_map_filename(""), - m_intrinsic_filename(""), - m_extrinsic_filename(""), - m_stereo_camera(TStereoCamera()) -{ - duo_params[this]; // Create -} - -TCaptureOptions_DUO3D::~TCaptureOptions_DUO3D() -{ - duo_params.erase(this); // Remove entry -} - -TCaptureOptions_DUO3D::TYMLReadResult - TCaptureOptions_DUO3D::m_rectify_map_from_yml(const string& _file_name) -{ -#if MRPT_HAS_OPENCV - const string file_name = - _file_name.empty() ? m_rectify_map_filename : _file_name; - - TDUOParams& dp = duo_params[this]; - - string aux = mrpt::system::extractFileName(file_name); - const size_t found = aux.find( - mrpt::format("_R%dx%d_", this->m_img_width, this->m_img_height)); - if (found == std::string::npos) - { - dp.m_rectify_map_left_x = dp.m_rectify_map_left_y = - dp.m_rectify_map_right_x = dp.m_rectify_map_right_y = cv::Mat(); - return yrr_NAME_NON_CONSISTENT; - } - // read file - FileStorage fs(file_name, FileStorage::READ); - fs["R0X"] >> dp.m_rectify_map_left_x; - fs["R0Y"] >> dp.m_rectify_map_left_y; - fs["R1X"] >> dp.m_rectify_map_right_x; - fs["R1Y"] >> dp.m_rectify_map_right_y; - - if (dp.m_rectify_map_left_x.size() == Size(0, 0) || - dp.m_rectify_map_left_y.size() == Size(0, 0) || - dp.m_rectify_map_right_x.size() == Size(0, 0) || - dp.m_rectify_map_right_y.size() == Size(0, 0)) - return yrr_EMPTY; - - return yrr_OK; -#else - THROW_EXCEPTION("This function requires building with OpenCV support"); -#endif -} - -TCaptureOptions_DUO3D::TYMLReadResult - TCaptureOptions_DUO3D::m_camera_ext_params_from_yml( - const string& _file_name) -{ -#if MRPT_HAS_OPENCV - const string file_name = - _file_name.empty() ? m_extrinsic_filename : _file_name; - - // this will look for R and t matrixes - cv::Mat aux_mat; - bool empty = false; - string aux = mrpt::system::extractFileName(file_name); - const size_t found = aux.find( - mrpt::format("_R%dx%d_", this->m_img_width, this->m_img_height)); - if (found == std::string::npos) - { - m_stereo_camera.rightCameraPose = TPose3DQuat(0, 0, 0, 1.0, 0, 0, 0); - return yrr_NAME_NON_CONSISTENT; - } - // read file - FileStorage fs(file_name, FileStorage::READ); - CMatrixDouble33 M; - CMatrixDouble13 t; - CMatrixDouble44 M2; - - // rotation matrix - fs["R"] >> aux_mat; - if (aux_mat.size() == Size(3, 3)) - { - for (size_t k1 = 0; k1 < 3; ++k1) - for (size_t k2 = 0; k2 < 3; ++k2) - M(k1, k2) = aux_mat.at(k1, k2); - } - else - { - empty = true; - m_stereo_camera.rightCameraPose = TPose3DQuat(0, 0, 0, 1, 0, 0, 0); - } - - // translation - fs["T"] >> aux_mat; - if (aux_mat.size() == Size(1, 3)) - { - t(0, 0) = aux_mat.at(0, 0) / 1000.0; - t(0, 1) = aux_mat.at(1, 0) / 1000.0; - t(0, 2) = aux_mat.at(2, 0) / 1000.0; - } - else - { - empty = true; - m_stereo_camera.rightCameraPose = TPose3DQuat(0, 0, 0, 1, 0, 0, 0); - } - - if (empty) return yrr_EMPTY; - - m_stereo_camera.rightCameraPose = CPose3DQuat(CPose3D(M, t)).asTPose(); - return yrr_OK; -#else - THROW_EXCEPTION("This function requires building with OpenCV support"); -#endif -} - -TCaptureOptions_DUO3D::TYMLReadResult - TCaptureOptions_DUO3D::m_camera_int_params_from_yml( - const string& _file_name) -{ -#if MRPT_HAS_OPENCV - const string file_name = - _file_name.empty() ? m_intrinsic_filename : _file_name; - - // this will look for M1, D1, M2 and D2 matrixes - cv::Mat aux_mat; - bool empty = false; - string aux = mrpt::system::extractFileName(file_name); - const size_t found = aux.find( - mrpt::format("_R%dx%d_", this->m_img_width, this->m_img_height)); - if (found == std::string::npos) - { - m_stereo_camera.leftCamera.intrinsicParams.setZero(); - m_stereo_camera.leftCamera.dist.fill(0); - m_stereo_camera.rightCamera.intrinsicParams.setZero(); - m_stereo_camera.rightCamera.dist.fill(0); - - return yrr_NAME_NON_CONSISTENT; - } - // read file - FileStorage fs(file_name, FileStorage::READ); - - // left camera - fs["M1"] >> aux_mat; - if (aux_mat.size() == Size(0, 0)) - { - empty = true; - m_stereo_camera.leftCamera.intrinsicParams.setZero(); - } - m_stereo_camera.leftCamera.setIntrinsicParamsFromValues( - aux_mat.at(0, 0), aux_mat.at(1, 1), - aux_mat.at(0, 2), aux_mat.at(1, 2)); - - fs["D1"] >> aux_mat; - if (aux_mat.size() == Size(0, 0)) - { - empty = true; - m_stereo_camera.leftCamera.dist.fill(0); - } - m_stereo_camera.leftCamera.setDistortionPlumbBob( - aux_mat.at(0, 0), aux_mat.at(0, 1), - aux_mat.at(0, 2), aux_mat.at(0, 3), - aux_mat.at(0, 4)); - - fs["M2"] >> aux_mat; - if (aux_mat.size() == Size(0, 0)) - { - empty = true; - m_stereo_camera.rightCamera.intrinsicParams.setZero(); - } - m_stereo_camera.rightCamera.setIntrinsicParamsFromValues( - aux_mat.at(0, 0), aux_mat.at(1, 1), - aux_mat.at(0, 2), aux_mat.at(1, 2)); - - fs["D2"] >> aux_mat; - if (aux_mat.size() == Size(0, 0)) - { - empty = true; - m_stereo_camera.rightCamera.dist.fill(0); - } - m_stereo_camera.rightCamera.setDistortionPlumbBob( - aux_mat.at(0, 0), aux_mat.at(0, 1), - aux_mat.at(0, 2), aux_mat.at(0, 3), - aux_mat.at(0, 4)); - - return empty ? yrr_EMPTY : yrr_OK; -#else - THROW_EXCEPTION("This function requires building with OpenCV support"); -#endif -} - -void TCaptureOptions_DUO3D::loadOptionsFrom( - const mrpt::config::CConfigFileBase& configSource, - const std::string& iniSection, const std::string& prefix) -{ - m_img_width = configSource.read_int(iniSection, "image_width", m_img_width); - m_img_height = - configSource.read_int(iniSection, "image_height", m_img_height); - - m_fps = configSource.read_float(iniSection, "fps", m_fps); - m_exposure = configSource.read_float(iniSection, "exposure", m_exposure); - m_led = configSource.read_float(iniSection, "led", m_led); - m_gain = configSource.read_float(iniSection, "gain", m_gain); - - m_capture_rectified = configSource.read_bool( - iniSection, "capture_rectified", m_capture_rectified); - m_capture_imu = - configSource.read_bool(iniSection, "capture_imu", m_capture_imu); - m_calibration_from_file = configSource.read_bool( - iniSection, "calibration_from_file", m_calibration_from_file); - - if (m_calibration_from_file) - { - m_intrinsic_filename = configSource.read_string( - iniSection, "intrinsic_filename", m_intrinsic_filename); - m_extrinsic_filename = configSource.read_string( - iniSection, "extrinsic_filename", m_extrinsic_filename); - m_stereo_camera.leftCamera.ncols = m_stereo_camera.rightCamera.ncols = - m_img_width; - m_stereo_camera.leftCamera.nrows = m_stereo_camera.rightCamera.nrows = - m_img_height; - } - else - m_stereo_camera.loadFromConfigFile("DUO3D", configSource); - - if (m_capture_rectified) - { - m_rectify_map_filename = configSource.read_string( - iniSection, "rectify_map_filename", m_rectify_map_filename); - } // end-capture-rectified -} - -#if MRPT_HAS_DUO3D -static void CALLBACK DUOCallback(const PDUOFrame pFrameData, void* pUserData) -{ - CDUO3DCamera* obj = static_cast(pUserData); - obj->setDataFrame(reinterpret_cast(pFrameData)); - SetEvent(reinterpret_cast(obj->getEvent())); -} -#endif - -/** Default constructor. */ -CDUO3DCamera::CDUO3DCamera() : m_options(TCaptureOptions_DUO3D()) -{ -#if MRPT_HAS_DUO3D - m_duo = new DUOInstance[1]; - M_DUO_VALUE = nullptr; // m_duo = nullptr; - - m_pframe_data = nullptr; - m_evFrame = CreateEvent(nullptr, FALSE, FALSE, nullptr); -#else - THROW_EXCEPTION( - "MRPT has been compiled with 'MRPT_BUILD_DUO3D'=OFF, so this class " - "cannot be used."); -#endif -} // end-constructor - -/** Custom initialization and start grabbing constructor. */ -CDUO3DCamera::CDUO3DCamera(const TCaptureOptions_DUO3D& options) - : m_duo(nullptr) -{ -#if MRPT_HAS_DUO3D - m_duo = new DUOInstance[1]; - M_DUO_VALUE = nullptr; // m_duo = nullptr; - - m_pframe_data = nullptr; - m_evFrame = CreateEvent(nullptr, FALSE, FALSE, nullptr); - this->open(options); -#else - THROW_EXCEPTION( - "MRPT has been compiled with 'MRPT_BUILD_DUO3D'=OFF, so this class " - "cannot be used."); -#endif -} // end-constructor - -/** Destructor */ -CDUO3DCamera::~CDUO3DCamera() -{ -#if MRPT_HAS_DUO3D - this->close(); - if (m_duo) - { - delete[] M_DUO_PTR; - m_duo = nullptr; - } -#endif -} // end-destructor - -/** Tries to open the camera with the given options. Raises an exception on - * error. \sa close() */ -void CDUO3DCamera::open( - const TCaptureOptions_DUO3D& options, const bool startCapture) -{ -#if MRPT_HAS_DUO3D - if (M_DUO_VALUE) this->close(); - this->m_options = options; - - if (this->m_options.m_calibration_from_file) - { - // get intrinsic parameters - TCaptureOptions_DUO3D::TYMLReadResult res = - this->m_options.m_camera_int_params_from_yml(); - if (res == TCaptureOptions_DUO3D::yrr_EMPTY) - cout << "[CDUO3DCamera] Warning: Some of the intrinsic params " - "could not be read (size=0). Check file content." - << endl; - else if (res == TCaptureOptions_DUO3D::yrr_NAME_NON_CONSISTENT) - cout << "[CDUO3DCamera] Warning: Intrinsic params filename is not " - "consistent with image size. Are you using the correct " - "calibration?. All params set to zero." - << endl; - - // get extrinsic parameters - res = this->m_options.m_camera_ext_params_from_yml(); - if (res == TCaptureOptions_DUO3D::yrr_EMPTY) - cout << "[CDUO3DCamera] Warning: Some of the extrinsic params " - "could not be read (size!=3x3). Check file content." - << endl; - else if (res == TCaptureOptions_DUO3D::yrr_NAME_NON_CONSISTENT) - cout << "[CDUO3DCamera] Warning: Extrinsic params filename is not " - "consistent with image size. Are you using the correct " - "calibration?. All params set to zero." - << endl; - - if (this->m_options.m_capture_rectified) - { - if (!this->m_options.m_rectify_map_filename.empty()) - { - // read "rectify_map" - res = this->m_options.m_rectify_map_from_yml(); - if (res == TCaptureOptions_DUO3D::yrr_EMPTY) - cout << "[CDUO3DCamera] Warning: Rectification map could " - "not be read (size==0). Check file content." - << endl; - else if (res == TCaptureOptions_DUO3D::yrr_NAME_NON_CONSISTENT) - cout << "[CDUO3DCamera] Warning: Rectification map " - "filename is not consistent with image size. Are " - "you using the correct calibration?. Rectification " - "map set to zero." - << endl; - - this->m_options.m_capture_rectified = - res == TCaptureOptions_DUO3D::yrr_OK; - - // const size_t area = - // this->m_options.dp.m_rectify_map_left_x.size().area(); - TDUOParams& dp = duo_params[&(this->m_options)]; - const size_t area = dp.m_rectify_map_left_x.size().area(); - vector v_left_x(area), v_right_x(area); - vector v_left_y(area), v_right_y(area); - - for (size_t k = 0; k < area; ++k) - { - v_left_x[k] = dp.m_rectify_map_left_x.at(k); - v_left_y[k] = dp.m_rectify_map_left_y.at(k); - v_right_x[k] = dp.m_rectify_map_right_x.at(k); - v_right_y[k] = dp.m_rectify_map_right_y.at(k); - - // v_left_x[k] = - // this->m_options.dp.m_rectify_map_left_x.at(k); - // v_left_y[k] = - // this->m_options.dp.m_rectify_map_left_y.at(k); - // v_right_x[k] = - // this->m_options.dp.m_rectify_map_right_x.at(k); - // v_right_y[k] = - // this->m_options.dp.m_rectify_map_right_y.at(k); - } - m_rectify_map.setFromCamParams(this->m_options.m_stereo_camera); - // m_rectify_map.setRectifyMaps( v_left_x, v_left_y, v_right_x, - // v_right_y ); - } - else - { - cout << "[CDUO3DCamera] Warning: Calibration information is " - "set to be read from a file, but the file was not " - "specified. Unrectified images will be grabbed." - << endl; - } - } // end-if - } // end-if - else if (this->m_options.m_capture_rectified) - { - m_rectify_map.setFromCamParams(this->m_options.m_stereo_camera); - } - - // Find optimal binning parameters for given (width, height) - // This maximizes sensor imaging area for given resolution - int binning = DUO_BIN_NONE; - if (this->m_options.m_img_width <= 752 / 2) binning += DUO_BIN_HORIZONTAL2; - if (this->m_options.m_img_height <= 480 / 4) binning += DUO_BIN_VERTICAL4; - else if (this->m_options.m_img_height <= 480 / 2) - binning += DUO_BIN_VERTICAL2; - - // Check if we support given resolution (width, height, binning, fps) - DUOResolutionInfo ri; - if (!EnumerateResolutions( - &ri, 1, this->m_options.m_img_width, this->m_options.m_img_height, - binning, this->m_options.m_fps)) - THROW_EXCEPTION("[CDUO3DCamera] Error: Resolution not supported."); - - if (!OpenDUO(&M_DUO_VALUE)) // was: m_duo - THROW_EXCEPTION("[CDUO3DCamera] Error: Camera could not be opened."); - - // Get and print some DUO parameter values - char name[260], version[260]; - GetDUODeviceName(M_DUO_VALUE, name); - GetDUOFirmwareVersion(M_DUO_VALUE, version); - cout << "[CDUO3DCamera::open] DUO3DCamera name: " << name << " (v" - << version << ")" << endl; - - // Set selected resolution - SetDUOResolutionInfo(M_DUO_VALUE, ri); - - // Set selected camera settings - SetDUOExposure(M_DUO_VALUE, m_options.m_exposure); - SetDUOGain(M_DUO_VALUE, m_options.m_gain); - SetDUOLedPWM(M_DUO_VALUE, m_options.m_led); - - // Start capture - if (startCapture) - { - if (!StartDUO(M_DUO_VALUE, DUOCallback, reinterpret_cast(this))) - THROW_EXCEPTION( - "[CDUO3DCamera] Error: Camera could not be started.") - } - -#endif -} // end-open - -/*------------------------------------------------------------- - getObservations --------------------------------------------------------------*/ -void CDUO3DCamera::getObservations( - CObservationStereoImages& outObservation_img, - CObservationIMU& outObservation_imu, bool& there_is_img, bool& there_is_imu) -{ -#if MRPT_HAS_DUO3D - there_is_img = false; - there_is_imu = false; - - m_pframe_data = m_get_duo_frame(); - if (!m_pframe_data) return; - - // ----------------------------------------------- - // Extract the observation: - // ----------------------------------------------- - outObservation_img.timestamp = outObservation_imu.timestamp = - mrpt::Clock::now(); - - outObservation_img.setStereoCameraParams(m_options.m_stereo_camera); - outObservation_img.imageLeft.loadFromMemoryBuffer( - m_options.m_img_width, m_options.m_img_height, false, - (unsigned char*)reinterpret_cast(m_pframe_data)->leftData); - - outObservation_img.imageRight.loadFromMemoryBuffer( - m_options.m_img_width, m_options.m_img_height, false, - (unsigned char*)reinterpret_cast(m_pframe_data)->rightData); - - if (this->m_options.m_capture_rectified) - m_rectify_map.rectify(outObservation_img); - - there_is_img = true; - - if (this->m_options.m_capture_imu) - { - if (!reinterpret_cast(m_pframe_data)->accelerometerPresent) - { - cout << "[CDUO3DCamera] Warning: This device does not provide IMU " - "data. No IMU observations will be created." - << endl; - this->m_options.m_capture_imu = false; - } - else - { - // Accelerometer data - for (size_t k = 0; k < 3; ++k) - { - outObservation_imu.rawMeasurements[k] = - reinterpret_cast(m_pframe_data)->accelData[k]; - outObservation_imu.dataIsPresent[k] = true; - } - - // Gyroscopes data - for (size_t k = 0; k < 3; ++k) - { - outObservation_imu.rawMeasurements[k + 3] = - reinterpret_cast(m_pframe_data)->gyroData[k]; - outObservation_imu.dataIsPresent[k + 3] = true; - } - there_is_imu = true; - } // end else - } // end-imu-info -#endif -} - -/** Closes DUO camera */ -void CDUO3DCamera::close() -{ -#if MRPT_HAS_DUO3D - if (!M_DUO_VALUE) return; - StopDUO(M_DUO_VALUE); - CloseDUO(M_DUO_VALUE); - M_DUO_VALUE = nullptr; -#endif -} // end-close - -// Waits until the new DUO frame is ready and returns it -void* CDUO3DCamera::m_get_duo_frame() -{ -#if MRPT_HAS_DUO3D - if (M_DUO_VALUE == nullptr) return 0; - if (WaitForSingleObject(m_evFrame, 1000) == WAIT_OBJECT_0) - return m_pframe_data; - else - return nullptr; -#else - return nullptr; // return something to silent compiler warnings. -#endif -} - -void CDUO3DCamera::m_set_exposure(float value) -{ -#if MRPT_HAS_DUO3D - if (M_DUO_VALUE == nullptr) return; - SetDUOExposure(M_DUO_VALUE, value); -#endif -} - -void CDUO3DCamera::m_set_gain(float value) -{ -#if MRPT_HAS_DUO3D - if (M_DUO_VALUE == nullptr) return; - SetDUOGain(M_DUO_VALUE, value); -#endif -} - -void CDUO3DCamera::m_set_led(float value) -{ -#if MRPT_HAS_DUO3D - if (M_DUO_VALUE == nullptr) return; - SetDUOLedPWM(M_DUO_VALUE, value); -#endif -} - -/** Queries the DUO3D Camera firmware version */ -bool CDUO3DCamera::queryVersion(std::string version, bool printOutVersion) -{ -#if MRPT_HAS_DUO3D - version = std::string(GetLibVersion()); - if (printOutVersion) - std::cout << "DUO3D Camera library version: " << version << std::endl; - return true; -#else - return false; -#endif -} diff --git a/libs/hwdrivers/src/CPtuDPerception.cpp b/libs/hwdrivers/src/CPtuDPerception.cpp deleted file mode 100644 index 0f7bb806c2..0000000000 --- a/libs/hwdrivers/src/CPtuDPerception.cpp +++ /dev/null @@ -1,820 +0,0 @@ -/* +------------------------------------------------------------------------+ - | Mobile Robot Programming Toolkit (MRPT) | - | https://www.mrpt.org/ | - | | - | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | - | See: https://www.mrpt.org/Authors - All rights reserved. | - | Released under BSD License. See: https://www.mrpt.org/License | - +------------------------------------------------------------------------+ */ - -#include "hwdrivers-precomp.h" // Precompiled headers -// -#include -#include -#include -#include - -#include -#include -#include -#include - -using namespace std; -using namespace mrpt; -using namespace mrpt::hwdrivers; -using namespace mrpt::system; - -/*------------------------------------------------------------- - moveToAbsPos --------------------------------------------------------------*/ - -bool CPtuDPerception::moveToAbsPos(char axis, double nRad) -{ - if (!radAsign(axis, 'P', nRad)) return false; - - return true; -} - -/*------------------------------------------------------------- - absPosQ --------------------------------------------------------------*/ - -bool CPtuDPerception::absPosQ(char axis, double& nRad) -{ - return radQuerry(axis, 'P', nRad); -} - -/*------------------------------------------------------------- - moveToOffPos --------------------------------------------------------------*/ - -bool CPtuDPerception::moveToOffPos(char axis, double nRad) -{ - if (!radAsign(axis, 'O', nRad)) return false; - - return true; -} - -/*------------------------------------------------------------- - offPosQ --------------------------------------------------------------*/ - -bool CPtuDPerception::offPosQ(char axis, double& nRad) -{ - return radQuerry(axis, 'O', nRad); -} - -/*------------------------------------------------------------- - maxPosQ --------------------------------------------------------------*/ - -bool CPtuDPerception::maxPosQ(char axis, double& nRad) -{ - return radQuerry(axis, 'X', nRad); -} - -/*------------------------------------------------------------- - minPosQ --------------------------------------------------------------*/ - -bool CPtuDPerception::minPosQ(char axis, double& nRad) -{ - return radQuerry(axis, 'N', nRad); -} - -/*------------------------------------------------------------- - speed --------------------------------------------------------------*/ - -bool CPtuDPerception::speed(char axis, double radSec) -{ - return radAsign(axis, 'S', radSec); -} - -/*------------------------------------------------------------- - speedQ --------------------------------------------------------------*/ - -bool CPtuDPerception::speedQ(char axis, double& radSec) -{ - return radQuerry(axis, 'S', radSec); -} - -/*------------------------------------------------------------- - aceleration --------------------------------------------------------------*/ - -bool CPtuDPerception::aceleration(char axis, double radSec2) -{ - return radAsign(axis, 'A', radSec2); -} - -/*------------------------------------------------------------- - acelerationQ --------------------------------------------------------------*/ - -bool CPtuDPerception::acelerationQ(char axis, double& radSec2) -{ - return radQuerry(axis, 'A', radSec2); -} - -/*------------------------------------------------------------- - baseSpeed --------------------------------------------------------------*/ - -bool CPtuDPerception::baseSpeed(char axis, double radSec) -{ - return radAsign(axis, 'B', radSec); -} - -/*------------------------------------------------------------- - baseSpeedQ --------------------------------------------------------------*/ - -bool CPtuDPerception::baseSpeedQ(char axis, double& radSec) -{ - return radQuerry(axis, 'B', radSec); -} - -/*------------------------------------------------------------- - upperSpeed --------------------------------------------------------------*/ - -bool CPtuDPerception::upperSpeed(char axis, double radSec) -{ - return radAsign(axis, 'U', radSec); -} - -/*------------------------------------------------------------- - upperSpeedQ --------------------------------------------------------------*/ - -bool CPtuDPerception::upperSpeedQ(char axis, double& radSec) -{ - return radQuerry(axis, 'U', radSec); -} - -/*------------------------------------------------------------- - lowerSpeed --------------------------------------------------------------*/ - -bool CPtuDPerception::lowerSpeed(char axis, double radSec) -{ - return radAsign(axis, 'L', radSec); -} - -/*------------------------------------------------------------- - lowerSpeedQ --------------------------------------------------------------*/ - -bool CPtuDPerception::lowerSpeedQ(char axis, double& radSec) -{ - return radQuerry(axis, 'L', radSec); -} - -/*------------------------------------------------------------- - enableLimitsQ --------------------------------------------------------------*/ - -bool CPtuDPerception::enableLimitsQ(bool& enable) -{ - char response[150]; - - if (!transmit("L") || !receive("L", response)) return false; - - if (strstr(upperCase(response).c_str(), "ENABLE") != nullptr) enable = true; - else - enable = false; - - return true; -} - -/*------------------------------------------------------------- - enableLimits --------------------------------------------------------------*/ - -bool CPtuDPerception::enableLimits(bool set) -{ - if (set) return (transmit("LE") && receive("LE", nullptr)); - else - return (transmit("LD") && receive("LD", nullptr)); -} - -/*------------------------------------------------------------- - inmediateExecution --------------------------------------------------------------*/ - -bool CPtuDPerception::inmediateExecution(bool set) -{ - if (set) return (transmit("I") && receive("I", nullptr)); - else - return (transmit("S") && receive("S", nullptr)); -} - -/*------------------------------------------------------------- - aWait --------------------------------------------------------------*/ - -bool CPtuDPerception::aWait() -{ - return (transmit("A") && receive("A", nullptr)); -} - -/*------------------------------------------------------------- - haltAll --------------------------------------------------------------*/ - -bool CPtuDPerception::haltAll() -{ - return (transmit("H") && receive("H", nullptr)); -} - -/*------------------------------------------------------------- - halt --------------------------------------------------------------*/ - -bool CPtuDPerception::halt(char axis) -{ - char sTrans[3]; - sTrans[0] = 'H'; - sTrans[1] = axis; - sTrans[2] = '\0'; - - return (transmit(sTrans) && receive(sTrans, nullptr)); -} - -/*------------------------------------------------------------- - reset --------------------------------------------------------------*/ - -bool CPtuDPerception::reset() -{ - if (!transmit("R")) return false; - receive("R", nullptr); - - return panTiltHitError(); -} - -/*------------------------------------------------------------- - save --------------------------------------------------------------*/ - -bool CPtuDPerception::save() -{ - return (transmit("DS") && receive("DS", nullptr)); -} - -/*------------------------------------------------------------- - restoreDefaults --------------------------------------------------------------*/ - -bool CPtuDPerception::restoreDefaults() -{ - return (transmit("DR") && receive("DR", nullptr)); -} - -/*------------------------------------------------------------- - restoreFactoryDefaults --------------------------------------------------------------*/ - -bool CPtuDPerception::restoreFactoryDefaults() -{ - return (transmit("DF") && receive("DF", nullptr)); -} - -/*------------------------------------------------------------- - version --------------------------------------------------------------*/ - -bool CPtuDPerception::version(char* sVersion) -{ - return (transmit("V") && receive("V", sVersion)); -} - -/*------------------------------------------------------------- - powerModeQ --------------------------------------------------------------*/ - -bool CPtuDPerception::powerModeQ(bool transit, char& mode) -{ - const char* response = ""; - - if (transit) - { - if (!transmit("PM")) return false; - else if (!transmit("PH")) - return false; - } - - if (strstr(upperCase(response).c_str(), "REGULAR") != nullptr) - mode = Regular; - else if (strstr(upperCase(response).c_str(), "HIGH") != nullptr) - mode = High; - else if (strstr(upperCase(response).c_str(), "LOW") != nullptr) - mode = Low; - else // OFF - mode = Off; - - return true; -} - -/*------------------------------------------------------------- - powerMode --------------------------------------------------------------*/ - -bool CPtuDPerception::powerMode(bool transit, char mode) -{ - char sTrans[4]; //=""; - sTrans[0] = 'P'; - sTrans[1] = transit ? 'M' : 'H'; - sTrans[2] = mode; - sTrans[3] = '\0'; - - return (transmit(sTrans) && receive(sTrans, nullptr)); -} - -/*------------------------------------------------------------- - init --------------------------------------------------------------*/ - -bool CPtuDPerception::init(const string& port) -{ - try - { - serPort.open(port); - - cout << endl << "[INFO] Start PTU communication config:" << endl; - - cout << "[PTU::OpenSerialPort] Opening serial port..."; - - if (serPort.isOpen()) {} - else - { - cout << " Error opening serial port"; - return false; - } - - cout << "OK" << endl; - - cout << "[PTU::SetTimeouts] Setting timeouts..."; - serPort.setTimeouts(1000, 1, 1000, 1, 1000); - cout << "OK" << endl; - - cout << "[PTU::setBaudRate] Setting baud rate..."; - serPort.setConfig(9600); - cout << "OK" << endl; - - // PTU initial configuration - cout << "[PTU::setInitialConfiguration] Setting initial " - "configuration..."; - if ((!verbose(true)) || // Original: false Actual: true - (!resolution()) || (!echoMode(true)) || (!inmediateExecution(true))) - { - cout << " Error setting initial configuration"; - serPort.close(); - return false; - } - - cout << "OK" << endl - << endl - << "[INFO] Pan Resolution: " << panResolution << " radians | " - << RAD2DEG(panResolution) << "degrees"; - cout << endl - << "[INFO] TitlResolution: " << tiltResolution << " radians | " - << RAD2DEG(tiltResolution) << "degrees" << endl - << endl; - } - catch (exception& e) - { - cerr << e.what() << endl; - return false; - } - - return true; -} - -/*------------------------------------------------------------- - close --------------------------------------------------------------*/ - -void CPtuDPerception::close() { serPort.close(); } -/*------------------------------------------------------------- - radError --------------------------------------------------------------*/ - -double CPtuDPerception::radError(char axis, double nRadMoved) -{ - double div; - - if (axis == Pan) - div = nRadMoved - long(nRadMoved / panResolution) * panResolution; - else - div = nRadMoved - long(nRadMoved / tiltResolution) * tiltResolution; - - return div; -} - -/*------------------------------------------------------------- - transmit --------------------------------------------------------------*/ - -bool CPtuDPerception::transmit(const char* command) -{ - char str[20] = ""; - - strcpy(str, command); - strcat(str, " "); - - size_t written = serPort.Write(str, strlen(str)); - - if (!written) { return false; } - - return true; -} - -/*------------------------------------------------------------- - receive --------------------------------------------------------------*/ - -bool CPtuDPerception::receive(const char* command, char* response) -{ - int cnt = 0; - unsigned long nReaden; - char str[150] = ""; - char* tmp; - - do - { - nReaden = serPort.Read(&str[cnt], 1); - if (nReaden != 0) cnt++; - } while ( - (nReaden != 0) && - (((tmp = strstr(str, command)) == nullptr) || (str[cnt - 1] != '\n'))); - - if (nReaden == 0) - { - nError = nError * TimeoutError; - return false; - } - - if (response != nullptr) - { - //*response=new char[150]; - strcpy(response, tmp); - } - - // cout << str << endl; - - if (strstr(tmp, "!") == nullptr) - { - nError = nError * NoError; - return true; - } - - if ((strstr(tmp, "!P") != nullptr) && (strstr(tmp, "!T") != nullptr)) - nError = nError * PanTiltHitError; - else if (strstr(tmp, "!T") != nullptr) - nError = nError * TiltHitError; - else if (strstr(tmp, "!P") != nullptr) - nError = nError * PanHitError; - else if (strstr(tmp, "! Maximum") != nullptr) - nError = nError * MaxLimitError; - else if (strstr(tmp, "! Minimum") != nullptr) - nError = nError * MinLimitError; - else if (strstr(tmp, "! Value") != nullptr) - nError = nError * OutOfRange; - else if (strstr(tmp, "! Illegal") != nullptr) - nError = nError * IllegalCommandError; - else - nError = nError * UnExpectedError; - - return false; -} - -/*------------------------------------------------------------- - verboseQ --------------------------------------------------------------*/ - -bool CPtuDPerception::verboseQ(bool& mode) -{ - char response[150]; - - if (!transmit("F") || !receive("F", response)) return false; - - if (strstr(response, "VERBOSE") != nullptr) mode = true; - else - mode = false; - - return true; -} - -/*------------------------------------------------------------- - verbose --------------------------------------------------------------*/ - -bool CPtuDPerception::verbose(bool set) -{ - if (set) return (transmit("FV") && (receive("FV", nullptr))); - else - return (transmit("FT") && (receive("FT", nullptr))); -} - -/*------------------------------------------------------------- - echoModeQ --------------------------------------------------------------*/ - -bool CPtuDPerception::echoModeQ(bool& mode) -{ - char response[150]; - - if (!transmit("E") || !receive("E", response)) return false; - - if (strstr(upperCase(response).c_str(), "ENABLE") != nullptr) mode = true; - else - mode = false; - - return true; -} - -/*------------------------------------------------------------- - echoMode --------------------------------------------------------------*/ - -bool CPtuDPerception::echoMode(bool mode) -{ - if (mode) return (transmit("EE") && receive("EE", nullptr)); - else - return (transmit("ED") && receive("ED", nullptr)); -} - -/*------------------------------------------------------------- - resolution --------------------------------------------------------------*/ - -bool CPtuDPerception::resolution() -{ - char response[150]; - - if ((!transmit("PR")) || (!receive("PR", response))) return false; - panResolution = DEG2RAD(convertToDouble(response) / 3600); - - if ((!transmit("TR")) || (!receive("TR", response))) return false; - tiltResolution = DEG2RAD(convertToDouble(response) / 3600); - - return true; -} - -/*------------------------------------------------------------- - radQuerry --------------------------------------------------------------*/ - -bool CPtuDPerception::radQuerry(char axis, char command, double& rad) -{ - char response[150]; - char sTrans[3]; - - sTrans[0] = axis; - sTrans[1] = command; - sTrans[2] = '\0'; - - if ((!transmit(sTrans)) || (!receive(sTrans, response))) return false; - - rad = posToRad(axis, convertToLong(response)); - - return true; -} - -/*------------------------------------------------------------- - radAsign --------------------------------------------------------------*/ - -bool CPtuDPerception::radAsign(char axis, char command, double nRad) -{ - char sPos[20]; - char sTrans[22]; - - char response[150]; - - os::sprintf(sPos, sizeof(sPos), "%li", radToPos(axis, nRad)); - - sTrans[0] = axis; - sTrans[1] = command; - strcpy(&sTrans[2], sPos); - - return (transmit(sTrans) && receive(sTrans, response)); -} - -/*------------------------------------------------------------- - scan --------------------------------------------------------------*/ - -bool CPtuDPerception::scan( - char axis, int tWait, float initial, float final, double radPre) -{ - // Check initial and final positions - if (initial < final) - { - float aux = initial; - initial = final; - final = aux; - } - - // Go to initial position - moveToAbsPos(axis, initial); - aWait(); - - std::this_thread::sleep_for(500ms); - - double j = 0; - offPosQ(axis, j); - - long steps = radToPos(axis, radPre); - long totalSteps; - - // Obtain total number of steps - int initialPos = radToPos(axis, initial); - int finalPos = radToPos(axis, final); - - totalSteps = std::abs(initialPos - finalPos); - - // Performs first sweep - for (int i = 0; i < totalSteps / steps; i++) - { - if (initial > final) { moveToOffPos(axis, -radPre); } - else - { - moveToOffPos(axis, radPre); - } - offPosQ(axis, j); - std::this_thread::sleep_for(std::chrono::milliseconds(tWait)); - } - - // Adjust steps for second scan - moveToOffPos(axis, radPre / 2); - aWait(); - - // Performs seecond scan - for (int i = 0; i < (totalSteps / steps) - 1; i++) - { - if (initial > final) { moveToOffPos(axis, radPre); } - else - { - moveToOffPos(axis, -radPre); - } - offPosQ(axis, j); - std::this_thread::sleep_for(std::chrono::milliseconds(tWait)); - } - - offPosQ(axis, j); - - // Return to initial position - moveToAbsPos(axis, 0); - - return true; -} - -/*------------------------------------------------------------- - radToPos --------------------------------------------------------------*/ - -long CPtuDPerception::radToPos(char axis, double nrad) -{ - if (axis == Pan) return (long)(nrad / panResolution); - else - return (long)(nrad / tiltResolution); -} - -/*------------------------------------------------------------- - posToRad --------------------------------------------------------------*/ - -double CPtuDPerception::posToRad(char axis, long nPos) -{ - if (axis == Pan) return (double)nPos * panResolution; - else - return (double)nPos * tiltResolution; -} - -/*------------------------------------------------------------- - convertToLong --------------------------------------------------------------*/ - -long CPtuDPerception::convertToLong(char* sLong) -{ - char* result = strpbrk(sLong, "-0123456789"); - char* stop; - - return strtol(result, &stop, 10); -} - -/*------------------------------------------------------------- - convertToDouble --------------------------------------------------------------*/ - -double CPtuDPerception::convertToDouble(char* sDouble) -{ - char* result = strpbrk(sDouble, "-0123456789"); - char* stop; - - return strtod(result, &stop); -} - -/*------------------------------------------------------------- - checkError --------------------------------------------------------------*/ - -int CPtuDPerception::checkErrors() -{ - int code = 0; - - // Check for errors - if (noError()) { code = 0; } - else - { - if (comError()) { code = 1; } - else if (timeoutError()) - { - code = 2; - } - else if (initError()) - { - code = 3; - } - else if (panTiltHitError()) - { - code = 4; - } - else if (panHitError()) - { - code = 5; - } - else if (tiltHitError()) - { - code = 6; - } - else if (maxLimitError()) - { - code = 7; - } - else if (minLimitError()) - { - code = 8; - } - else if (outOfRange()) - { - code = 9; - } - else if (illegalCommandError()) - { - code = 10; - } - else if (unExpectedError()) - { - code = 11; - } - } - - return code; -} - -/*------------------------------------------------------------- - nVersion --------------------------------------------------------------*/ - -void CPtuDPerception::nversion(double& nVersion) -{ - cout << "[ERROR] Function not defined for this PTU model"; - nVersion = 0; -} - -/*------------------------------------------------------------- - setLimits --------------------------------------------------------------*/ - -bool CPtuDPerception::setLimits( - [[maybe_unused]] char axis, [[maybe_unused]] double& l, - [[maybe_unused]] double& u) -{ - cout << "[ERROR] Function not defined for this PTU model"; - return false; -} - -/*------------------------------------------------------------- - changeMotionDir --------------------------------------------------------------*/ - -bool CPtuDPerception::changeMotionDir() -{ - cout << "[ERROR] Function not defined for this PTU model"; - return false; -} - -/*------------------------------------------------------------- - rangeMeasure --------------------------------------------------------------*/ - -bool CPtuDPerception::rangeMeasure() -{ - cout << "[ERROR] Function not defined for this PTU model"; - return false; -} diff --git a/libs/hwdrivers/src/CRoboticHeadInterface.cpp b/libs/hwdrivers/src/CRoboticHeadInterface.cpp deleted file mode 100644 index a16016e390..0000000000 --- a/libs/hwdrivers/src/CRoboticHeadInterface.cpp +++ /dev/null @@ -1,123 +0,0 @@ -/* +------------------------------------------------------------------------+ - | Mobile Robot Programming Toolkit (MRPT) | - | https://www.mrpt.org/ | - | | - | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | - | See: https://www.mrpt.org/Authors - All rights reserved. | - | Released under BSD License. See: https://www.mrpt.org/License | - +------------------------------------------------------------------------+ */ - -#include "hwdrivers-precomp.h" // Precompiled headers -// -#include -#include - -using namespace mrpt::math; -using namespace mrpt::hwdrivers; -using namespace mrpt::serialization; - -/*------------------------------------------------------------- - CRoboticHeadInterface --------------------------------------------------------------*/ -CRoboticHeadInterface::CRoboticHeadInterface() - : mrpt::system::COutputLogger("CRoboticHeadInterface") -{ - MRPT_START - - m_serialNumber = "OREJA001"; - gain.resize(3); - gain[0] = 127; // 0x7F (maximo) - gain[1] = 127; // 0x7F (maximo) - gain[2] = 127; // 0x7F (maximo) - head_yaw = 0; - head_pitch = 0; - - MRPT_END -} - -/*------------------------------------------------------------- - loadConfig_sensorSpecific --------------------------------------------------------------*/ -void CRoboticHeadInterface::loadConfig_sensorSpecific( - const mrpt::config::CConfigFileBase* configSource, - const std::string& iniSection) -{ - configSource->read_vector(iniSection.c_str(), "gain0", gain, gain); - m_serialNumber = configSource->read_string( - iniSection.c_str(), "OREJA_serialNumber", m_serialNumber); - head_yaw = configSource->read_int(iniSection.c_str(), "HeadYaw", head_yaw); - head_pitch = - configSource->read_int(iniSection.c_str(), "HeadPitch", head_pitch); -} - -/*------------------------------------------------------------- - GetGain --------------------------------------------------------------*/ -void CRoboticHeadInterface::GetGain(int& _gain, int& channel) -{ - msg.type = 0x58; - msg.content.resize(1); - msg.content[0] = (unsigned char)channel; - archiveFrom(m_usbConnection).sendMessage(msg); - while (!archiveFrom(m_usbConnection).receiveMessage(msg)) - ; - _gain = msg.content[0]; - if (msg.content[0]) - THROW_EXCEPTION( - "ERROR LEYENDO LA GANANCIA DEL AMPLIFICADOR DE LA OREJA \n"); -} - -/*------------------------------------------------------------- - SetGain --------------------------------------------------------------*/ -bool CRoboticHeadInterface::SetGain(int& new_gain, int& channel) -{ - msg.type = 0x57; - msg.content.resize(2); - msg.content[0] = (unsigned char)channel; - msg.content[1] = (unsigned char)new_gain; - archiveFrom(m_usbConnection).sendMessage(msg); - while (!archiveFrom(m_usbConnection).receiveMessage(msg)) - ; - if (msg.content[0] == 0) return false; - else - return true; -} - -/*------------------------------------------------------------- - GetSoundLocation --------------------------------------------------------------*/ -void CRoboticHeadInterface::GetSoundLocation(int& ang) -{ - msg.type = 0x59; - msg.content.resize(0); - archiveFrom(m_usbConnection).sendMessage(msg); - while (!archiveFrom(m_usbConnection).receiveMessage(msg)) - ; - ang = 256 * (int)msg.content[1] + (int)msg.content[0]; -} - -/*------------------------------------------------------------- - Get3SoundBuffer --------------------------------------------------------------*/ -void CRoboticHeadInterface::Get3SoundBuffer(CMatrixDynamic& buf) -{ - buf.setSize(3, 500); // 3 channel, 500 samples per channel - msg.type = 0x51; - msg.content.resize(0); - archiveFrom(m_usbConnection).sendMessage(msg); - - // Las lecturas se haran de 100 en 100 datos y hay 3 buffers de 500 muestras - // cada uno - for (size_t k = 0; k < 3; k++) // Lectura de cada canal - { - for (size_t j = 0; j < 500 / 100; j++) // Lectura de un canal completo - { - while (!archiveFrom(m_usbConnection).receiveMessage(msg)) - ; - for (size_t i = 0; i < 100; i++) // Lectura de un envio - buf(k, 100 * j + i) = - 256 * (int)msg.content[2 * i + 1] + (int)msg.content[2 * i]; - } - } -} diff --git a/libs/hwdrivers/src/CTuMicos.cpp b/libs/hwdrivers/src/CTuMicos.cpp deleted file mode 100644 index 216324303a..0000000000 --- a/libs/hwdrivers/src/CTuMicos.cpp +++ /dev/null @@ -1,819 +0,0 @@ -/* +------------------------------------------------------------------------+ - | Mobile Robot Programming Toolkit (MRPT) | - | https://www.mrpt.org/ | - | | - | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file | - | See: https://www.mrpt.org/Authors - All rights reserved. | - | Released under BSD License. See: https://www.mrpt.org/License | - +------------------------------------------------------------------------+ */ - -#include "hwdrivers-precomp.h" // Precompiled headers -// -#include -#include -#include - -#include -#include -#include -#include -#include - -using namespace std; -using namespace mrpt; -using namespace mrpt::hwdrivers; -using namespace mrpt::system; - -/*------------------------------------------------------------- - rangeMeasure --------------------------------------------------------------*/ - -bool CTuMicos::rangeMeasure() -{ - char command[50]; - - // char command2[50]; - // sprintf(command2,"%u %s ",axis_index,"nreset"); - - // if (!transmit(command2)) return false; - - sprintf(command, "%u %s ", axis_index, "nrm"); - - if (!transmit(command)) return false; - - return true; -} - -/*------------------------------------------------------------- - moveToAbsPos --------------------------------------------------------------*/ - -bool CTuMicos::moveToAbsPos(char axis, double nRad) -{ - char command[300]; - sprintf(command, "%f %u %s", RAD2DEG(nRad), axis_index, "nm"); - - if (!transmit(command)) return false; - - return true; -} - -/*------------------------------------------------------------- - absPosQ --------------------------------------------------------------*/ - -bool CTuMicos::absPosQ(char axis, double& nRad) -{ - return radQuerry(axis, 'p', nRad); -} - -/*------------------------------------------------------------- - moveToOffPos --------------------------------------------------------------*/ - -bool CTuMicos::moveToOffPos(char axis, double nRad) -{ - char command[300]; - sprintf(command, "%f %u %s", RAD2DEG(nRad), axis_index, "nr"); - - if (!transmit(command)) return false; - - return true; -} - -/*------------------------------------------------------------- - offPosQ --------------------------------------------------------------*/ - -bool CTuMicos::offPosQ(char axis, double& nRad) -{ - return radQuerry(axis, 'p', nRad); -} - -/*------------------------------------------------------------- - maxPosQ --------------------------------------------------------------*/ - -bool CTuMicos::maxPosQ(char axis, double& nRad) -{ - return radQuerry('u', 'l', nRad); // Up limit -} - -/*------------------------------------------------------------- - minPosQ --------------------------------------------------------------*/ - -bool CTuMicos::minPosQ(char axis, double& nRad) -{ - return radQuerry('l', 'l', nRad); // Low limit -} - -/*------------------------------------------------------------- - speed --------------------------------------------------------------*/ - -bool CTuMicos::speed(char axis, double radSec) -{ - return radAsign(axis, 'v', radSec); -} - -/*------------------------------------------------------------- - speedQ --------------------------------------------------------------*/ - -bool CTuMicos::speedQ(char axis, double& radSec) -{ - return radQuerry(axis, 'v', radSec); -} - -/*------------------------------------------------------------- - aceleration --------------------------------------------------------------*/ - -bool CTuMicos::aceleration(char axis, double radSec2) -{ - cout << endl << "[ERROR] Command not defined for this PTunit" << endl; - - return false; -} - -/*------------------------------------------------------------- - acelerationQ --------------------------------------------------------------*/ - -bool CTuMicos::acelerationQ(char axis, double& radSec2) -{ - cout << endl << "[ERROR] Command not defined for this PTunit" << endl; - - return false; -} - -/*------------------------------------------------------------- - baseSpeed --------------------------------------------------------------*/ - -bool CTuMicos::baseSpeed(char axis, double radSec) -{ - cout << endl << "[ERROR] Command not defined for this PTunit" << endl; - - return false; -} - -/*------------------------------------------------------------- - baseSpeedQ --------------------------------------------------------------*/ - -bool CTuMicos::baseSpeedQ(char axis, double& radSec) -{ - cout << endl << "[ERROR] Command not defined for this PTunit" << endl; - - return false; -} - -/*------------------------------------------------------------- - upperSpeed --------------------------------------------------------------*/ - -bool CTuMicos::upperSpeed(char axis, double radSec) -{ - cout << endl << "[ERROR] Command not defined for this PTunit" << endl; - - return false; -} - -/*------------------------------------------------------------- - upperSpeedQ --------------------------------------------------------------*/ - -bool CTuMicos::upperSpeedQ(char axis, double& radSec) -{ - radSec = 26.0_deg; - - return true; -} - -/*------------------------------------------------------------- - lowerSpeed --------------------------------------------------------------*/ - -bool CTuMicos::lowerSpeed(char axis, double radSec) -{ - cout << endl << "[ERROR] Command not defined for this PTunit" << endl; - - return false; -} - -/*------------------------------------------------------------- - lowerSpeedQ --------------------------------------------------------------*/ - -bool CTuMicos::lowerSpeedQ(char axis, double& radSec) -{ - radSec = 1.0_deg; - - return true; -} - -/*------------------------------------------------------------- - enableLimitsQ --------------------------------------------------------------*/ - -bool CTuMicos::enableLimitsQ(bool& enable) -{ - cout << endl << "[ERROR] Command not defined for this PTunit" << endl; - - return false; -} - -/*------------------------------------------------------------- - enableLimits --------------------------------------------------------------*/ - -bool CTuMicos::enableLimits(bool set) -{ - cout << endl << "[ERROR] Command not defined for this PTunit" << endl; - - return false; -} - -/*------------------------------------------------------------- - inmediateExecution --------------------------------------------------------------*/ - -bool CTuMicos::inmediateExecution(bool set) -{ - cout << endl << "[ERROR] Command not defined for this PTunit" << endl; - - return false; -} - -/*------------------------------------------------------------- - aWait --------------------------------------------------------------*/ - -bool CTuMicos::aWait() { return true; } -/*------------------------------------------------------------- - haltAll --------------------------------------------------------------*/ - -bool CTuMicos::haltAll() -{ - cout << endl << "[ERROR] Command not defined for this PTunit" << endl; - - return false; -} - -/*------------------------------------------------------------- - halt --------------------------------------------------------------*/ - -bool CTuMicos::halt(char axis) -{ - char command[50]; - sprintf(command, "%u %s", axis_index, "nabort"); - - if (!transmit(command)) return false; - - return true; -} - -/*------------------------------------------------------------- - reset --------------------------------------------------------------*/ - -bool CTuMicos::reset() -{ - char command[50], command2[50]; - sprintf(command, "%u %s ", axis_index, "nreset"); - - if (!transmit(command)) return false; - - sprintf(command2, "%u %s ", axis_index, "ncal"); - - std::this_thread::sleep_for(1000ms); - - if (!transmit(command2)) return false; - - return true; -} - -/*------------------------------------------------------------- - save --------------------------------------------------------------*/ - -bool CTuMicos::save() -{ - cout << endl << "[ERROR] Command not defined for this PTunit" << endl; - - return false; -} - -/*------------------------------------------------------------- - restoreDefaults --------------------------------------------------------------*/ - -bool CTuMicos::restoreDefaults() -{ - cout << endl << "[ERROR] Command not defined for this PTunit" << endl; - - return false; -} - -/*------------------------------------------------------------- - restoreFactoryDefaults --------------------------------------------------------------*/ - -bool CTuMicos::restoreFactoryDefaults() -{ - cout << endl << "[ERROR] Command not defined for this PTunit" << endl; - - return false; -} - -/*------------------------------------------------------------- - version --------------------------------------------------------------*/ - -bool CTuMicos::version(char* sVersion) -{ - cout << endl << "[ERROR] Command not defined for this PTunit" << endl; - - return false; -} - -/*------------------------------------------------------------- - nversion --------------------------------------------------------------*/ - -void CTuMicos::nversion(double& nVersion) -{ - if (!radQuerry(0, 'n', nVersion)) - throw std::runtime_error("INCORRECT VERSION"); -} - -/*------------------------------------------------------------- - powerModeQ --------------------------------------------------------------*/ - -bool CTuMicos::powerModeQ(bool transit, char& mode) -{ - cout << endl << "[ERROR] Command not defined for this PTunit" << endl; - - return false; -} - -/*------------------------------------------------------------- - powerMode --------------------------------------------------------------*/ - -bool CTuMicos::powerMode(bool transit, char mode) -{ - cout << endl << "[ERROR] Command not defined for this PTunit" << endl; - - return false; -} - -/*------------------------------------------------------------- - clear --------------------------------------------------------------*/ - -bool CTuMicos::clear() -{ - char command[300]; - sprintf(command, "%u %s", axis_index, "nclear"); - - if (!transmit(command)) return false; - - return true; -} - -/*------------------------------------------------------------- - setLimits --------------------------------------------------------------*/ - -bool CTuMicos::setLimits(char axis, double& l, double& u) -{ - char command[300] = ""; - sprintf(command, "%f %f %u setnlimit", l, u, axis_index); - - if (!transmit(command)) return false; - - return true; -} - -/*------------------------------------------------------------- - changeMotionDir --------------------------------------------------------------*/ - -bool CTuMicos::changeMotionDir() -{ - double motionDir; - unsigned int newMotionDir; - char command[300] = ""; - - // Otains actual motion dir - if (!radQuerry(0, 'c', motionDir)) return false; - - if (!motionDir) newMotionDir = 1; - else - newMotionDir = 0; - - // Change motion direction - sprintf(command, "%u %u setmotiondir", newMotionDir, axis_index); - - if (!transmit(command)) return false; - - return true; -} - -/*------------------------------------------------------------- - init --------------------------------------------------------------*/ - -bool CTuMicos::init(const string& port) -{ - try - { - serPort.open(port); - - cout << endl << "[INFO] Start Tu MICOS communication config:" << endl; - - cout << "[PTU::OpenSerialPort] Opening serial port..."; - - if (serPort.isOpen()) { cout << "OK" << endl; } - else - { - cout << " Error opening serial port"; - return false; - } - - cout << "[PTU::SetTimeouts] Setting timeouts..."; - serPort.setTimeouts(1000, 1, 1000, 1, 1000); - cout << "OK" << endl; - - cout << "[PTU::setBaudRate] Setting baud rate..."; - serPort.setConfig(19200); - cout << "OK" << endl; - - // PTU initial configuration - cout << "[PTU::setInitialConfiguration] Setting initial " - "configuration..."; - - axis_index = 1; - double version; - nversion(version); - if ((!version) || (!clear())) - { - cout << " Error setting initial configuration"; - serPort.close(); - return false; - } - - cout << "OK" << endl; - } - catch (const std::exception& e) - { - MRPT_LOG_ERROR_STREAM("Error initializating: " << e.what()); - return false; - } - catch (...) - { - MRPT_LOG_ERROR_STREAM("Error initializating."); - return false; - } - - return true; -} - -/*------------------------------------------------------------- - close --------------------------------------------------------------*/ - -void CTuMicos::close() -{ - // Check if serPort is open - if (serPort.isOpen()) - { - serPort.close(); - cout << endl << "[INFO] TuMICOS Serial port closed" << endl; - } -} - -/*------------------------------------------------------------- - radError --------------------------------------------------------------*/ - -double CTuMicos::radError(char axis, double nRadMoved) -{ - cout << endl << "[ERROR] Command not defined for this PTunit" << endl; - - return false; -} - -/*------------------------------------------------------------- - transmit --------------------------------------------------------------*/ - -bool CTuMicos::transmit(const char* command) -{ - char str[300] = ""; - - // Copy command in str char - strcpy(str, command); - strcat(str, " "); - - // Wirte in serial port - size_t written = serPort.Write(str, strlen(str)); - - if (!written) { return false; } - - return true; -} - -/*------------------------------------------------------------- - receive --------------------------------------------------------------*/ - -bool CTuMicos::receive(const char* command, char* response) -{ - int cnt = 0; - unsigned long nReaden; - char str[150]; //=""; - // char * tmp=""; - - do - { - nReaden = serPort.Read(&str[cnt], 1); - if (nReaden != 0) cnt++; - } while ((nReaden != 0) && (str[cnt - 1] != '\n')); - - if (nReaden == 0) return false; - - // cout << str << endl; - - if (str[0]) - { - strcpy(response, str); - return true; - } - - return false; -} - -/*------------------------------------------------------------- - verboseQ --------------------------------------------------------------*/ - -bool CTuMicos::verboseQ(bool& mode) -{ - cout << endl << "[ERROR] Command not defined for this PTunit" << endl; - - return false; -} - -/*------------------------------------------------------------- - verbose --------------------------------------------------------------*/ - -bool CTuMicos::verbose(bool set) -{ - cout << endl << "[ERROR] Command not defined for this PTunit" << endl; - - return false; -} - -/*------------------------------------------------------------- - echoModeQ --------------------------------------------------------------*/ - -bool CTuMicos::echoModeQ(bool& mode) -{ - cout << endl << "[ERROR] Command not defined for this PTunit" << endl; - - return false; -} - -/*------------------------------------------------------------- - echoMode --------------------------------------------------------------*/ - -bool CTuMicos::echoMode(bool mode) -{ - cout << endl << "[ERROR] Command not defined for this PTunit" << endl; - - return false; -} - -/*------------------------------------------------------------- - resolution --------------------------------------------------------------*/ - -bool CTuMicos::resolution() -{ - cout << endl << "[ERROR] Command not defined for this PTunit" << endl; - - return false; -} - -/*------------------------------------------------------------- - status --------------------------------------------------------------*/ - -double CTuMicos::status(double& rad) { return radQuerry(0, 's', rad); } -/*------------------------------------------------------------- - radQuerry --------------------------------------------------------------*/ - -bool CTuMicos::radQuerry(char axis, char command, double& rad) -{ - char response[150]; - char command2[300]; - bool toRad = true, select = false; - - if (command == 'p') - { // Actual position - sprintf(command2, "%u %s", axis_index, "np"); - } - else if (command == 'v') - { // Velocity for move - sprintf(command2, "%u %s", axis_index, "gnv"); - } - else if (command == 's') - { // Actual status - sprintf(command2, "%u %s", axis_index, "nst"); - toRad = false; - } - else if (command == 'e') - { // Errors - sprintf(command2, "%u %s", axis_index, "gne"); - toRad = false; - } - else if (command == 'l') - { // Limits of the travel - sprintf(command2, "%u %s", axis_index, "getnlimit"); - select = true; - } - else if (command == 'n') - { // Number of version - sprintf(command2, "%u %s", axis_index, "nversion"); - toRad = false; - } - else if (command == 'c') - { // Motion direction - sprintf(command2, "%u %s", axis_index, "getmotiondir"); - toRad = false; - } - - if ((!transmit(command2)) || (!receive(nullptr, response))) return false; - - // If we can convert the result to radians - if (toRad) - { - // If is necesary to select a part of the reponse - if (select) - { - char s2[] = " "; - char *ptr1, *ptr2; - char* strContext; - ptr1 = mrpt::system::strtok(response, s2, &strContext); - ptr2 = mrpt::system::strtok(nullptr, s2, &strContext); - if (axis == 'l') rad = (long)atof((const char*)ptr1); - else - rad = (long)atof((const char*)ptr2); - } - else - { - // Else converts deegres to radians - rad = DEG2RAD((double)atof((const char*)response)); - } - } - else - { - // Else converts char to long - rad = (long)atof((const char*)response); - } - - return true; -} - -/*------------------------------------------------------------- - radAsign --------------------------------------------------------------*/ - -bool CTuMicos::radAsign(char axis, char command, double nRad) -{ - char command2[300]; - - if (command == 'v') - { - sprintf(command2, "%f %u %s", RAD2DEG(nRad), axis_index, "snv"); - } - - return transmit(command2); -} - -/*------------------------------------------------------------- - scan --------------------------------------------------------------*/ - -bool CTuMicos::scan( - char axis, int tWait, float initial, float final, double radPre) -{ - cout << endl << "[ERROR] Command not defined for this PTunit" << endl; - - return false; -} - -/*------------------------------------------------------------- - radToPos --------------------------------------------------------------*/ - -long CTuMicos::radToPos(char axis, double nrad) -{ - cout << endl << "[ERROR] Command not defined for this PTunit" << endl; - - return false; -} - -/*------------------------------------------------------------- - posToRad --------------------------------------------------------------*/ - -double CTuMicos::posToRad(char axis, long nPos) -{ - cout << endl << "[ERROR] Command not defined for this PTunit" << endl; - - return false; -} - -/*------------------------------------------------------------- - convertToLong --------------------------------------------------------------*/ - -long CTuMicos::convertToLong(char* sLong) -{ - long a = (long)atof((const char*)sLong); - - return a; -} - -/*------------------------------------------------------------- - convertToDouble --------------------------------------------------------------*/ - -double CTuMicos::convertToDouble(char* sDouble) -{ - char* result = strpbrk(sDouble, "-0123456789"); - char* stop; - - return strtod(result, &stop); -} - -/*------------------------------------------------------------- - checkError --------------------------------------------------------------*/ - -int CTuMicos::checkErrors() -{ - double code = 0; - - radQuerry(0, 'e', code); - - if ((int)code == 0) { cout << endl << "[No Error]" << endl; } - else - { - switch ((int)code) - { - case 1: - case 2: - case 3: - case 4: cout << endl << "[Error] Internal error" << endl; break; - case 1001: - cout << endl << "[Error] Wrong parameter type" << endl; - break; - case 1002: - cout << endl - << "[Error] Insufficient parameters on the stack" << endl; - break; - case 1003: - cout << endl << "[Error] Value range is exceeded" << endl; - break; - case 1004: - cout << endl - << "[Error] Movement range should be exceeded" << endl; - break; - case 1008: - cout << endl - << "[Error] Insufficient parameters on the stack" << endl; - break; - case 1015: - cout << endl - << "[Error] Parameter out of the movement area" << endl; - break; - case 2000: cout << endl << "[Error] Unknown command" << endl; break; - default: break; - } - } - - return code; -} diff --git a/libs/hwdrivers/src/registerAllClasses.cpp b/libs/hwdrivers/src/registerAllClasses.cpp index 4164c6e91e..4ea2d870b5 100644 --- a/libs/hwdrivers/src/registerAllClasses.cpp +++ b/libs/hwdrivers/src/registerAllClasses.cpp @@ -29,14 +29,12 @@ MRPT_INITIALIZER(registerAllClasses_mrpt_hwdrivers) CHokuyoURG::doRegister(); CRoboPeakLidar::doRegister(); CGPSInterface::doRegister(); - CBoardSonars::doRegister(); CIMUXSens_MT4::doRegister(); CCameraSensor::doRegister(); CWirelessPower::doRegister(); CRaePID::doRegister(); CImpinjRFID::doRegister(); CSickLaserSerial::doRegister(); - CBoardENoses::doRegister(); CEnoseModular::doRegister(); CGillAnemometer::doRegister(); CNTRIPEmitter::doRegister(); diff --git a/parse-files/config.h.in b/parse-files/config.h.in index fb17c7a7f5..fd336a9306 100644 --- a/parse-files/config.h.in +++ b/parse-files/config.h.in @@ -67,9 +67,6 @@ ${CMAKE_MRPT_BUILD_SHARED_LIB} /* do we have support for XBox Kinect? */ #define MRPT_HAS_KINECT ${CMAKE_MRPT_HAS_KINECT} -/** Define for including DUO3D interface through the vendor's proprietary API, which enables the definition of some classes in the MRVL namespace. \sa CDUO3DCamera */ -#define MRPT_HAS_DUO3D ${CMAKE_MRPT_HAS_DUO3D} - /* Kinect SDK variants: */ #define MRPT_HAS_KINECT_FREENECT ${CMAKE_MRPT_HAS_FREENECT} #define MRPT_HAS_KINECT_FREENECT_SYSTEM ${CMAKE_MRPT_HAS_FREENECT_SYSTEM} diff --git a/python/python.conf b/python/python.conf index 7d7545f687..f3fce5ee10 100644 --- a/python/python.conf +++ b/python/python.conf @@ -172,11 +172,7 @@ # # Hardware-related # --class mrpt::hwdrivers::CDUO3DCamera -class mrpt::comms::CInterfaceFTDI --class mrpt::hwdrivers::CBoardSonars --class mrpt::hwdrivers::CServoeNeck --class mrpt::hwdrivers::CRoboticHeadInterface +class mrpt::serialization::CSerializable::Ptr # # Missing includes diff --git a/python/src/pymrpt.sources b/python/src/pymrpt.sources index 69c04e8439..9b4f4e43ce 100644 --- a/python/src/pymrpt.sources +++ b/python/src/pymrpt.sources @@ -157,7 +157,6 @@ mrpt/hwdrivers/C2DRangeFinderAbstract.cpp mrpt/containers/circular_buffer.cpp mrpt/comms/CInterfaceFTDI.cpp mrpt/obs/CObservationGasSensors.cpp -mrpt/hwdrivers/CBoardENoses.cpp mrpt/obs/CObservationRange.cpp mrpt/obs/CObservationCANBusJ1939.cpp mrpt/hwdrivers/CCANBusReader.cpp @@ -168,7 +167,6 @@ mrpt/poses/CPoint.cpp mrpt/poses/CPose3DQuat.cpp mrpt/obs/CObservationStereoImages.cpp mrpt/vision/CStereoRectifyMap.cpp -mrpt/hwdrivers/CDUO3DCamera.cpp mrpt/math/CMatrixDynamic.cpp mrpt/math/CMatrixDynamic_1.cpp mrpt/math/CMatrixFixed.cpp diff --git a/share/mrpt/config_files/rawlog-grabber/camera_duo3d.ini b/share/mrpt/config_files/rawlog-grabber/camera_duo3d.ini deleted file mode 100644 index a77ba2a818..0000000000 --- a/share/mrpt/config_files/rawlog-grabber/camera_duo3d.ini +++ /dev/null @@ -1,103 +0,0 @@ -# ------------------------------------------------------------------- -# Config file for the `rawlog-grabber` MRPT application. -# Usage: -# rawlog-grabber CONFIG_FILE.ini -# -# Each section `[XXXXX]` but `[global]` defines a dedicated thread where a -# sensor-specific driver runs. Each thread collects observations in parallel -# and the main thread sort them by timestamp and dumps them to a RAWLOG file. -# The driver for each thread is set with the field `driver`, which must -# match the name of any of the classes in mrpt::hwdrivers implementing -# a CGenericSensor. -# -# Read more online: -# https://www.mrpt.org/list-of-mrpt-apps/application-rawlog-grabber/ -# ------------------------------------------------------------------- - -# ======================================================= -# Section: Global settings to the application -# ======================================================= -[global] -# The prefix can contain a relative or absolute path. -# The final name will be _date_time.rawlog -rawlog_prefix = duo3d_dataset - -# Milliseconds between thread launches -time_between_launches = 800 - -use_sensoryframes = 0 - -GRABBER_PERIOD_MS = 1000 - -# ======================================================= -# SENSOR: DUO3DCamera -# ======================================================= -[DUO3DCamera] -driver = CCameraSensor -process_rate = 80 // Hz - -sensorLabel = DUO3DCamera - -grabber_type = duo3d -preview_decimation = 5 - -pose_x = 0.5 // position on the robot (meters) -pose_y = 0 -pose_z = 0 -pose_yaw = 0 // Angles in degrees -pose_pitch = 0 -pose_roll = 0 - -# For externaly stored images, the format of image files (default=jpg) -external_images_format = jpg - -# For externaly stored images: whether to spawn an independent process to save the image files. -external_images_own_thread = 1 - -# If external_images_own_thread=1, this changes the number of threads to launch -# to save image files. The default is determined from mrpt::system::getNumberOfProcessors() -# and should be OK unless you want to save processor time for other things. -#external_images_own_thread_count = 2 // >=1 - -# (Only when external_images_format=jpg): Optional parameter to set the JPEG compression quality: -#external_images_jpeg_quality = 95 // [1-100]. Default: 95 - -[DUO3DOptions] -rawlog-grabber-ignore = true // Instructs rawlog-grabber to ignore this section (it is not a separate device!) - -image_width = 640 -image_height = 480 -capture_rectified = true -capture_imu = true -calibration_from_file = false - -exposure = 50 -gain = 50 -led = 0 - -intrinsic_filename = ./duo_calibration/intrinsics_R640x480_B1x1.yml -extrinsic_filename = ./duo_calibration/stereo_R640x480_B1x1.yml -rectify_map_filename = ./duo_calibration/rectifyMap_R640x480_B1x1.yml - -[DUO3D_LEFT] -rawlog-grabber-ignore = true // Instructs rawlog-grabber to ignore this section (it is not a separate device!) -resolution=[640 480] -cx=290.08707 -cy=272.52321 -fx=378.84599 -fy=378.75536 -dist=[-1.246667e-001 -5.436665e-001 -2.376481e-003 -5.422322e-003 -2.217852e+000] - -[DUO3D_RIGHT] -rawlog-grabber-ignore = true // Instructs rawlog-grabber to ignore this section (it is not a separate device!) -resolution=[640 480] -cx=294.13485 -cy=275.04717 -fx=378.84599 -fy=378.75536 -dist=[3.245474e+000 -2.404337e+000 -3.230123e-004 1.050248e-003 -3.428238e+000] - -[DUO3D_LEFT2RIGHT_POSE] -rawlog-grabber-ignore = true // Instructs rawlog-grabber to ignore this section (it is not a separate device!) -pose_quaternion=[0.028050 0.000514 -0.000711 0.999965 -0.005614 0.005452 -0.002796] - diff --git a/share/mrpt/config_files/rawlog-grabber/ptuHokuyo.ini b/share/mrpt/config_files/rawlog-grabber/ptuHokuyo.ini deleted file mode 100644 index 74871c63d8..0000000000 --- a/share/mrpt/config_files/rawlog-grabber/ptuHokuyo.ini +++ /dev/null @@ -1,55 +0,0 @@ -# ------------------------------------------------------------------- -# Config file for the `rawlog-grabber` MRPT application. -# Usage: -# rawlog-grabber CONFIG_FILE.ini -# -# Each section `[XXXXX]` but `[global]` defines a dedicated thread where a -# sensor-specific driver runs. Each thread collects observations in parallel -# and the main thread sort them by timestamp and dumps them to a RAWLOG file. -# The driver for each thread is set with the field `driver`, which must -# match the name of any of the classes in mrpt::hwdrivers implementing -# a CGenericSensor. -# -# Read more online: -# https://www.mrpt.org/list-of-mrpt-apps/application-rawlog-grabber/ -# ------------------------------------------------------------------- - -# ======================================================= -# Section: Global settings to the application -# ======================================================= -[global] -# The prefix can contain a relative or absolute path. -# The final name will be _date_time.rawlog -rawlog_prefix = ./dataset - -# Milliseconds between thread launches -time_between_launches = 300 - -# Maximum time (seconds) between a sequence of observations -# is splitted into sensory frames -SF_max_time_span = 0.005 - -# Whether to use sensory-frames to group close-in-time observations (useful for some SLAM algorithms). -use_sensoryframes = 0 - - -# ======================================================= -# SENSOR: LASER_3D -# -# ======================================================= -[LASER_3D] -driver = CPtuHokuyo -process_rate = 40 // Hz - -sensorLabel = PTUHOKUYO -ptu_type = 0 -velocity = 0.1 -initial_pos = 30 -final_pos = -40 -hokuyo_frec = 0.25 - -COM_ptu_port_WIN = COM1 -COM_ptu_port_LIN = ttyUSB0 - -COM_hokuyo_port_WIN = COM4 -COM_hokuyo_port_LIN = ttyACM0 diff --git a/share/mrpt/config_files/rawlog-grabber/ptuHokuyoCamera.ini b/share/mrpt/config_files/rawlog-grabber/ptuHokuyoCamera.ini deleted file mode 100644 index 6906f8a776..0000000000 --- a/share/mrpt/config_files/rawlog-grabber/ptuHokuyoCamera.ini +++ /dev/null @@ -1,94 +0,0 @@ -# ------------------------------------------------------------------- -# Config file for the `rawlog-grabber` MRPT application. -# Usage: -# rawlog-grabber CONFIG_FILE.ini -# -# Each section `[XXXXX]` but `[global]` defines a dedicated thread where a -# sensor-specific driver runs. Each thread collects observations in parallel -# and the main thread sort them by timestamp and dumps them to a RAWLOG file. -# The driver for each thread is set with the field `driver`, which must -# match the name of any of the classes in mrpt::hwdrivers implementing -# a CGenericSensor. -# -# Read more online: -# https://www.mrpt.org/list-of-mrpt-apps/application-rawlog-grabber/ -# ------------------------------------------------------------------- - -# ======================================================= -# Section: Global settings to the application -# ======================================================= -[global] -# The prefix can contain a relative or absolute path. -# The final name will be _date_time.rawlog -rawlog_prefix = ./dataset - -# Milliseconds between thread launches -time_between_launches = 300 - -# Maximum time (seconds) between a sequence of observations -# is splitted into sensory frames -SF_max_time_span = 0.005 - -# Whether to use sensory-frames to group close-in-time observations (useful for some SLAM algorithms). -use_sensoryframes = 0 - - -# ======================================================= -# SENSOR: LASER_3D -# -# ======================================================= -[LASER_3D] -driver = CPtuHokuyo -process_rate = 40 // Hz - -sensorLabel = PTUHOKUYO -ptu_type = 0 -velocity = 0.1 -initial_pos = 30 -final_pos = -40 -hokuyo_frec = 0.25 -high_ptuHokuyo = 0.12 - -COM_ptu_port_WIN = COM1 -COM_ptu_port_LIN = ttyUSB0 - -COM_hokuyo_port_WIN = COM4 -COM_hokuyo_port_LIN = ttyACM0 - - -# ======================================================= -# SENSOR: CameraOpenCV -# -# ======================================================= -[CameraOpenCV] -driver = CCameraSensor -process_rate = 30 // Hz - -sensorLabel = CAMERA1 -preview_decimation = 5 - -grabber_type = dc1394 // Select one of two grabber implementations. - -# Options for grabber_type= dc1394 -dc1394_camera_guid = 0 // 0 (or not present): the first camera// A hexadecimal number: The GUID of the camera to open -#dc1394_camera_guid = 0x7099112233 // 0 (or not present): the first camera// A hexadecimal number: The GUID of the camera to open -dc1394_camera_unit = 0 // 0 (or not present): the first camera// 0,1,2,...: The unit number (within the given GUID) of the camera to open (Stereo cameras: 0 or 1) -dc1394_frame_width = 1024 -dc1394_frame_height = 768 -dc1394_framerate = 7.5 // eg: 7.5, 15, 30, 60, etc... For possibilities see mrpt::vision::TCaptureOptions_dc1394 -dc1394_color_coding = COLOR_CODING_YUV422 // For possibilities see mrpt::vision::TCaptureOptions_dc1394 -dc1394_shutter = -1 // A value, or -1 (or not present) for not to change this parameter in the camera -dc1394_gain = -1 // A value, or -1 (or not present) for not to change this parameter in the camera -dc1394_gamma = -1 // A value, or -1 (or not present) for not to change this parameter in the camera -dc1394_brightness = -1 // A value, or -1 (or not present) for not to change this parameter in the camera -dc1394_exposure = -1 // A value, or -1 (or not present) for not to change this parameter in the camera -dc1394_sharpness = -1 // A value, or -1 (or not present) for not to change this parameter in the camera -dc1394_white_balance = -1 // A value, or -1 (or not present) for not to change this parameter in the camera - -# Pose of the sensor on the robot: -#pose_x = 0// position on the robot (meters) -#pose_y = 0 -#pose_z = 0.5 -#pose_yaw = 0 // Angles in degrees -#pose_pitch = 0 -#pose_roll = 0 diff --git a/share/mrpt/config_files/rawlog-grabber/tuHokuyo.ini b/share/mrpt/config_files/rawlog-grabber/tuHokuyo.ini deleted file mode 100644 index bf3de2b8b4..0000000000 --- a/share/mrpt/config_files/rawlog-grabber/tuHokuyo.ini +++ /dev/null @@ -1,55 +0,0 @@ -# ------------------------------------------------------------------- -# Config file for the `rawlog-grabber` MRPT application. -# Usage: -# rawlog-grabber CONFIG_FILE.ini -# -# Each section `[XXXXX]` but `[global]` defines a dedicated thread where a -# sensor-specific driver runs. Each thread collects observations in parallel -# and the main thread sort them by timestamp and dumps them to a RAWLOG file. -# The driver for each thread is set with the field `driver`, which must -# match the name of any of the classes in mrpt::hwdrivers implementing -# a CGenericSensor. -# -# Read more online: -# https://www.mrpt.org/list-of-mrpt-apps/application-rawlog-grabber/ -# ------------------------------------------------------------------- - -# ======================================================= -# Section: Global settings to the application -# ======================================================= -[global] -# The prefix can contain a relative or absolute path. -# The final name will be _date_time.rawlog -rawlog_prefix = ./dataset - -# Milliseconds between thread launches -time_between_launches = 300 - -# Maximum time (seconds) between a sequence of observations -# is splitted into sensory frames -SF_max_time_span = 0.005 - -# Whether to use sensory-frames to group close-in-time observations (useful for some SLAM algorithms). -use_sensoryframes = 0 - - -# ======================================================= -# SENSOR: LASER_3D -# -# ======================================================= -[LASER_3D] -driver = CPtuHokuyo -process_rate = 40 // Hz - -sensorLabel = PTUHOKUYO -ptu_type = 1 -velocity = 5 -initial_pos = 10 -final_pos = 50 -hokuyo_frec = 0.25 - -COM_ptu_port_WIN = COM11 -COM_ptu_port_LIN = ttyUSB0 - -COM_hokuyo_port_WIN = COM13 -COM_hokuyo_port_LIN = ttyACM0