123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228 |
- 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;
- 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 int readReply(CountDataInputStream di, boolean isSub) 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 PackageType.ROBOT_MODE_DATA:
- var md=readRobotModeData(di, size);
- log.info(md.toString());
- break;
- case PackageType.JOINT_DATA:
- var jd=readJointData(di, size);
- log.info(jd.toString());
- break;
- case PackageType.TOOL_DATA:
- var td=readToolData(di, size);
- log.info(td.toString());
- break;
- case PackageType.MASTERBOARD_DATA:
- var mb=readMasterBoardData(di, size);
- log.info(mb.toString());
- break;
- case PackageType.CARTESIAN_INFO:
- var c=readCartesianInfo(di, size);
- log.info(c.toString());
- break;
- case PackageType.KINEMATICS_INFO:
- var ki=readKinemsticsInfo(di, size);
- log.info(ki.toString());
- break;
- case PackageType.CONFIGURATION_DATA:
- var cd=readConfigurationData(di, size);
- log.info(cd.toString());
- break;
- case PackageType.FORCE_MODE_DATA:
- var fmd=readForceModeData(di, size);
- log.info(fmd.toString());
- break;
- case PackageType.ADDITIONAL_INFO:
- var ai=readAdditionalInfo(di, size);
- log.info(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);
- log.info(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);
- return mb;
- }
- private ToolData readToolData(CountDataInputStream di, int size) throws IOException {
- ToolData td=new ToolData();
- 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();
- char source = (char) (di.readByte() & 0xFF);
- char robotMessageType = (char) (di.readByte() & 0xFF);
- switch (robotMessageType) {
- default:
- log.warn("unknown msg" + (int) robotMessageType);
- //byte[] buf=new byte[size-11];
- byte[] buf = new byte[size - 10];
- di.readFully(buf);
- System.out.println(buf);
- }
- }
- void readRobotState(CountDataInputStream di, int size) throws IOException {
- int remaining=size-5;
- while(remaining>0)
- remaining-=readReply(di, true);
- }
- 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;
- }
- JointData[] readJointData(CountDataInputStream di, int size) throws IOException {
- int joints = size / 41;
- JointData[] jds = new JointData[size];
- for (int i = 0; i < joints; i++) {
- JointData jd = new JointData();
- jd.read(di, size);
- jds[i] = jd;
- }
- return jds;
- }
- 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, false);
- }
- }
|