00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032 #ifndef SIMPLE_MSG_H
00033 #define SIMPLE_MSG_H
00034
00035 #define ROS_MAX_JOINT 10
00036
00037
00038
00039
00040
00041 typedef struct
00042 {
00043 int length;
00044 } SmPrefix;
00045
00046
00047
00048
00049
00050 typedef enum
00051 {
00052 ROS_MSG_ROBOT_STATUS = 13,
00053 ROS_MSG_JOINT_TRAJ_PT_FULL = 14,
00054 ROS_MSG_JOINT_FEEDBACK = 15,
00055 ROS_MSG_MOTO_MOTION_CTRL = 2001,
00056 ROS_MSG_MOTO_MOTION_REPLY = 2002
00057 } SmMsgType;
00058
00059
00060 typedef enum
00061 {
00062 ROS_COMM_INVALID = 0,
00063 ROS_COMM_TOPIC = 1,
00064 ROS_COMM_SERVICE_RESQUEST = 2,
00065 ROS_COMM_SERVICE_REPLY = 3
00066 } SmCommType;
00067
00068
00069 typedef enum
00070 {
00071 ROS_REPLY_INVALID = 0,
00072 ROS_REPLY_SUCCESS = 1,
00073 ROS_REPLY_FAILURE = 2,
00074 } SmReplyType;
00075
00076
00077 typedef struct
00078 {
00079 SmMsgType msgType;
00080 SmCommType commType;
00081 SmReplyType replyType;
00082 } SmHeader;
00083
00084
00085
00086
00087
00088 typedef struct
00089 {
00090 int drives_powered;
00091 int e_stopped;
00092 int error_code;
00093 int in_error;
00094 int in_motion;
00095 int mode;
00096 int motion_possible;
00097 } SmBodyRobotStatus;
00098
00099
00100 typedef struct
00101 {
00102 int groupNo;
00103 int sequence;
00104 int validFields;
00105 float time;
00106 float pos[ROS_MAX_JOINT];
00107 float vel[ROS_MAX_JOINT];
00108 float acc[ROS_MAX_JOINT];
00109 } SmBodyJointTrajPtFull;
00110
00111
00112 typedef struct
00113 {
00114 int groupNo;
00115 int validFields;
00116 float time;
00117 float pos[ROS_MAX_JOINT];
00118 float vel[ROS_MAX_JOINT];
00119 float acc[ROS_MAX_JOINT];
00120 } SmBodyJointFeedback;
00121
00122
00123 typedef enum
00124 {
00125 ROS_CMD_CHECK_MOTION_READY = 200101,
00126 ROS_CMD_CHECK_QUEUE_CNT = 200102,
00127 ROS_CMD_STOP_MOTION = 200111,
00128 ROS_CMD_START_TRAJ_MODE = 200121,
00129 ROS_CMD_STOP_TRAJ_MODE = 200122,
00130 ROS_CMD_DISCONNECT = 200130
00131 } SmCommandType;
00132
00133
00134 typedef struct
00135 {
00136 int groupNo;
00137 int sequence;
00138 SmCommandType command;
00139 float data[ROS_MAX_JOINT];
00140 } SmBodyMotoMotionCtrl;
00141
00142
00143 typedef enum
00144 {
00145 ROS_RESULT_SUCCESS = 0,
00146 ROS_RESULT_TRUE = 0,
00147 ROS_RESULT_BUSY = 1,
00148 ROS_RESULT_FAILURE = 2,
00149 ROS_RESULT_FALSE = 2,
00150 ROS_RESULT_INVALID = 3,
00151 ROS_RESULT_ALARM = 4,
00152 ROS_RESULT_NOT_READY = 5,
00153 ROS_RESULT_MP_FAILURE = 6
00154 } SmResultType;
00155
00156 typedef enum
00157 {
00158 ROS_RESULT_INVALID_UNSPECIFIED = 3000,
00159 ROS_RESULT_INVALID_MSGSIZE,
00160 ROS_RESULT_INVALID_MSGHEADER,
00161 ROS_RESULT_INVALID_MSGTYPE,
00162 ROS_RESULT_INVALID_GROUPNO,
00163 ROS_RESULT_INVALID_SEQUENCE,
00164 ROS_RESULT_INVALID_COMMAND,
00165 ROS_RESULT_INVALID_DATA = 3010,
00166 ROS_RESULT_INVALID_DATA_START_POS,
00167 ROS_RESULT_INVALID_DATA_POSITION,
00168 ROS_RESULT_INVALID_DATA_SPEED,
00169 ROS_RESULT_INVALID_DATA_ACCEL,
00170 ROS_RESULT_INVALID_DATA_INSUFFICIENT
00171 } SmInvalidSubCode;
00172
00173
00174 typedef enum
00175 {
00176 ROS_RESULT_NOT_READY_UNSPECIFIED = 5000,
00177 ROS_RESULT_NOT_READY_ALARM,
00178 ROS_RESULT_NOT_READY_ERROR,
00179 ROS_RESULT_NOT_READY_ESTOP,
00180 ROS_RESULT_NOT_READY_NOT_PLAY,
00181 ROS_RESULT_NOT_READY_NOT_REMOTE,
00182 ROS_RESULT_NOT_READY_SERVO_OFF,
00183 ROS_RESULT_NOT_READY_HOLD,
00184 ROS_RESULT_NOT_READY_NOT_STARTED,
00185 ROS_RESULT_NOT_READY_WAITING_ROS,
00186 ROS_RESULT_NOT_READY_SKILLSEND
00187 } SmNotReadySubcode;
00188
00189
00190 typedef struct
00191 {
00192 int groupNo;
00193 int sequence;
00194 int command;
00195 SmResultType result;
00196 int subcode;
00197 float data[ROS_MAX_JOINT];
00198 } SmBodyMotoMotionReply;
00199
00200
00201 typedef union
00202 {
00203 SmBodyRobotStatus robotStatus;
00204 SmBodyJointTrajPtFull jointTrajData;
00205 SmBodyJointFeedback jointFeedback;
00206 SmBodyMotoMotionCtrl motionCtrl;
00207 SmBodyMotoMotionReply motionReply;
00208 } SmBody;
00209
00210
00211
00212
00213
00214 struct _SimpleMsg
00215 {
00216 SmPrefix prefix;
00217 SmHeader header;
00218 SmBody body;
00219 } __attribute__((__packed__));
00220 typedef struct _SimpleMsg SimpleMsg;
00221
00222
00223
00224
00225
00226
00227 extern int Ros_SimpleMsg_JointFeedback(CtrlGroup* ctrlGroup, SimpleMsg* sendMsg);
00228
00229 extern int Ros_SimpleMsg_MotionReply(SimpleMsg* receiveMsg, int result, int subcode, SimpleMsg* replyMsg);
00230
00231
00232
00233 #ifdef DEBUG
00234
00235 extern void Ros_SimpleMsg_DumpTrajPtFull(SmBodyJointTrajPtFull* data);
00236 #endif
00237
00238 #endif