CartographerAdapter.h

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @author     Fabian Reister ( fabian dot reister at kit dot edu )
 * @date       2021
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
#include <cstdint>
#include <filesystem>
#include <memory>
#include <mutex>
#include <optional>
#include <vector>
 
#include <Eigen/Core>
#include <Eigen/Geometry>
 
#include <opencv2/core/mat.hpp>
 
#include <ArmarXCore/core/PackagePath.h>
#include <ArmarXCore/interface/observers/ObserverInterface.h>
#include <ArmarXCore/libraries/DebugObserverHelper/DebugObserverHelper.h>
#include <ArmarXCore/libraries/logging/FrequencyReporter.h>
 
#include <RobotAPI/interface/units/LaserScannerUnit.h>
 
#include <armarx/localization_and_mapping/cartographer_adapter/interfaces.h>
#include <armarx/localization_and_mapping/cartographer_adapter/types.h>
 
#include <cartographer/mapping/trajectory_builder_interface.h>
 
namespace cartographer::mapping
{
    class MapBuilderInterface;
} // namespace cartographer::mapping
 
namespace armarx::localization_and_mapping::cartographer_adapter
{
    struct PoseStamped;
}
 
namespace armarx::localization_and_mapping::cartographer_adapter
{
    class ApproximateTimeQueue;
    struct LaserMessage;
 
    class CartographerAdapter : virtual public MessageCallee
    {
    public:
        struct Config
        {
            bool useOdometry{false};
            bool useLaserScanner{true};
            bool useImu{false};
 
            float frequency{-1.F};
 
            bool useOdometryBasedLaserCorrection{true};
 
            std::set<std::string> laserScanners; // "LaserScannerFront", "LaserScannerBack";
 
            bool enableDebugTiming{false};
 
            float global_localization_min_score = 0.5F;
            float min_score = 0.55F;
        };
 
        CartographerAdapter(
            const std::filesystem::path& mapPath,
            const std::filesystem::path& configPath,
            SlamDataCallable& slamDataCallable,
            const Config& config,
            const std::optional<std::filesystem::path>& mapToLoad = std::nullopt,
            const std::optional<Eigen::Isometry3f>& map_T_robot_prior = std::nullopt,
            const DebugObserverInterfacePrx& debugObserver = nullptr);
 
        virtual ~CartographerAdapter();
 
        void processSensorValues(LaserScannerMessage data);
 
        void onTimedDataAvailable(const TimedData& data) override;
 
        struct OdomData
        {
            EIGEN_MAKE_ALIGNED_OPERATOR_NEW
            Eigen::Vector3f position;
            Eigen::Quaternionf orientation;
 
            //! timestamp in [ms]
            int64_t timestamp;
        };
 
        void processOdometryPose(OdomData odomData);
 
        using SensorId = ::cartographer::mapping::TrajectoryBuilderInterface::SensorId;
 
        class LaserScannerSensor : public LaserScannerSensorBase
        {
        public:
            SensorId id;
 
            [[nodiscard]] auto
            origin() const -> Eigen::Vector3f
            {
                return pose.translation();
            }
 
            explicit LaserScannerSensor(const SensorId& sensorId, Eigen::Isometry3f pose) :
                LaserScannerSensorBase(sensorId.id, pose), id(sensorId)
            {
            }
        };
 
        OptimizedGraphData optimizedGraphData() const;
 
        bool createMap(const armarx::PackagePath& mapOutputPath);
        void setSensorPose(const std::string& sensorId, const Eigen::Isometry3f& pose);
        bool hasReceivedSensorData() const noexcept;
 
        std::vector<std::string> laserSensorIds() const;
 
        // void triggerCallbacks() const;
 
        ::cartographer::mapping::MapBuilderInterface& getMapBuilder();
 
        unsigned int numConstraints() const;
 
    private:
        void pushInOdomData(
            const armarx::localization_and_mapping::cartographer_adapter::PoseStamped& odomPose);
        void pushInLaserData(const std::vector<LaserMessage>& laserMessages);
 
        // laser scanners will be merged
        const SensorId sensorIdLaser{SensorId::SensorType::RANGE, "laser"};
        const SensorId sensorIdIMU{SensorId::SensorType::IMU, "imu"};
        const SensorId sensorIdOdom{SensorId::SensorType::ODOMETRY, "odom"};
 
        const LaserScannerSensor& lookupSensor(const std::string& device) const;
 
        void getLocalSlamResultCallback(
            const int /*trajectory_id*/,
            const ::cartographer::common::Time /*time*/,
            const ::cartographer::transform::Rigid3d localPose,
            ::cartographer::sensor::RangeData rangeDataInLocal,
            const std::unique_ptr<
                const ::cartographer::mapping::TrajectoryBuilderInterface::InsertionResult>
                insertionResult);
 
        std::unique_ptr<::cartographer::mapping::MapBuilderInterface> mapBuilder;
 
        std::map<std::string, int64_t> lastSensorDataTimestamp;
        std::optional<std::int64_t> lastOdomTimestamp;
 
        std::optional<std::int64_t> lastLaserScannerInsertCartoTimestamp;
 
        const Config config;
 
        int trajectoryId{0};
 
        std::filesystem::path mapPath;
 
        std::set<SensorId> sensorSet;
 
        SlamDataCallable& slamDataCallable;
 
        ::cartographer::mapping::proto::TrajectoryBuilderOptions trajectoryBuilderOptions;
 
        mutable std::mutex cartoInsertMtx;
 
        std::unique_ptr<ApproximateTimeQueue> approxTimeQueue;
 
        std::unordered_map<std::string, LaserScannerSensor> laserSensorMap;
 
        std::optional<FrequencyReporter> cartographerInputDataReporterLaserScanner;
        std::optional<FrequencyReporter> cartographerInputDataReporterOdometry;
 
        mutable std::optional<DebugObserverHelper> debugObserverHelper;
        mutable std::mutex debugObserverHelperMtx;
 
        struct DebugObserverScopedTimer
        {
            DebugObserverScopedTimer(const CartographerAdapter* parent, const std::string& name);
 
            ~DebugObserverScopedTimer();
 
            const CartographerAdapter* parent;
            std::string name;
            armarx::DateTime start;
        };
    };
} // namespace armarx::localization_and_mapping::cartographer_adapter