vesc_sim_driver.h

Go to the documentation of this file.

#pragma once

#include <ros/ros.h>

#include <drive_msgs/drive_param.h>
#include <std_msgs/Float64.h>

#include "vesc_sim.h"

constexpr const char* COMMAND_POSITION = "/commands/servo/position";
constexpr const char* COMMAND_THROTTLE = "/commands/motor/speed";
constexpr const char* COMMAND_BRAKE = "/commands/motor/brake";
constexpr const char* TOPIC_DRIVE_PARAM = "/commands/drive_param";
constexpr const char* CMD_VEL = "cmd_vel";

namespace simulation
{
    constexpr const char* WHEEL_LEFT_BACK_VELOCITY = "/racer/left_wheel_back_velocity_controller/command";
    constexpr const char* WHEEL_LEFT_FRONT_VELOCITY = "/racer/left_wheel_front_velocity_controller/command";
    constexpr const char* WHEEL_RIGHT_BACK_VELOCITY = "/racer/right_wheel_back_velocity_controller/command";
    constexpr const char* WHEEL_RIGHT_FRONT_VELOCITY = "/racer/right_wheel_front_velocity_controller/command";
    constexpr const char* LEFT_STEERING_POSITION = "/racer/left_steering_hinge_position_controller/command";
    constexpr const char* RIGHT_STEERING_POSITION = "/racer/right_steering_hinge_position_controller/command";
};

class VESCSimulationDriver
{
    public:
    VESCSimulationDriver();

    void motorSpeedCallback(const std_msgs::Float64::ConstPtr& throttle_message);

    void servoPositionCallback(const std_msgs::Float64::ConstPtr& servo_position);

    void motorBrakeCallback(const std_msgs::Float64::ConstPtr& motor_brake);

    private:
    struct AckermannSteeringAngles
    {
        double left_wheel_angle;
        double right_wheel_angle;
    };

    AckermannSteeringAngles calculateSteeringAngles(const double& angle);

    ros::NodeHandle m_node_handle;

    ros::Subscriber m_servo_position_subscriber;

    ros::Subscriber m_motor_speed_subscriber;

    ros::Subscriber m_motor_brake_subscriber;

    ros::Publisher m_left_rear_wheel_velocity_publisher;

    ros::Publisher m_right_rear_wheel_velocity_publisher;
    ros::Publisher m_left_front_wheel_velocity_publisher;

    ros::Publisher m_right_front_wheel_velocity_publisher;

    ros::Publisher m_left_steering_position_publisher;

    ros::Publisher m_right_steering_position_publisher;

    VESCSimulator m_VESC_simulator;
};