00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019 #ifndef ROBOT_STATE_H_
00020 #define ROBOT_STATE_H_
00021
00022 #include <inttypes.h>
00023 #include <vector>
00024 #include <stdlib.h>
00025 #include <string.h>
00026 #include <mutex>
00027 #include <condition_variable>
00028 #include <netinet/in.h>
00029
00030 namespace message_types {
00031 enum message_type {
00032 ROBOT_STATE = 16, ROBOT_MESSAGE = 20, PROGRAM_STATE_MESSAGE = 25
00033 };
00034 }
00035 typedef message_types::message_type messageType;
00036
00037 namespace package_types {
00038 enum package_type {
00039 ROBOT_MODE_DATA = 0,
00040 JOINT_DATA = 1,
00041 TOOL_DATA = 2,
00042 MASTERBOARD_DATA = 3,
00043 CARTESIAN_INFO = 4,
00044 KINEMATICS_INFO = 5,
00045 CONFIGURATION_DATA = 6,
00046 FORCE_MODE_DATA = 7,
00047 ADDITIONAL_INFO = 8,
00048 CALIBRATION_DATA = 9
00049 };
00050 }
00051 typedef package_types::package_type packageType;
00052
00053 namespace robot_message_types {
00054 enum robot_message_type {
00055 ROBOT_MESSAGE_TEXT = 0,
00056 ROBOT_MESSAGE_PROGRAM_LABEL = 1,
00057 PROGRAM_STATE_MESSAGE_VARIABLE_UPDATE = 2,
00058 ROBOT_MESSAGE_VERSION = 3,
00059 ROBOT_MESSAGE_SAFETY_MODE = 5,
00060 ROBOT_MESSAGE_ERROR_CODE = 6,
00061 ROBOT_MESSAGE_KEY = 7,
00062 ROBOT_MESSAGE_REQUEST_VALUE = 9,
00063 ROBOT_MESSAGE_RUNTIME_EXCEPTION = 10
00064 };
00065 }
00066 typedef robot_message_types::robot_message_type robotMessageType;
00067
00068 namespace robot_state_type_v18 {
00069 enum robot_state_type {
00070 ROBOT_RUNNING_MODE = 0,
00071 ROBOT_FREEDRIVE_MODE = 1,
00072 ROBOT_READY_MODE = 2,
00073 ROBOT_INITIALIZING_MODE = 3,
00074 ROBOT_SECURITY_STOPPED_MODE = 4,
00075 ROBOT_EMERGENCY_STOPPED_MODE = 5,
00076 ROBOT_FATAL_ERROR_MODE = 6,
00077 ROBOT_NO_POWER_MODE = 7,
00078 ROBOT_NOT_CONNECTED_MODE = 8,
00079 ROBOT_SHUTDOWN_MODE = 9,
00080 ROBOT_SAFEGUARD_STOP_MODE = 10
00081 };
00082 }
00083 typedef robot_state_type_v18::robot_state_type robotStateTypeV18;
00084 namespace robot_state_type_v30 {
00085 enum robot_state_type {
00086 ROBOT_MODE_DISCONNECTED = 0,
00087 ROBOT_MODE_CONFIRM_SAFETY = 1,
00088 ROBOT_MODE_BOOTING = 2,
00089 ROBOT_MODE_POWER_OFF = 3,
00090 ROBOT_MODE_POWER_ON = 4,
00091 ROBOT_MODE_IDLE = 5,
00092 ROBOT_MODE_BACKDRIVE = 6,
00093 ROBOT_MODE_RUNNING = 7,
00094 ROBOT_MODE_UPDATING_FIRMWARE = 8
00095 };
00096 }
00097
00098 typedef robot_state_type_v30::robot_state_type robotStateTypeV30;
00099
00100 struct version_message {
00101 uint64_t timestamp;
00102 int8_t source;
00103 int8_t robot_message_type;
00104 int8_t project_name_size;
00105 char project_name[15];
00106 uint8_t major_version;
00107 uint8_t minor_version;
00108 int svn_revision;
00109 char build_date[25];
00110 };
00111
00112 struct masterboard_data {
00113 int digitalInputBits;
00114 int digitalOutputBits;
00115 char analogInputRange0;
00116 char analogInputRange1;
00117 double analogInput0;
00118 double analogInput1;
00119 char analogOutputDomain0;
00120 char analogOutputDomain1;
00121 double analogOutput0;
00122 double analogOutput1;
00123 float masterBoardTemperature;
00124 float robotVoltage48V;
00125 float robotCurrent;
00126 float masterIOCurrent;
00127 unsigned char safetyMode;
00128 unsigned char masterOnOffState;
00129 char euromap67InterfaceInstalled;
00130 int euromapInputBits;
00131 int euromapOutputBits;
00132 float euromapVoltage;
00133 float euromapCurrent;
00134 };
00135
00136 struct robot_mode_data {
00137 uint64_t timestamp;
00138 bool isRobotConnected;
00139 bool isRealRobotEnabled;
00140 bool isPowerOnRobot;
00141 bool isEmergencyStopped;
00142 bool isProtectiveStopped;
00143 bool isProgramRunning;
00144 bool isProgramPaused;
00145 unsigned char robotMode;
00146 unsigned char controlMode;
00147 double targetSpeedFraction;
00148 double speedScaling;
00149 };
00150
00151 class RobotState {
00152 private:
00153 version_message version_msg_;
00154 masterboard_data mb_data_;
00155 robot_mode_data robot_mode_;
00156
00157 std::recursive_mutex val_lock_;
00158
00159 std::condition_variable* pMsg_cond_;
00160 bool new_data_available_;
00161 unsigned char robot_mode_running_;
00162
00163 double ntohd(uint64_t nf);
00164
00165 public:
00166 RobotState(std::condition_variable& msg_cond);
00167 ~RobotState();
00168 double getVersion();
00169 double getTime();
00170 std::vector<double> getQTarget();
00171 int getDigitalInputBits();
00172 int getDigitalOutputBits();
00173 char getAnalogInputRange0();
00174 char getAnalogInputRange1();
00175 double getAnalogInput0();
00176 double getAnalogInput1();
00177 char getAnalogOutputDomain0();
00178 char getAnalogOutputDomain1();
00179 double getAnalogOutput0();
00180 double getAnalogOutput1();
00181 std::vector<double> getVActual();
00182 float getMasterBoardTemperature();
00183 float getRobotVoltage48V();
00184 float getRobotCurrent();
00185 float getMasterIOCurrent();
00186 unsigned char getSafetyMode();
00187 unsigned char getInReducedMode();
00188 char getEuromap67InterfaceInstalled();
00189 int getEuromapInputBits();
00190 int getEuromapOutputBits();
00191 float getEuromapVoltage();
00192 float getEuromapCurrent();
00193
00194 bool isRobotConnected();
00195 bool isRealRobotEnabled();
00196 bool isPowerOnRobot();
00197 bool isEmergencyStopped();
00198 bool isProtectiveStopped();
00199 bool isProgramRunning();
00200 bool isProgramPaused();
00201 unsigned char getRobotMode();
00202 bool isReady();
00203
00204 void setDisconnected();
00205
00206 bool getNewDataAvailable();
00207 void finishedReading();
00208
00209 void unpack(uint8_t * buf, unsigned int buf_length);
00210 void unpackRobotMessage(uint8_t * buf, unsigned int offset, uint32_t len);
00211 void unpackRobotMessageVersion(uint8_t * buf, unsigned int offset,
00212 uint32_t len);
00213 void unpackRobotState(uint8_t * buf, unsigned int offset, uint32_t len);
00214 void unpackRobotStateMasterboard(uint8_t * buf, unsigned int offset);
00215 void unpackRobotMode(uint8_t * buf, unsigned int offset);
00216 };
00217
00218 #endif