dms_controller.h

Go to the documentation of this file.

#pragma once

#include "drive_mode.h"
#include <ros/ros.h>

#include <std_msgs/Bool.h>
#include <std_msgs/Int32.h>
#include <std_msgs/Time.h>

constexpr const char* PARAMETER_DMS_CHECK_RATE = "dms_check_rate";
constexpr const char* PARAMETER_DMS_EXPIRATION = "dms_expiration";
constexpr const char* PARAMETER_EMERGENCYSTOP_EXPIRATION = "emergencystop_expiration";
constexpr const char* PARAMETER_MODE_OVERRIDE = "mode_override";

constexpr const char* TOPIC_HEARTBEAT_MANUAL = "/input/heartbeat_manual";
constexpr const char* TOPIC_HEARTBEAT_AUTONOMOUS = "/input/heartbeat_autonomous";
constexpr const char* TOPIC_EMERGENCYSTOP = "/input/emergencystop";
constexpr const char* TOPIC_DRIVE_MODE = "/commands/drive_mode";
constexpr const char* TOPIC_COMMAND_EMERGENCYSTOP = "/commands/emergency_stop";

constexpr DriveMode NO_OVERRIDE = DriveMode::LOCKED;

class DMSController
{
    public:
    DMSController();

    void spin();

    private:
    int m_update_frequency;

    DriveMode m_mode_override;

    ros::Duration m_expiration_time;
    ros::Duration m_emergencystop_expiration_time;

    ros::Time m_last_heartbeat_manual;
    ros::Time m_last_heartbeat_autonomous;
    ros::Time m_last_emergencystop;

    ros::NodeHandle m_node_handle;
    ros::Subscriber m_heartbeat_manual_subscriber;
    ros::Subscriber m_heartbeat_autonomous_subscriber;
    ros::Subscriber m_emergencystop_subscriber;
    ros::Publisher m_drive_mode_publisher;
    ros::Publisher m_command_emergencystop_publisher;

    void configureParameters();
    void publishDriveMode();
    void publishCommandEmergencyStop();

    void heartbeatManualCallback(const std_msgs::Time::ConstPtr& message);
    void heartbeatAutonomousCallback(const std_msgs::Time::ConstPtr& message);
    void emergencystopCallback(const std_msgs::Time::ConstPtr& message);
    DriveMode getDriveMode();
    bool getEmergencyStop();
};