roboteq.h

Go to the documentation of this file.

#ifndef ROBOTEQ_H
#define ROBOTEQ_H

#include <ros/ros.h>
#include <serial/serial.h>

#include <std_msgs/Bool.h>
#include <roboteq_control/Service.h>
#include <roboteq_control/Peripheral.h>

#include <diagnostic_updater/diagnostic_updater.h>
#include <diagnostic_updater/publisher.h>
#include <hardware_interface/robot_hw.h>

#include <roboteq_control/RoboteqControllerConfig.h>

#include "configurator/gpio_analog.h"
#include "configurator/gpio_pulse.h"
#include "configurator/gpio_encoder.h"
#include "roboteq/serial_controller.h"
#include "roboteq/motor.h"

using namespace std;

namespace roboteq
{

typedef struct joint
{
    Motor *motor;
    // State of the motor
    double position;
    double velocity;
    double effort;
    double velocity_command;
} joint_t;

typedef struct _status_flag {
    uint8_t serial_mode : 1;
    uint8_t pulse_mode : 1;
    uint8_t analog_mode : 1;
    uint8_t spectrum : 1;
    uint8_t power_stage_off : 1;
    uint8_t stall_detect : 1;
    uint8_t at_limit : 1;
    uint8_t microbasic_running : 1;
} status_flag_t;
// Reference in pag 245
typedef struct _status_fault {
    uint8_t overheat : 1;
    uint8_t overvoltage : 1;
    uint8_t undervoltage : 1;
    uint8_t short_circuit : 1;
    uint8_t emergency_stop : 1;
    uint8_t brushless_sensor_fault : 1;
    uint8_t mosfet_failure : 1;
} status_fault_t;

class Roboteq : public hardware_interface::RobotHW, public diagnostic_updater::DiagnosticTask
{
public:
    Roboteq(const ros::NodeHandle &nh, const ros::NodeHandle &private_nh, serial_controller *serial);
    ~Roboteq();
    void run(diagnostic_updater::DiagnosticStatusWrapper &stat);
    void initialize();
    void initializeInterfaces(hardware_interface::JointStateInterface &joint_state_interface,
                              hardware_interface::VelocityJointInterface &velocity_joint_interface);
    void updateDiagnostics();

    void initializeDiagnostic();

    void write(const ros::Time& time, const ros::Duration& period);

    void read(const ros::Time& time, const ros::Duration& period);

    bool prepareSwitch(const std::list<hardware_interface::ControllerInfo>& start_list, const std::list<hardware_interface::ControllerInfo>& stop_list);

    void doSwitch(const std::list<hardware_interface::ControllerInfo>& start_list, const std::list<hardware_interface::ControllerInfo>& stop_list);

private:
    //Initialization object
    //NameSpace for bridge controller
    ros::NodeHandle mNh;
    ros::NodeHandle private_mNh;
    // Serial controller
    serial_controller *mSerial;
    // Diagnostic
    diagnostic_updater::Updater diagnostic_updater;
    // Publisher status periheral
    ros::Publisher pub_peripheral;
    // stop publisher
    ros::Subscriber sub_stop;
    // Service board
    ros::ServiceServer srv_board;

    urdf::Model model;

    //hardware_interface::JointStateInterface joint_state_interface;
    //hardware_interface::VelocityJointInterface velocity_joint_interface;

    // Check if is the first run
    bool _first;
    // Motor definition
    //map<string, Motor*> mMotor;
    std::vector<Motor*> mMotor;

    string _type, _model;
    string _version;
    string _uid;
    // Status Roboteq board
    status_flag_t _flag;
    // Fault flags Roboteq board
    status_fault_t _fault;
    // Volts internal
    double _volts_internal, _volts_five;
    // Tempearture inside the Roboteq board
    double _temp_mcu, _temp_bridge;

    bool first_read_ = true;
    double first_read_pos_[2];

    // GPIO enable read
    bool _isGPIOreading;
    roboteq_control::Peripheral msg_peripheral;
    std::vector<GPIOAnalogConfigurator*> _param_analog;
    std::vector<GPIOPulseConfigurator*> _param_pulse;
    // Encoder
    std::vector<GPIOEncoderConfigurator*> _param_encoder;


    // stop callback
    void stop_Callback(const std_msgs::Bool::ConstPtr& msg);
    void getRoboteqInformation();

    bool setup_controller;

    dynamic_reconfigure::Server<roboteq_control::RoboteqControllerConfig> *ds_controller;
    void reconfigureCBController(roboteq_control::RoboteqControllerConfig &config, uint32_t level);

    // Default parameter config
    roboteq_control::RoboteqControllerConfig default_controller_config, _last_controller_config;

    void getControllerFromRoboteq();

    bool service_Callback(roboteq_control::Service::Request &req, roboteq_control::Service::Response &msg_system);
    void connectionCallback(const ros::SingleSubscriberPublisher& pub);

};

}


#endif // ROBOTEQ_H