123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135 |
- package at.acdp.urweb.rtde.packets;
- import java.io.DataInputStream;
- import java.io.DataOutputStream;
- import java.io.IOException;
- import java.util.HashMap;
- import static at.acdp.urweb.rtde.CommandType.RTDE_DATA_PACKAGE;
- import static at.acdp.urweb.rtde.CommandType.RTDE_TEXT_MESSAGE;
- public class RtdeInDataPackage implements IRtdeData {
- private final RtdeSetupInputs inputs;
- private HashMap<String, String> pdata=new HashMap<>();
- public RtdeInDataPackage(RtdeSetupInputs inputs) {
- this.inputs=inputs;
- }
- @Override
- public int getType() {
- return RTDE_DATA_PACKAGE;
- }
- public HashMap<String, String> getPdata() {
- return pdata;
- }
- @Override
- public RtdeInDataPackage read(DataInputStream di, int length) throws IOException {
- return this;
- }
- private int calcSize() {
- int size = 0;
- for (int i = 0; i < inputs.variables.size(); i++) {
- String output = inputs.variables.get(i);
- String type = inputs.types.get(i);
- switch (type) {
- case "BOOL":
- case "UINT8":
- size+=1;
- break;
- case "UINT32":
- case "INT32":
- size+=4;
- break;
- case "UINT64":
- case "DOUBLE":
- size+=8;
- break;
- case "VECTOR3D":
- case "VECTOR6INT32":
- case "VECTOR6UINT32":
- size+=24;
- break;
- case "VECTOR6D":
- size+=48;
- break;
- }
- }
- return size;
- }
- @Override
- public RtdeInDataPackage send(DataOutputStream dos) throws IOException {
- sendHeader(dos,calcSize()+1);
- dos.writeByte(inputs.recipe);
- for(int i=0; i<inputs.variables.size(); i++) {
- String output = inputs.variables.get(i);
- String type = inputs.types.get(i);
- switch (type) {
- case "BOOL":
- dos.writeBoolean(Boolean.parseBoolean(pdata.get(output)));
- break;
- case "UINT8":
- dos.writeByte(Byte.parseByte(pdata.get(output)));
- break;
- case "UINT32":
- case "INT32":
- dos.writeInt(Integer.parseInt(pdata.get(output)));
- break;
- case "UINT64":
- dos.writeLong(Long.parseLong(pdata.get(output)));
- break;
- case "DOUBLE":
- dos.writeDouble(Double.parseDouble(pdata.get(output)));
- break;
- case "VECTOR3D": {
- String s=pdata.get(output);
- var doubles=s.split(" ");
- dos.writeDouble(Double.parseDouble(doubles[0]));
- dos.writeDouble(Double.parseDouble(doubles[1]));
- dos.writeDouble(Double.parseDouble(doubles[2]));
- break;}
- case "VECTOR6D": {
- String s=pdata.get(output);
- var doubles=s.split(" ");
- dos.writeDouble(Double.parseDouble(doubles[0]));
- dos.writeDouble(Double.parseDouble(doubles[1]));
- dos.writeDouble(Double.parseDouble(doubles[2]));
- dos.writeDouble(Double.parseDouble(doubles[3]));
- dos.writeDouble(Double.parseDouble(doubles[4]));
- dos.writeDouble(Double.parseDouble(doubles[5]));
- break;}
- case "VECTOR6INT32":
- case "VECTOR6UINT32": {
- String s=pdata.get(output);
- var ints=s.split(" ");
- dos.writeInt(Integer.parseInt(ints[0]));
- dos.writeInt(Integer.parseInt(ints[1]));
- dos.writeInt(Integer.parseInt(ints[2]));
- dos.writeInt(Integer.parseInt(ints[3]));
- dos.writeInt(Integer.parseInt(ints[4]));
- dos.writeInt(Integer.parseInt(ints[5]));
- break;}
- default:
- throw new IllegalStateException("Unexpected value: " + type);
- }
- }
- return this;
- }
- public void put(String key, String value) {
- pdata.put(key, value);
- }
- @Override
- public boolean hasReply() {
- return false;
- }
- }
|