123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299 |
- package at.acdp.urweb.sclient;
- import at.acdp.urweb.CountDataInputStream;
- import at.acdp.urweb.sclient.data.*;
- import org.slf4j.Logger;
- import org.slf4j.LoggerFactory;
- import java.io.*;
- import java.net.Socket;
- import java.nio.charset.StandardCharsets;
- public class SecondaryClient {
- private static final Logger log = LoggerFactory.getLogger(SecondaryClient.class);
- private final String ip;
- private final int port;
- private volatile boolean _running = true;
- private Socket rt;
- private OutputStream os;
- private CountDataInputStream in;
- private MasterBoardData lastMB=new MasterBoardData();
- public SecondaryClient(String ip, int port) {
- this.ip = ip;
- this.port = port;
- }
- public VersionMessage connect() throws IOException {
- this.rt = new Socket(ip, port);
- this.os = rt.getOutputStream();
- this.in = new CountDataInputStream(rt.getInputStream());
- VersionMessage vm = new VersionMessage();
- vm.read(in, -1);
- return vm;
- }
- public MasterBoardData getLastMB() {
- return lastMB;
- }
- public int readReply(CountDataInputStream di) throws IOException {
- int beforeCount=di.getCount();
- int size = di.readInt(); //4
- int pType = di.readByte() & 0xff; //+1=5
- switch (pType) {
- case MessageType.ROBOT_MESSAGE:
- readRobotMessage(di, size);
- break;
- case MessageType.ROBOT_STATE:
- readRobotState(di, size);
- break;
- case MessageType.PROGRAM_STATE_MESSAGE:
- var psm=new ProgramStateMessage();
- psm.read(di, size);
- URLog.add(psm);
- break;
- case MessageType.MODBUS_INFO_MESSAGE:
- var m=new ModbusInfoMessage();
- m.read(di, size);
- URLog.add(m);
- break;
- default:
- log.warn("unknown ptype: "+pType+", size: "+size);
- byte[] pack=new byte[size-5];
- di.readFully(pack);
- }
- int afterCount=di.getCount();
- int diff=afterCount-beforeCount-size;
- if(diff!=0) {
- log.warn("size mismatch: " +diff + "package type: "+pType);
- }
- return size;
- }
- public int readSub(CountDataInputStream di) throws IOException {
- int beforeCount=di.getCount();
- int size = di.readInt(); //4
- int pType = di.readByte() & 0xff; //+1=5
- switch (pType) {
- case PackageType.ROBOT_MODE_DATA:
- var md=readRobotModeData(di, size);
- URLog.add(md);
- log.debug(md.toString());
- break;
- case PackageType.JOINT_DATA:
- var jd=readJointData(di, size);
- URLog.add(jd);
- log.debug(jd.toString());
- break;
- case PackageType.TOOL_DATA:
- var td=readToolData(di, size);
- URLog.add(td);
- log.debug(td.toString());
- break;
- case PackageType.MASTERBOARD_DATA:
- var mb=readMasterBoardData(di, size);
- URLog.add(mb);
- log.debug(mb.toString());
- break;
- case PackageType.CARTESIAN_INFO:
- var c=readCartesianInfo(di, size);
- URLog.add(c);
- log.debug(c.toString());
- break;
- case PackageType.KINEMATICS_INFO:
- var ki=readKinemsticsInfo(di, size);
- URLog.add(ki);
- log.debug(ki.toString());
- break;
- case PackageType.CONFIGURATION_DATA:
- var cd=readConfigurationData(di, size);
- URLog.add(cd);
- log.debug(cd.toString());
- break;
- case PackageType.FORCE_MODE_DATA:
- var fmd=readForceModeData(di, size);
- URLog.add(fmd);
- log.debug(fmd.toString());
- break;
- case PackageType.ADDITIONAL_INFO:
- var ai=readAdditionalInfo(di, size);
- URLog.add(ai);
- log.debug(ai.toString());
- break;
- case PackageType.NEEDED_FOR_CALIB_DATA:
- skip(PackageType.NEEDED_FOR_CALIB_DATA,di, size);
- break;
- case PackageType.SAFETY_DATA:
- skip(PackageType.SAFETY_DATA,di, size);
- break;
- case PackageType.TOOL_COMM_INFO:
- var tci=readToolCommInfo(di, size);
- URLog.add(tci);
- log.debug(tci.toString());
- break;
- default:
- log.warn("unknown ptype: "+pType+", size: "+size);
- byte[] pack=new byte[size-5];
- di.readFully(pack);
- }
- int afterCount=di.getCount();
- int diff=afterCount-beforeCount-size;
- if(diff!=0) {
- log.warn("size mismatch: " +diff + "package type: "+pType);
- }
- return size;
- }
- private MasterBoardData readMasterBoardData(CountDataInputStream di, int size) throws IOException {
- MasterBoardData mb=new MasterBoardData();
- mb.read(di, size);
- lastMB=mb;
- return mb;
- }
- private ToolData readToolData(CountDataInputStream di, int size) throws IOException {
- ToolData td=new ToolData();
- td.read(di, size);
- return td;
- }
- private JointDataList readJointData(CountDataInputStream di, int size) throws IOException {
- JointDataList td=new JointDataList();
- td.read(di, size);
- return td;
- }
- private ToolCommInfo readToolCommInfo(CountDataInputStream di, int size) throws IOException {
- ToolCommInfo tci=new ToolCommInfo();
- tci.read(di, size);
- return tci;
- }
- private void skip(int pType, CountDataInputStream di, int size) throws IOException {
- log.trace("skip data for package type: "+pType);
- byte[] data=new byte[size-5];
- di.readFully(data);
- }
- private AdditionalInfo readAdditionalInfo(CountDataInputStream di, int size) throws IOException {
- AdditionalInfo ai=new AdditionalInfo();
- ai.read(di, size);
- return ai;
- }
- private ForceModeData readForceModeData(CountDataInputStream di, int size) throws IOException {
- ForceModeData fmd=new ForceModeData();
- fmd.read(di, size);
- return fmd;
- }
- private ConfigurationData readConfigurationData(CountDataInputStream di, int size) throws IOException {
- ConfigurationData cd=new ConfigurationData();
- cd.read(di, size);
- return cd;
- }
- void readRobotMessage(CountDataInputStream di, int size) throws IOException {
- long ts = di.readLong();
- byte source = di.readByte();
- int robotMessageType = di.readUnsignedByte();
- switch (robotMessageType) {
- case RobotMessageType.RUNTIME_EXCEPTION:
- var rte=new RobotRuntimeException();
- rte.read(di, size);
- URLog.add(rte);
- log.debug(rte.toString());
- break;
- case RobotMessageType.KEY:
- var rmk=new RobotMessageKey();
- rmk.read(di, size);
- URLog.add(rmk);
- log.debug(rmk.toString());
- break;
- case RobotMessageType.PROGRAM_LABEL:
- var rpl=new RobotProgramLabel();
- rpl.read(di, size);
- URLog.add(rpl);
- log.debug(rpl.toString());
- break;
- case RobotMessageType.ERROR_CODE:
- var rme=new RobotMessageError();
- rme.read(di, size);
- URLog.add(rme);
- log.debug(rme.toString());
- break;
- case RobotMessageType.ZWOELF:
- var zm=new ZwoelfMessageKey();
- zm.read(di, size);
- URLog.add(zm);
- log.debug(zm.toString());
- break;
- case RobotMessageType.TEXT:
- var t=new RobotMessageText();
- t.read(di, size);
- URLog.add(t);
- log.debug(t.toString());
- break;
- default:
- log.info("unknown msg: {} size: {}", (int) robotMessageType, size);
- byte[] buf = new byte[size - 15];
- di.readFully(buf);
- break;
- }
- }
- void readRobotState(CountDataInputStream di, int size) throws IOException {
- int remaining=size-5;
- while(remaining>0)
- remaining-=readSub(di);
- }
- CartesianInfo readCartesianInfo(CountDataInputStream di, int size) throws IOException {
- CartesianInfo ci = new CartesianInfo();
- ci.read(di, size);
- return ci;
- }
- KinematicsInfo readKinemsticsInfo(CountDataInputStream di, int size) throws IOException {
- KinematicsInfo ki = new KinematicsInfo();
- ki.read(di, size);
- return ki;
- }
- ModeData readRobotModeData(CountDataInputStream di, int size) throws IOException {
- long ts = di.readLong();
- ModeData md = new ModeData();
- md.realRobotConnected = di.readBoolean();
- md.realRobotEnabled = di.readBoolean();
- md.robotPoweredOn = di.readBoolean();
- md.emergencyStopped = di.readBoolean();
- md.protectiveStopped = di.readBoolean();
- md.isProgramRunning = di.readBoolean();
- md.isProgramPaused = di.readBoolean();
- md.robotMode = di.readByte();
- md.controlMode = di.readByte();
- md.targetSpeedFraction = di.readDouble();
- md.speedScaling = di.readDouble();
- md.targetSpeedFractionLimit = di.readDouble();
- md.internal = di.readByte();
- return md;
- }
- public void writeCmd(String cmd) {
- try {
- System.out.println("send cmd:" + cmd);
- os.write(cmd.getBytes(StandardCharsets.UTF_8));
- } catch (IOException e) {
- e.printStackTrace();
- }
- }
- public void readPackage() throws IOException {
- readReply(in);
- }
- }
|