universal_robots_ros_driver/include/ur_modern_driver/robot_state.h

/*
 * robot_state.h
 *
 * Copyright 2015 Thomas Timm Andersen
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#ifndef ROBOT_STATE_H_
#define ROBOT_STATE_H_

#include <condition_variable>
#include <inttypes.h>
#include <mutex>
#include <netinet/in.h>
#include <stdlib.h>
#include <string.h>
#include <vector>

namespace message_types {
enum message_type {
    ROBOT_STATE = 16,
    ROBOT_MESSAGE = 20,
    PROGRAM_STATE_MESSAGE = 25
};
}
typedef message_types::message_type messageType;

namespace package_types {
enum package_type {
    ROBOT_MODE_DATA = 0,
    JOINT_DATA = 1,
    TOOL_DATA = 2,
    MASTERBOARD_DATA = 3,
    CARTESIAN_INFO = 4,
    KINEMATICS_INFO = 5,
    CONFIGURATION_DATA = 6,
    FORCE_MODE_DATA = 7,
    ADDITIONAL_INFO = 8,
    CALIBRATION_DATA = 9
};
}
typedef package_types::package_type packageType;

namespace robot_message_types {
enum robot_message_type {
    ROBOT_MESSAGE_TEXT = 0,
    ROBOT_MESSAGE_PROGRAM_LABEL = 1,
    PROGRAM_STATE_MESSAGE_VARIABLE_UPDATE = 2,
    ROBOT_MESSAGE_VERSION = 3,
    ROBOT_MESSAGE_SAFETY_MODE = 5,
    ROBOT_MESSAGE_ERROR_CODE = 6,
    ROBOT_MESSAGE_KEY = 7,
    ROBOT_MESSAGE_REQUEST_VALUE = 9,
    ROBOT_MESSAGE_RUNTIME_EXCEPTION = 10
};
}
typedef robot_message_types::robot_message_type robotMessageType;

namespace robot_state_type_v18 {
enum robot_state_type {
    ROBOT_RUNNING_MODE = 0,
    ROBOT_FREEDRIVE_MODE = 1,
    ROBOT_READY_MODE = 2,
    ROBOT_INITIALIZING_MODE = 3,
    ROBOT_SECURITY_STOPPED_MODE = 4,
    ROBOT_EMERGENCY_STOPPED_MODE = 5,
    ROBOT_FATAL_ERROR_MODE = 6,
    ROBOT_NO_POWER_MODE = 7,
    ROBOT_NOT_CONNECTED_MODE = 8,
    ROBOT_SHUTDOWN_MODE = 9,
    ROBOT_SAFEGUARD_STOP_MODE = 10
};
}
typedef robot_state_type_v18::robot_state_type robotStateTypeV18;
namespace robot_state_type_v30 {
enum robot_state_type {
    ROBOT_MODE_DISCONNECTED = 0,
    ROBOT_MODE_CONFIRM_SAFETY = 1,
    ROBOT_MODE_BOOTING = 2,
    ROBOT_MODE_POWER_OFF = 3,
    ROBOT_MODE_POWER_ON = 4,
    ROBOT_MODE_IDLE = 5,
    ROBOT_MODE_BACKDRIVE = 6,
    ROBOT_MODE_RUNNING = 7,
    ROBOT_MODE_UPDATING_FIRMWARE = 8
};
}

typedef robot_state_type_v30::robot_state_type robotStateTypeV30;

struct version_message {
    uint64_t timestamp;
    int8_t source;
    int8_t robot_message_type;
    int8_t project_name_size;
    char project_name[15];
    uint8_t major_version;
    uint8_t minor_version;
    int svn_revision;
    char build_date[25];
};

struct masterboard_data {
    int digitalInputBits;
    int digitalOutputBits;
    char analogInputRange0;
    char analogInputRange1;
    double analogInput0;
    double analogInput1;
    char analogOutputDomain0;
    char analogOutputDomain1;
    double analogOutput0;
    double analogOutput1;
    float masterBoardTemperature;
    float robotVoltage48V;
    float robotCurrent;
    float masterIOCurrent;
    unsigned char safetyMode;
    unsigned char masterOnOffState;
    char euromap67InterfaceInstalled;
    int euromapInputBits;
    int euromapOutputBits;
    float euromapVoltage;
    float euromapCurrent;
};

struct robot_mode_data {
    uint64_t timestamp;
    bool isRobotConnected;
    bool isRealRobotEnabled;
    bool isPowerOnRobot;
    bool isEmergencyStopped;
    bool isProtectiveStopped;
    bool isProgramRunning;
    bool isProgramPaused;
    unsigned char robotMode;
    unsigned char controlMode;
    double targetSpeedFraction;
    double speedScaling;
};

class RobotState {
private:
    version_message version_msg_;
    masterboard_data mb_data_;
    robot_mode_data robot_mode_;

    std::recursive_mutex val_lock_; // Locks the variables while unpack parses data;

    std::condition_variable* pMsg_cond_; //Signals that new vars are available
    bool new_data_available_; //to avoid spurious wakes
    unsigned char robot_mode_running_;

    double ntohd(uint64_t nf);

public:
    RobotState(std::condition_variable& msg_cond);
    ~RobotState();
    double getVersion();
    double getTime();
    std::vector<double> getQTarget();
    int getDigitalInputBits();
    int getDigitalOutputBits();
    char getAnalogInputRange0();
    char getAnalogInputRange1();
    double getAnalogInput0();
    double getAnalogInput1();
    char getAnalogOutputDomain0();
    char getAnalogOutputDomain1();
    double getAnalogOutput0();
    double getAnalogOutput1();
    std::vector<double> getVActual();
    float getMasterBoardTemperature();
    float getRobotVoltage48V();
    float getRobotCurrent();
    float getMasterIOCurrent();
    unsigned char getSafetyMode();
    unsigned char getInReducedMode();
    char getEuromap67InterfaceInstalled();
    int getEuromapInputBits();
    int getEuromapOutputBits();
    float getEuromapVoltage();
    float getEuromapCurrent();

    bool isRobotConnected();
    bool isRealRobotEnabled();
    bool isPowerOnRobot();
    bool isEmergencyStopped();
    bool isProtectiveStopped();
    bool isProgramRunning();
    bool isProgramPaused();
    unsigned char getRobotMode();
    bool isReady();

    void setDisconnected();

    bool getNewDataAvailable();
    void finishedReading();

    void unpack(uint8_t* buf, unsigned int buf_length);
    void unpackRobotMessage(uint8_t* buf, unsigned int offset, uint32_t len);
    void unpackRobotMessageVersion(uint8_t* buf, unsigned int offset,
        uint32_t len);
    void unpackRobotState(uint8_t* buf, unsigned int offset, uint32_t len);
    void unpackRobotStateMasterboard(uint8_t* buf, unsigned int offset);
    void unpackRobotMode(uint8_t* buf, unsigned int offset);
};

#endif /* ROBOT_STATE_H_ */