Map2D_nws_ros2.h

Go to the documentation of this file.

/*
 * SPDX-FileCopyrightText: 2023 Istituto Italiano di Tecnologia (IIT)
 * SPDX-License-Identifier: BSD-3-Clause
 */
 
#ifndef YARP_DEV_MAP2D_NWS_ROS2_H
#define YARP_DEV_MAP2D_NWS_ROS2_H
 
#include <vector>
#include <iostream>
#include <string>
#include <sstream>
#include <mutex>
 
#include <yarp/os/Network.h>
#include <yarp/os/Port.h>
#include <yarp/os/Bottle.h>
#include <yarp/os/Time.h>
#include <yarp/os/Property.h>
 
#include <yarp/os/PeriodicThread.h>
#include <yarp/os/Thread.h>
#include <yarp/os/BufferedPort.h>
#include <yarp/os/RpcServer.h>
#include <yarp/sig/Vector.h>
#include <yarp/dev/IMap2D.h>
#include <yarp/dev/MapGrid2D.h>
#include <yarp/dev/Map2DLocation.h>
#include <yarp/dev/Map2DArea.h>
#include <yarp/dev/Map2DPath.h>
#include <yarp/dev/WrapperSingle.h>
#include <yarp/os/ResourceFinder.h>
 
#include <yarp/dev/PolyDriver.h>
#include <yarp/dev/DeviceDriver.h>
#include <yarp/dev/api.h>
#include "Map2D_nws_ros2_ParamsParser.h"
 
#include <rclcpp/rclcpp.hpp>
 
#include <nav_msgs/msg/occupancy_grid.hpp>
#include <nav_msgs/srv/get_map.hpp>
#include <test_msgs/srv/basic_types.hpp>
#include <visualization_msgs/msg/marker_array.hpp>
#include <visualization_msgs/msg/marker.hpp>
 
//Custom ros2 interfaces
#include <map2d_nws_ros2_msgs/srv/get_map_by_name.hpp>
 
 
class Map2D_nws_ros2 :
        public yarp::os::Thread,
        public yarp::dev::DeviceDriver,
        public yarp::os::PortReader,
        public yarp::dev::WrapperSingle,
        Map2D_nws_ros2_ParamsParser
{
public:
    Map2D_nws_ros2();
    ~Map2D_nws_ros2() override = default;
 
 
    //IMultipleWrapper
    bool attach(yarp::dev::PolyDriver* driver) override;
    bool detach() override;
 
    // DeviceDriver
    bool open(yarp::os::Searchable &config) override;
    bool close() override;
    bool read(yarp::os::ConnectionReader& connection) override;
 
    //Other stuff
    void getMapCallback(const std::shared_ptr<rmw_request_id_t> request_header,
                        const std::shared_ptr<nav_msgs::srv::GetMap::Request> request,
                        std::shared_ptr<nav_msgs::srv::GetMap::Response> response);
    void getMapByNameCallback(const std::shared_ptr<rmw_request_id_t> request_header,
                              const std::shared_ptr<map2d_nws_ros2_msgs::srv::GetMapByName::Request> request,
                              std::shared_ptr<map2d_nws_ros2_msgs::srv::GetMapByName::Response> response);
    void rosCmdParserCallback(const std::shared_ptr<rmw_request_id_t> request_header,
                         const std::shared_ptr<test_msgs::srv::BasicTypes::Request> request,
                         std::shared_ptr<test_msgs::srv::BasicTypes::Response> response);
    bool updateVizMarkers();
 
    void run() override;
 
private:
    //drivers and interfaces
    yarp::dev::Nav2D::IMap2D*    m_iMap2D = nullptr;
    yarp::dev::PolyDriver        m_drv;
 
    std::mutex                   m_mutex;
    std::string                  m_rpcPortName;
    std::string                  m_currentMapName{"none"};
    bool                         m_spinned{false};
 
    yarp::os::RpcServer                                                    m_rpcPort;
    rclcpp::Node::SharedPtr                                                m_node;
    rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr     m_ros2Publisher_markers{nullptr};
    rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr             m_ros2Publisher_map{nullptr};
    rclcpp::Service<nav_msgs::srv::GetMap>::SharedPtr                      m_ros2Service_getMap{nullptr};
    rclcpp::Service<map2d_nws_ros2_msgs::srv::GetMapByName>::SharedPtr     m_ros2Service_getMapByName{nullptr};
    rclcpp::Service<test_msgs::srv::BasicTypes>::SharedPtr                 m_ros2Service_rosCmdParser{nullptr};
};
 
 
#endif  //YARP_DEV_MAP2D_NWS_ROS2_H