component.hpp

Go to the documentation of this file.

//| This file is a part of the CAFER framework developped within
//| the DREAM project (http://www.robotsthatdream.eu/).
//| Copyright 2015, ISIR / Universite Pierre et Marie Curie (UPMC)
//| Main contributor(s):
//|   * Stephane Doncieux, stephane.doncieux@isir.upmc.fr
//|
//|
//| This experiment allows to generate neural networks for simple
//| navigation tasks (obstacle avoidance and maze navigation).
//|
//| This software is governed by the CeCILL license under French law
//| and abiding by the rules of distribution of free software.  You
//| can use, modify and/ or redistribute the software under the terms
//| of the CeCILL license as circulated by CEA, CNRS and INRIA at the
//| following URL "http://www.cecill.info".
//|
//| As a counterpart to the access to the source code and rights to
//| copy, modify and redistribute granted by the license, users are
//| provided only with a limited warranty and the software's author,
//| the holder of the economic rights, and the successive licensors
//| have only limited liability.
//|
//| In this respect, the user's attention is drawn to the risks
//| associated with loading, using, modifying and/or developing or
//| reproducing the software by the user in light of its specific
//| status of free software, that may mean that it is complicated to
//| manipulate, and that also therefore means that it is reserved for
//| developers and experienced professionals having in-depth computer
//| knowledge. Users are therefore encouraged to load and test the
//| software's suitability as regards their requirements in conditions
//| enabling the security of their systems and/or data to be ensured
//| and, more generally, to use and operate it in the same conditions
//| as regards security.
//|
//| The fact that you are presently reading this means that you have
//| had knowledge of the CeCILL license and that you accept its terms.

#ifndef _SFERES_CAFER_HPP
#define _SFERES_CAFER_HPP

#include <unordered_map>
#include <functional>

#include "cafer_core/Management.h"
#include "cafer_core/GetID.h"
#include "cafer_core/aliases.hpp"

#include <ros/ros.h>
#include <ros/spinner.h>
#include <ros/callback_queue.h>


namespace cafer_core {

    void init(int argc, char **argv, std::string node_name);
    // std::string get_node_group(std::string namespace_base, std::string launch_file, double frequency);
    // void release_node_group(std::string namespace_base, std::string gr_namespace);
    // void kill_node_group(std::string namespace_base, std::string gr_namespace);
    // void kill_all_allocated_node_groups(void);

    typedef enum {
        CHG_FREQ = 0,  
        LOCAL_CLIENT_DEATH, 
        COMPLETE_NODE_DEATH, 
        WATCHDOG, 
        ACK_CREATION, 
        ASK_NEW_ACK 
    } MgmtType;

    class ClientDescriptor {
    public:
        std::string ns;
        long int id;
        std::string type;
    };

    bool operator==(ClientDescriptor const&, ClientDescriptor const&);

    struct ClientDescriptorHasher {
        std::size_t operator()(const ClientDescriptor& cd) const
        {

            std::ostringstream oss;
            oss << cd.ns << "_" << cd.id;
            std::hash<std::string> string_hash;

            return string_hash(oss.str());
        }
    };

    using MapWatchDog_t=std::unordered_map<ClientDescriptor, ros::Time, ClientDescriptorHasher>;
    using CreatedNodes_t=std::unordered_map<std::string, std::vector<ClientDescriptor> >;


    class Component {

    public:

        Component(std::string mgmt_topic, std::string _type, double freq = 10, bool new_nodehandle = false);


        ~Component(void)
        {
            //disconnect_from_ros();
        }


        virtual void update() = 0;

        virtual void client_connect_to_ros() = 0;

        virtual void client_disconnect_from_ros() = 0;

        virtual void init() = 0;

        virtual bool is_initialized()
        { return _is_init; }


        bool get_terminate(void) const
        { return terminate; }

        std::string get_type(void) const
        { return type; }


        long int get_id(void) const
        { return id; }

        bool is_connected_to_ros() const
        { return _is_connected_to_ros; }

        void connect_to_ros(void)
        {
            // connection to management topic is done in the constructor only.
            client_connect_to_ros();
            _is_connected_to_ros = true;
        }

        void disconnect_from_ros(void)
        {
            client_disconnect_from_ros();
            management_s.reset();
            watchdog.reset();
            _is_connected_to_ros = false;
            sleep();
        }

        void shutdown(void)
        {
            disconnect_from_ros();
            ros::shutdown();
        }

        std::string call_launch_file(std::string launch_file, std::string namespace_base,
                                     std::string management_topic = "");


        void sleep(void)
        {rate->sleep();}

        void spin(void);

        unsigned int how_many_client_from_type(std::string _type, bool up_only = true);


        void get_connected_client_with_type(std::string _type, std::vector<ClientDescriptor>& vcd, bool up_only = true);

        bool is_it_recent_enough(ros::Time t);

        bool is_client_up(std::string ns, long int id)
        {
            return is_it_recent_enough(get_watchdog(ns, id));
        }


        void wait_for_client(std::string ns, long int id);

        void wait_for_init(void);

        std::string get_namespace(void) const
        {return my_ros_nh->getNamespace();}

        CreatedNodes_t& get_created_nodes(void)
        {return created_nodes;}

        std::vector<ClientDescriptor>& get_created_nodes(std::string created_ns)
        {return created_nodes[created_ns];}

        void kill_created_nodes(void);

        void management_cb(const cafer_core::Management& mgmt);

        void ask_new_ack();

        void ack_creation();

        void update_watchdog(std::string ns, long int id, std::string _type);

        ros::Time get_watchdog(std::string ns, long int id);

        void watchdog_cb(const ros::TimerEvent& event);

        void send_complete_node_death(std::string ns, long int id);

        void send_local_node_death(std::string ns, long int id);

    public:
        MapWatchDog_t map_watchdog;
        CreatedNodes_t created_nodes;
        shared_ptr<NodeHandle> my_ros_nh;
        shared_ptr<ros::CallbackQueue> my_ros_queue;


        shared_ptr<Subscriber> management_s;
        shared_ptr<Publisher> management_p;
    protected :
        int id;
        std::string type;
        int creator_id;
        std::string creator_ns;
        std::string created_ns;
        std::string _mgmt_topic;
        bool terminate;
        shared_ptr<ros::Rate> rate;
        shared_ptr<ros::Timer> watchdog;
        bool _is_init = false;
        bool _is_connected_to_ros = false;

    };

}

#endif