joystick_controller.h

Go to the documentation of this file.

#pragma once

#include <ros/ros.h>

#include <algorithm>
#include <drive_msgs/drive_param.h>
#include <sensor_msgs/Joy.h>
#include <string>

#include <dynamic_reconfigure/server.h>
#include <teleoperation/joystick_controllerConfig.h>

constexpr const char* PARAMETER_JOYSTICK_TYPE = "joystick_type";

constexpr const char* PARAMETER_JOYSTICK_STEERING_AXIS = "joystick_steering_axis";
constexpr const char* PARAMETER_JOYSTICK_ACCELERATION_AXIS = "joystick_acceleration_axis";
constexpr const char* PARAMETER_JOYSTICK_DECELERATION_AXIS = "joystick_deceleration_axis";
constexpr const char* PARAMETER_JOYSTICK_ENABLE_MANUAL_BTN = "joystick_enable_manual_button";
constexpr const char* PARAMETER_JOYSTICK_ENABLE_AUTONOMOUS_BTN = "joystick_enable_autonomous_button";

constexpr const char* TOPIC_DRIVE_PARAMETERS = "input/drive_param/joystick";
constexpr const char* TOPIC_HEARTBEAT_MANUAL = "/input/heartbeat_manual";
constexpr const char* TOPIC_HEARTBEAT_AUTONOMOUS = "/input/heartbeat_autonomous";

constexpr float EPSILON = 0.001;

class JoystickController
{

    float m_acceleration_scaling_factor = 0.35f;

    float m_deceleration_scaling_factor = 0.35f;

    float m_steering_scaling_factor = 0.8f;

    struct JoystickMapping
    {
        int steeringAxis;
        int accelerationAxis;
        int decelerationAxis;
        int enableManualButton;
        int enableAutonomousButton;
        std::string name;
    };

    const struct JoystickMapping joystick_mapping_ps3 = { 0, 13, 12, 14, 13, "ps3" };
    const struct JoystickMapping joystick_mapping_xbox360 = { 0, 4, 5, 0, 1, "xbox360" };
    const struct JoystickMapping joystick_mapping_xboxone = { 0, 5, 2, 0, 1, "xbone" };
    const struct JoystickMapping joystick_mapping_default = joystick_mapping_xbox360;

    public:
    JoystickController();

    private:
    dynamic_reconfigure::Server<teleoperation::joystick_controllerConfig> m_dyn_cfg_server;

    ros::NodeHandle m_node_handle;
    ros::Publisher m_drive_parameter_publisher;
    ros::Subscriber m_joystick_subscriber;
    ros::Publisher m_enable_manual_publisher;
    ros::Publisher m_enable_autonomous_publisher;

    JoystickMapping m_joystick_map;

    bool m_acceleration_locked;
    bool m_deceleration_locked;

    void joystickCallback(const sensor_msgs::Joy::ConstPtr& joystick);

    void publishDriveParameters(double velocity, double steering_angle);

    void selectJoystickMapping();

    void updateDynamicConfig();
};