dynamic_graph_manager.hh

Go to the documentation of this file.

#ifndef DYNAMIC_GRAPH_MANAGER_HH
#define DYNAMIC_GRAPH_MANAGER_HH

// used to join the different processes
#include <unistd.h>
#ifndef __APPLE__
#include <wait.h>
#endif  // __APPLE__

// use of std::bind
#include <functional>

// used to synchronise the control loop
#include <chrono>

// here this is used to use atomic (thread safe objects)
#include <atomic>

// use to protect the add and consumption of the user commands
#include <mutex>

// get the yaml configuration
#include "yaml_cpp_catkin/yaml_cpp_fwd.hpp"

// ROS includes
#include <ros/ros.h>
#include <std_srvs/Empty.h>

// used to spawn the real time thread
#include "real_time_tools/thread.hpp"

// use the realtime spinner to time the loops
#include <real_time_tools/spinner.hpp>

// time measurement
#include "real_time_tools/timer.hpp"

// used to deal with shared memory
#include "shared_memory/locked_condition_variable.hpp"

// import the python interpreter ros binding
#include "dynamic_graph_manager/ros_interpreter.hh"

// some useful tools like the yaml parsing
#include "dynamic_graph_manager/tools.hh"

// the device of the dynamic-graph
#include "dynamic_graph_manager/device.hh"

namespace dynamic_graph
{
typedef std::chrono::steady_clock clock;

class DynamicGraphManager
{
    /*******************
     *  Public methods *
     *******************/
public:
    DynamicGraphManager();

    virtual ~DynamicGraphManager();

    void initialize(YAML::Node param);

    void run();

    void wait_start_dynamic_graph();

    void wait_stop_dynamic_graph();

    void wait_stop_hardware_communication();

    void initialize_dynamic_graph_process();

    void run_python_command(std::ostream& file, const std::string& command);

    void python_prologue();

    virtual void run_dynamic_graph_process();

    virtual void run_hardware_communication_process();

    virtual void run_single_process();

    /***************************
     * method to be overloaded *
     ***************************/

    virtual void initialize_hardware_communication_process()
    {
        throw(
            std::runtime_error(std::string("DynamicGraphManager::") +
                               "initialize_hardware_communication_process(): " +
                               "this method needs to be overloaded"));
    }

    virtual void get_sensors_to_map(VectorDGMap&)
    {
        throw(
            std::runtime_error("DynamicGraphManager::"
                               "get_sensors_to_map(VectorDGMap&): "
                               "this method needs to be overloaded"));
    }

    virtual void set_motor_controls_from_map(const VectorDGMap&)
    {
        throw(std::runtime_error(
            "DynamicGraphManager::"
            "set_motor_controls_from_map(const VectorDGMap&): "
            "this method needs to be overloaded"));
    }

    virtual void compute_safety_controls();

    /************************
     * gettters and setters *
     ************************/

    void stop_dynamic_graph()
    {
        is_dynamic_graph_stopped_ = true;
    }

    void start_dynamic_graph()
    {
        is_dynamic_graph_stopped_ = false;
    }

    bool is_dynamic_graph_stopped()
    {
        return is_dynamic_graph_stopped_;
    }

    void stop_hardware_communication()
    {
        is_hardware_communication_stopped_ = true;
    }

    void start_hardware_communication()
    {
        is_hardware_communication_stopped_ = false;
    }

    bool is_hardware_communication_stopped()
    {
        return is_hardware_communication_stopped_;
    }

    pid_t pid_dynamic_graph_process()
    {
        return pid_dynamic_graph_process_;
    }

    pid_t pid_hardware_communication_process()
    {
        return pid_hardware_communication_process_;
    }

    Device& device()
    {
        if (device_ != nullptr) return *device_;
        throw(
            std::runtime_error("DynamicGraphManager::device():Try"
                               "accessing a device that has not been created"));
    }

    bool has_dynamic_graph_process_died();

    virtual bool is_in_safety_mode()
    {
        bool too_much_missed_control =
            (missed_control_count_ >= max_missed_control_);
        static bool once_tmmc = false;
        if (too_much_missed_control && !once_tmmc)
        {
            rt_printf(
                "HARDWARE: Too much missed control (%d/%d), going in safe "
                "mode\n",
                missed_control_count_,
                max_missed_control_);
            once_tmmc = true;
        }

        bool dg_died = has_dynamic_graph_process_died();
        static bool once_hdgd = false;
        if (dg_died && !once_hdgd)
        {
            rt_printf(
                "HARDWARE: Dynamic Graph process died, going in safe mode\n");
            once_hdgd = true;
        }
        return too_much_missed_control || dg_died;
    }

    static const std::string dg_ros_node_name_;

    static const std::string hw_com_ros_node_name_;

    static const std::string shared_memory_name_;

    static const std::string sensors_map_name_;

    static const std::string motor_controls_map_name_;

    static const std::string cond_var_name_;

protected:
    void add_user_command(std::function<void(void)> func);

private:
    bool start_dynamic_graph(std_srvs::Empty::Request&,
                             std_srvs::Empty::Response&)
    {
        start_dynamic_graph();
        return true;
    }

    bool stop_dynamic_graph(std_srvs::Empty::Request&,
                            std_srvs::Empty::Response&)
    {
        stop_dynamic_graph();
        return true;
    }

    void start_ros_service(ros::NodeHandle& ros_node_handle);

    void* dynamic_graph_real_time_loop();

    static void* dynamic_graph_real_time_loop_helper(void* context)
    {
        return static_cast<DynamicGraphManager*>(context)
            ->dynamic_graph_real_time_loop();
    }

    void* hardware_communication_real_time_loop();

    static void* hardware_communication_real_time_loop_helper(void* context)
    {
        return static_cast<DynamicGraphManager*>(context)
            ->hardware_communication_real_time_loop();
    }

    void* single_process_real_time_loop();

    static void* single_process_real_time_loop_helper(void* context)
    {
        return static_cast<DynamicGraphManager*>(context)
            ->single_process_real_time_loop();
    }

    /***********************
     *  Private attributes *
     ***********************/
protected:
    ros::ServiceServer ros_service_start_dg_;

    ros::ServiceServer ros_service_stop_dg_;

    std::atomic<bool> is_dynamic_graph_stopped_;

    std::atomic<bool> is_hardware_communication_stopped_;

    std::unique_ptr<dynamic_graph::RosPythonInterpreter>
        ros_python_interpreter_;

    std::unique_ptr<real_time_tools::RealTimeThread> thread_dynamic_graph_;

    std::unique_ptr<real_time_tools::RealTimeThread>
        thread_hardware_communication_;

    pid_t pid_dynamic_graph_process_;

    pid_t pid_hardware_communication_process_;

    std::unique_ptr<Device> device_;

    VectorDGMap sensors_map_;

    VectorDGMap motor_controls_map_;

    std::unique_ptr<shared_memory::LockedConditionVariable> cond_var_;

    bool has_been_waken_by_dg_;

    unsigned missed_control_count_;

    unsigned max_missed_control_;

    clock::duration control_period_;

    clock::time_point hw_time_loop_before_sleep_;

    clock::time_point hw_time_loop_after_sleep_;

    clock::duration hw_meas_sleep_time_;

    clock::duration hw_ref_sleep_time_;

    clock::duration hw_meas_active_time_;

    bool is_real_robot_;

    std::string dg_active_timer_file_;

    std::string dg_sleep_timer_file_;

    std::string dg_timer_file_;

    std::string hwc_active_timer_file_;

    std::string hwc_sleep_timer_file_;

    std::string hwc_timer_file_;

    std::string log_dir_;

    std::string python_log_file_;

    std::string app_dir_;

    real_time_tools::Timer dg_active_timer_;

    real_time_tools::Timer dg_sleep_timer_;

    real_time_tools::Timer dg_timer_;

    real_time_tools::Timer hwc_active_timer_;

    real_time_tools::Timer hwc_sleep_timer_;

    real_time_tools::Timer hwc_timer_;

    unsigned memory_buffer_timers_;

    real_time_tools::Spinner hwc_spinner_;

    double hwc_predicted_sleeping_time_;

    double maximum_time_for_user_cmd_;

    std::deque<std::function<void(void)> > user_commands_;

protected:
    std::deque<ros::ServiceServer> ros_user_commands_;

    double control_period_sec_;

    YAML::Node params_;

    std::mutex hwc_mutex_;
};

}  // namespace dynamic_graph

#endif  // DYNAMIC_GRAPH_MANAGER_HH