CartographerAdapter.cpp

Go to the documentation of this file.

#include "CartographerAdapter.h"
 
// STL
#include <algorithm>
#include <cmath>
#include <cstdlib>
#include <ctime>
#include <filesystem>
#include <functional>
#include <iterator>
#include <memory>
#include <mutex>
#include <optional>
#include <stdexcept>
#include <string>
 
// Eigen
#include <Eigen/Core>
#include <Eigen/Geometry>
 
// Ice
#include <IceUtil/Time.h>
 
// PCL
#include <pcl/common/transforms.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
 
// OpenCV
#include <opencv2/core/mat.hpp>
#include <opencv2/imgcodecs.hpp>
 
// Simox
#include <SimoxUtility/json/json.hpp>
#include <SimoxUtility/math.h>
 
// ArmarX
#include "ArmarXCore/util/CPPUtility/trace.h"
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/util/algorithm.h>
 
// RobotComponents
#include <RobotComponents/libraries/cartographer/interfaces.h>
 
// cartographer (this has to come last!)
#include <cartographer/common/configuration_file_resolver.h>
#include <cartographer/common/time.h>
#include <cartographer/io/file_writer.h>
#include <cartographer/io/points_processor_pipeline_builder.h>
#include <cartographer/io/proto_stream.h>
#include <cartographer/mapping/2d/grid_2d.h>
#include <cartographer/mapping/2d/probability_grid.h>
#include <cartographer/mapping/2d/submap_2d.h>
#include <cartographer/mapping/internal/2d/tsdf_2d.h>
#include <cartographer/mapping/map_builder.h>
#include <cartographer/mapping/map_builder_interface.h>
#include <cartographer/mapping/submaps.h>
#include <cartographer/mapping/trajectory_builder_interface.h>
#include <cartographer/mapping/trajectory_node.h>
#include <cartographer/sensor/point_cloud.h>
#include <cartographer/sensor/rangefinder_point.h>
#include <cartographer/transform/rigid_transform.h>
#include <cartographer/transform/transform.h>
 
// this library
#include "ApproximateTimeQueue.h"
#include "ArVizDrawer.h"
#include "types.h"
#include "grid_conversion.h"
 
#include "util/eigen_conversions.h"
#include "util/pcl_conversions.h"
#include "util/cartographer_utils.h"
#include "util/laser_scanner_conversion.h"
 
namespace carto = ::cartographer;
 
constexpr int SCALE_M_TO_MM = 1000;
 
namespace fs = ::std::filesystem;
 
 
std::string timestamp()
{
    // http://www.cplusplus.com/reference/ctime/strftime/
 
    time_t rawtime{};
    struct tm* timeinfo {};
    std::array < char, 4 + 1 + 2 + 1 + 2 + 1 + 2 + 1 + 2 + 1 > buffer;
 
    time(&rawtime);
    timeinfo = localtime(&rawtime);
 
    strftime(buffer.data(), buffer.size(), "%Y-%m-%d-%H-%M", timeinfo);
 
    std::string timestamp(buffer.data());
    return timestamp;
}
 
 
 
 
namespace armarx::cartographer
{
 
    CartographerAdapter::~CartographerAdapter()
    {
        ARMARX_TRACE;
 
        ARMARX_INFO << "Finishing cartographer";
        mapBuilder->FinishTrajectory(trajectoryId);
 
        ARMARX_INFO << "Releasing cartographer";
        mapBuilder.reset();
 
        ARMARX_INFO << "Done";
    }
 
    CartographerAdapter::CartographerAdapter(
        const std::filesystem::path& mapPath,
        const std::filesystem::path& configPath,
        SlamDataCallable& slamDataCallable,
        const Config& config,
        const std::optional<std::filesystem::path>& mapToLoad,
        const std::optional<Eigen::Affine3f>& map_T_robot_prior)
        : config(config), mapPath(mapPath), slamDataCallable(slamDataCallable)
    {
        const std::string mapBuilderLuaCode = R"text(
          include "map_builder.lua"
          return MAP_BUILDER)text";
 
        const std::string trajectoryBuilderLuaCode = R"text(
          include "trajectory_builder.lua"
          return TRAJECTORY_BUILDER)text";
 
        const auto mapBuilderParameters = cartographer::resolveLuaParameters(mapBuilderLuaCode, configPath);
        const auto mapBuilderOptions =
            carto::mapping::CreateMapBuilderOptions(mapBuilderParameters.get());
 
        const auto trajectoryBuilderParameters =
            cartographer::resolveLuaParameters(trajectoryBuilderLuaCode, configPath);
        trajectoryBuilderOptions =
            carto::mapping::CreateTrajectoryBuilderOptions(trajectoryBuilderParameters.get());
 
        // create map builder
        mapBuilder = carto::mapping::CreateMapBuilder(mapBuilderOptions);
 
        if (config.useLaserScanner)
        {
            sensorSet.insert(sensorIdLaser);
        }
 
        if (config.useOdometry)
        {
            sensorSet.insert(sensorIdOdom);
        }
 
        if (config.useImu)
        {
            sensorSet.insert(sensorIdIMU);
        }
 
        std::vector<std::string> sensorSetNames;
        std::transform(sensorSet.begin(),
                       sensorSet.end(),
                       std::back_inserter(sensorSetNames),
                       [](const auto & sensorId)
        {
            return sensorId.id;
        });
 
        ARMARX_IMPORTANT << "Sensor set '" << sensorSetNames << "' will be used";
        ARMARX_IMPORTANT << "This includes the following laser scanners: "
                         << std::vector<std::string>(config.laserScanners.begin(),
                                 config.laserScanners.end());
 
        std::for_each(config.laserScanners.begin(),
                      config.laserScanners.end(),
                      [&](const auto & laserScannerName)
        {
            const SensorId sensorId{.type = SensorId::SensorType::RANGE,
                                    .id   = laserScannerName};
            laserSensorMap.insert(
            {
                laserScannerName,
                LaserScannerSensor(sensorId, Eigen::Affine3f::Identity())});
        });
 
        if (mapToLoad)
        {
            ARMARX_DEBUG << "Loading map";
            carto::io::ProtoStreamReader reader(mapToLoad->string());
            mapBuilder->LoadState(&reader, true /* load_frozen_state */);
            mapBuilder->pose_graph()->RunFinalOptimization();
 
 
            if (map_T_robot_prior)
            {
                ARMARX_INFO << "Using pose prior " << std::endl << map_T_robot_prior->matrix();
 
                ::cartographer::mapping::proto::InitialTrajectoryPose* initial_trajectory_pose = new ::cartographer::mapping::proto::InitialTrajectoryPose;
 
                initial_trajectory_pose->set_allocated_relative_pose(new ::cartographer::transform::proto::Rigid3d(::cartographer::transform::ToProto(fromEigen(*map_T_robot_prior))));
 
                trajectoryBuilderOptions.set_allocated_initial_trajectory_pose(std::move(initial_trajectory_pose)); // owner -> trajectoryBuilderOptions
            }
 
        }
 
 
        trajectoryId = mapBuilder->AddTrajectoryBuilder(
                           sensorSet, trajectoryBuilderOptions, [ = ](auto&& ...args)
        {
            return getLocalSlamResultCallback(std::forward<decltype(args)>(args)...);
        });
 
        // TODO(fabian.reister): variable sensor set
        const auto dt = static_cast<int64_t>(1'000'000 / config.frequency);
        const float dtHistoryLength = 1'000'000;
 
        ARMARX_IMPORTANT << "Frequency: " << config.frequency << ", dt: " << dt;
 
        approxTimeQueue = std::make_unique<ApproximateTimeQueue>(dt, dtHistoryLength, config.laserScanners, *this);
    }
 
    void CartographerAdapter::onTimedDataAvailable(const TimedData& data)
    {
        pushInOdomData(data.odomPose);
        pushInLaserData(data.laserData);
    }
 
    void CartographerAdapter::pushInOdomData(const armarx::cartographer::PoseStamped& odomPose)
    {
 
 
        const auto& timestamp = odomPose.timestamp;
        ARMARX_DEBUG << "New odom pose received " << timestamp;
 
        auto isOdomSensor = [](const SensorId & sensorId)
        {
            return sensorId.type == SensorId::SensorType::ODOMETRY;
        };
 
        if (not std::any_of(sensorSet.begin(), sensorSet.end(), isOdomSensor))
        {
            ARMARX_DEBUG << deactivateSpam(2.) << "Odometry will not be used";
            return;
        }
 
        ARMARX_DEBUG << "Processing odom pose";
 
        const Eigen::Quaternionf q(odomPose.pose.linear());
        const Eigen::Vector3f p = odomPose.pose.translation();
 
        carto::sensor::OdometryData odomData;
        odomData.pose = carto::transform::Rigid3d::FromArrays(
                            std::array<double, 4> {q.w(), q.x(), q.y(), q.z()},
                            std::array<double, 3> {p.x(), p.y(), p.z()});
        odomData.time = cartographer::toCarto(timestamp);
 
        ARMARX_DEBUG << "Will now insert odom data";
 
        {
            std::lock_guard guard{cartoInsertMtx};
            mapBuilder->GetTrajectoryBuilder(trajectoryId)
            ->AddSensorData(sensorIdOdom.id, odomData);
        }
 
        ARMARX_DEBUG << "Inserted odom data";
    }
 
    void CartographerAdapter::pushInLaserData(const LaserMessages& laserMessages)
    {
 
        carto::sensor::TimedPointCloudData timedPointCloudData;
        timedPointCloudData.origin = Eigen::Vector3f::Zero();
 
        for (const auto& message : laserMessages)
        {
            const auto& data = message.data;
 
            // assumption: all points from single sensor
            const LaserScannerSensor& sensor = lookupSensor(data.frame);
 
            ARMARX_DEBUG << "Sensor " << data.frame << " position is "
                         << sensor.pose.translation();
 
            const Eigen::Affine3f robot_T_sensor = sensor.pose; // [m]
 
            timedPointCloudData.time = cartographer::toCarto(data.timestamp); // universal is in 100ns
            timedPointCloudData.ranges.reserve(timedPointCloudData.ranges.size() +
                                               data.scan.size());
 
            ARMARX_DEBUG << "Laser range data: " << data.scan.size() << " points";
 
            std::transform(data.scan.begin(),
                           data.scan.end(),
                           std::back_inserter(timedPointCloudData.ranges),
                           [&](const auto & scanStep)
                           -> carto::sensor::TimedRangefinderPoint
            {
                auto rangePoint = toCartesian<Eigen::Vector3f>(scanStep); // [mm]
                rangePoint /= 1000;                           // [m]
 
                carto::sensor::TimedRangefinderPoint point;
                // point.position = robot_time_correction_T_sensor* rangePoint;
                point.position = robot_T_sensor* rangePoint;
                point.time     = .0; // relative measurement time (not known)
                return point;
            });
        }
 
        auto cloud = toPCL(timedPointCloudData.ranges);
        slamDataCallable.onLaserSensorData({cartographer::fromCarto(timedPointCloudData.time), cloud});
 
        {
            std::lock_guard guard{cartoInsertMtx};
 
            auto trajBuilder = mapBuilder->GetTrajectoryBuilder(trajectoryId);
            if(not trajBuilder)
            {
                ARMARX_WARNING << "no traj builder";
                return;
            }
 
            trajBuilder->AddSensorData(sensorIdLaser.id, timedPointCloudData);
        }
 
        ARMARX_DEBUG << "Inserted laser data";
    }
 
    void CartographerAdapter::processOdometryPose(OdomData odom)
    {
        armarx::cartographer::PoseStamped odomPose;
 
        odomPose.timestamp = odom.timestamp;
        odomPose.pose.setIdentity();
        odomPose.pose.translation() = odom.position;
        odomPose.pose.linear()      = odom.orientation.toRotationMatrix();
 
        approxTimeQueue->insertOdomData(std::move(odomPose));
    }
 
    void CartographerAdapter::processSensorValues(LaserScannerMessage data)
    {
        ARMARX_DEBUG << "processing sensor values (intro)";
 
 
 
        auto isSensorRegistered = [&](const std::string & sensorId)
        {
            return std::any_of(
                       sensorSet.begin(), sensorSet.end(), [&sensorId](const SensorId & id) -> bool
            {
                return id.id == sensorId;
            });
        };
 
        if (false and not isSensorRegistered(data.frame))
        {
            ARMARX_WARNING << deactivateSpam(1.0) << "Sensor " << data.frame
                           << " not registered";
            return;
        }
 
        approxTimeQueue->insertLaserData(std::move(data));
    }
 
 
 
 
    void CartographerAdapter::getLocalSlamResultCallback(
        const int trajectoryId,
        const ::cartographer::common::Time time,
        const ::cartographer::transform::Rigid3d localPose,
        ::cartographer::sensor::RangeData rangeDataInLocal,
        const std::unique_ptr <
        const ::cartographer::mapping::TrajectoryBuilderInterface::InsertionResult >
        insertion_result)
    {
        // std::lock_guard g{cartoInsertMtx};
 
        ARMARX_DEBUG << "Callback (getLocalSlamResultCallback)";
 
 
//        std::lock_guard g{cartoInsertMtx};
 
        // const auto constraints = mapBuilder->pose_graph()->constraints();
        // ARMARX_DEBUG << "number of contraints: " << constraints.size();
 
        // const size_t nConstraintsInter = std::count_if(
        //                                      constraints.begin(),
        //                                      constraints.end(),
        //                                      [](const ::cartographer::mapping::PoseGraphInterface::Constraint & constraint)
        // {
        //     return constraint.tag ==
        //            ::cartographer::mapping::PoseGraphInterface::Constraint::INTER_SUBMAP;
        // });
 
        // ARMARX_DEBUG << "Inter submap constraints: " << nConstraintsInter;
 
        LocalSlamData localSlamData;
 
        const auto timestampInMicroSeconds = fromCarto(time);
 
        ARMARX_DEBUG << "local slam data timestamp: "
                     << IceUtil::Time::microSeconds(timestampInMicroSeconds);
 
        localSlamData.local_points = toPCL(rangeDataInLocal.returns.points());
        localSlamData.misses       = toPCL(rangeDataInLocal.misses.points());
        localSlamData.local_pose   = toEigen(localPose);
        localSlamData.timestamp    = timestampInMicroSeconds;
        localSlamData.trajectory_pose =
            toEigen(mapBuilder->pose_graph()->GetLocalToGlobalTransform(trajectoryId));
 
        ARMARX_DEBUG << "Callbacks will be triggered (getLocalSlamResultCallback)";
 
        slamDataCallable.onLocalSlamData(localSlamData);
        // triggerCallbacks();
 
        ARMARX_DEBUG << "Callback done (getLocalSlamResultCallback)";
    }
 
    // void CartographerAdapter::triggerCallbacks() const
    // {
    //     // slamDataCallable.onGraphOptimized(optimizedGraphData());
    //     // slamDataCallable.onGridMap(submapData(trajectoryId));
    // }
 
    bool CartographerAdapter::hasReceivedSensorData() const noexcept
    {
        std::lock_guard g{cartoInsertMtx};
 
        const auto trajectoryNodes = mapBuilder->pose_graph()->GetTrajectoryNodes();
 
        auto matchesTrajectoryId = [&](const auto & node) -> bool
        {
            return node.id.trajectory_id == trajectoryId;
        };
 
        // checks if nodes wrt. trajectory_id exist
        return std::any_of(trajectoryNodes.begin(), trajectoryNodes.end(), matchesTrajectoryId);
    }
 
    bool storeDefaultRegistrationFile(const fs::path& jsonFile)
    {
        std::ofstream ofs(jsonFile, std::ios::out);
 
        ARMARX_CHECK(ofs.is_open());
        ARMARX_CHECK(not ofs.fail());
 
        nlohmann::json data;
        data["x"] = 0.F;
        data["y"] = 0.F;
        data["yaw"] = 0.F;
 
        ARMARX_INFO << "Creating dummy registration file `" << jsonFile << "`";
        ofs << data;
 
        return true;
    }
 
    bool CartographerAdapter::createMap()
    {
        const std::string filenameNoExtension = timestamp();
 
        const fs::path mapFilename = mapPath / (filenameNoExtension + ".carto");
        ARMARX_DEBUG << "The concrete map file is " << mapFilename.string();
        assert(!fs::exists(map_filename));
 
        // Create a new trajectory such that new data will not interfer with the map
        // that we are optimizing. Therefore, keep track of the trajectory that we
        // optimize.
        const int trajectoryIdToBeOptimized = trajectoryId;
 
        std::lock_guard g{cartoInsertMtx};
 
        trajectoryId = mapBuilder->AddTrajectoryBuilder(
                           sensorSet, trajectoryBuilderOptions, [&](auto... args)
        {
            return getLocalSlamResultCallback(std::forward<decltype(args)>(args)...);
        });
 
        ARMARX_IMPORTANT << "Optimizing map ...";
        mapBuilder->FinishTrajectory(trajectoryIdToBeOptimized);
        mapBuilder->pose_graph()->RunFinalOptimization();
        ARMARX_IMPORTANT << "... done.";
 
        carto::io::ProtoStreamWriter writer(mapFilename);
        mapBuilder->SerializeState(true, &writer);
        writer.Close();
 
        ARMARX_IMPORTANT << "Saved map to '" << mapFilename << "'";
 
        const fs::path registrationFilename = mapPath / (filenameNoExtension + ".json");
        ARMARX_INFO << "The default registration data will be stored in " << registrationFilename;
        storeDefaultRegistrationFile(registrationFilename);
 
        // slamDataCallable.onGraphOptimized(optimizedGraphData());
        // slamDataCallable.onGridMap(submapData(trajectoryIdToBeOptimized));
 
        return true;
    }
 
    const CartographerAdapter::LaserScannerSensor&
    CartographerAdapter::lookupSensor(const std::string& device) const
    {
        return laserSensorMap.at(device);
    }
 
    void CartographerAdapter::setSensorPose(const std::string& sensorId,
                                            const Eigen::Affine3f& pose)
    {
        laserSensorMap.at(sensorId).pose = pose;
    }
 
    std::vector<std::string> CartographerAdapter::laserSensorIds() const
    {
        return armarx::getMapKeys(laserSensorMap);
    }
 
} // namespace armarx::cartographer