Program Listing for File map_server_node.h

↰ Return to documentation for file (include/mrpt_map_server/map_server_node.h)

/* +------------------------------------------------------------------------+
   |                             mrpt_navigation                            |
   |                                                                        |
   | Copyright (c) 2014-2024, Individual contributors, see commit authors   |
   | See: https://github.com/mrpt-ros-pkg/mrpt_navigation                   |
   | All rights reserved. Released under BSD 3-Clause license. See LICENSE  |
   +------------------------------------------------------------------------+ */

#pragma once

#include <mp2p_icp/metricmap.h>
#include <mrpt/config/CConfigFile.h>
#include <mrpt/maps/CMultiMetricMap.h>

#include "mrpt_msgs/msg/generic_object.hpp"
#include "mrpt_nav_interfaces/srv/get_gridmap_layer.hpp"
#include "mrpt_nav_interfaces/srv/get_layers.hpp"
#include "mrpt_nav_interfaces/srv/get_pointmap_layer.hpp"
#include "nav_msgs/msg/map_meta_data.hpp"
#include "nav_msgs/msg/occupancy_grid.hpp"
#include "nav_msgs/srv/get_map.hpp"
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"

class MapServer : public rclcpp::Node
{
   public:
    MapServer();
    ~MapServer();
    void init();
    void loop();

   private:
    // member variables
    double frequency_ = 1.0;

    double force_republish_period_ = 0;

    // params that come from launch file
    std::string pub_mm_topic_ = "map_server";

    std::string service_map_str_ = "static_map";

    std::string frame_id_ = "map";

    rclcpp::Service<nav_msgs::srv::GetMap>::SharedPtr
        m_service_map;
    nav_msgs::srv::GetMap::Response
        m_response_ros;

    mp2p_icp::metric_map_t theMap_;
    std::mutex theMapMtx_;

    void publish_map();

    // ------ publishers --------

    template <typename msg_t>
    struct PerTopicData
    {
        typename rclcpp::Publisher<msg_t>::SharedPtr pub;
        size_t subscribers = 0;
        double lastPublishTime = 0;

        bool new_subscribers(
            const rclcpp::Time& now, double forceRepublishPeriodSeconds)
        {
            const auto N = pub->get_subscription_count();
            bool ret = N > subscribers;
            subscribers = N;

            if (forceRepublishPeriodSeconds > 0)
            {
                if (now.seconds() - lastPublishTime >
                    forceRepublishPeriodSeconds)
                    ret = true;
            }

            if (ret) lastPublishTime = now.seconds();
            return ret;
        }
    };

    // for the top-level mm metric map:
    PerTopicData<mrpt_msgs::msg::GenericObject> pubMM_;

    // clang-format off
    // Binary form of each layer:
    std::map<std::string, PerTopicData<mrpt_msgs::msg::GenericObject>> pubLayers_;

    // for grid map layers:
    std::map<std::string, PerTopicData<nav_msgs::msg::OccupancyGrid>> pubGridMaps_;
    std::map<std::string, PerTopicData<nav_msgs::msg::MapMetaData>> pubGridMapsMetaData_;
    // for points layers:
    std::map<std::string,PerTopicData<sensor_msgs::msg::PointCloud2>> pubPointMaps_;

    // clang-format on

    sensor_msgs::msg::PointCloud2 pointmap_layer_to_msg(
        const mrpt::maps::CPointsMap::Ptr& pts);

    // Services:
    rclcpp::Service<mrpt_nav_interfaces::srv::GetLayers>::SharedPtr
        srvMapLayers_;

    void srv_map_layers(
        const std::shared_ptr<mrpt_nav_interfaces::srv::GetLayers::Request> req,
        std::shared_ptr<mrpt_nav_interfaces::srv::GetLayers::Response> resp);

    rclcpp::Service<mrpt_nav_interfaces::srv::GetGridmapLayer>::SharedPtr
        srvGetGrid_;

    void srv_get_gridmap(
        const std::shared_ptr<
            mrpt_nav_interfaces::srv::GetGridmapLayer::Request>
            req,
        std::shared_ptr<mrpt_nav_interfaces::srv::GetGridmapLayer::Response>
            resp);

    rclcpp::Service<mrpt_nav_interfaces::srv::GetPointmapLayer>::SharedPtr
        srvGetPoints_;

    void srv_get_pointmap(
        const std::shared_ptr<
            mrpt_nav_interfaces::srv::GetPointmapLayer::Request>
            req,
        std::shared_ptr<mrpt_nav_interfaces::srv::GetPointmapLayer::Response>
            resp);
};