123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187 |
- package at.acdp.urweb.web;
- import java.io.DataInputStream;
- import java.io.IOException;
- import java.net.Socket;
- import java.net.SocketException;
- import java.util.HashMap;
- import java.util.Map;
- public class GetRobotRealtimeData implements Runnable {
- private final String ip;
- private final int port;
- private volatile boolean _running=true;
- /*****
- * Creates a new RealTime reader
- * Default Constructor
- * Uses localhost (127.0.0.1) and port 30003
- */
- public GetRobotRealtimeData(String ip, int port) {
- this.ip=ip;
- this.port=port;
- }
- // Internal method that actually reads the data
- private void readSocket() throws IOException {
- try(Socket rt = new Socket(ip, port);){
- rt.setSoTimeout(1000);
- if (rt.isConnected()){
- System.out.println("Connected to UR Realtime Client");
- }
- DataInputStream in = new DataInputStream(rt.getInputStream());
- while(true) {
- int length = in.readInt();
- double[] rtm = new double[length];
- rtm[0] = length;
- // Calculate how much data is available from the length
- int data_available = (length - 4) / 8;
- for(int i=0; i<data_available; i++){
- rtm[i] = in.readDouble();
- }
- }
- }
- }
- @Override
- public void run() {
- while(_running) {
- try {
- readSocket();
- } catch (IOException e) {
- e.printStackTrace();
- }
- }
- }
- /*
- * Creating enum to map start index and lengths of data entries
- * According to specification of RealTimeClient in Excel sheet
- */
- private enum RTinfo {
- // name (index in plot, number of doubles)
- q_target (2, 6),
- qd_target (8, 6),
- qdd_target (14, 6),
- q_actual (32, 6),
- qd_actual (38, 6),
- TCP_actual (56, 6),
- TCPd_actual (62, 6),
- TCP_force (68, 6),
- TCP_target (74, 6),
- TCPd_target (80, 6),
- temp_joint (87, 6),
- robotmode (95, 1),
- jointmode (96, 6),
- safetymode (97, 1),
- tcp_accel (109, 3),
- speedscaling (118, 1),
- prgstate (132, 1);
- // More data points could be added if desired, by following above example and according to specification.
- private final int index;
- private final int count;
- RTinfo(int index, int count){
- this.index = index;
- this.count = count;
- }
- private int index() {return index;}
- private int count() {return count;}
- }
- /*****************************************************************
- * Methods for returning the relevant data to the calling classes
- *****************************************************************/
- /*
- * Example get function, to read actual joint positions as double[]
- */
- public double[] getActualJointPose(double []rtm){
- double[] val = new double[RTinfo.q_actual.count()];
- int i = 0;
- while (i < RTinfo.q_actual.count()){
- val[i] = rtm[RTinfo.q_actual.index()+i];
- ++i;
- }
- return val;
- }
- /*
- * Example get function, to read actual TCP position as double[]
- */
- public double[] getActualTcpPose(double[] rtm){
- double[] val = new double[RTinfo.TCP_actual.count()];
- int i = 0;
- while (i < RTinfo.TCP_actual.count()){
- val[i] = rtm[RTinfo.TCP_actual.index()+i];
- ++i;
- }
- return val;
- }
- /*
- * Example get function, to read joint temperatures as double[]
- */
- public double[] getJointTemperatures(double[] rtm){
- double[] val = new double[RTinfo.temp_joint.count()];
- int i = 0;
- while (i < RTinfo.temp_joint.count()){
- val[i] = rtm[RTinfo.temp_joint.index()+i];
- ++i;
- }
- return val;
- }
- /*
- * Example to read joint state as useful information
- */
- public String[] getJointStatus(double[] rtm){
- // Create a map binding message code to state message
- // According to Excel sheet sclient interface specification
- Map<Double, String> jointStates = new HashMap<Double, String>();
- jointStates.put((double) 236, "SHUTTING_DOWN");
- jointStates.put((double) 237, "DUAL_CALIB_MODE");
- jointStates.put((double) 238, "BACKDRIVE");
- jointStates.put((double) 239, "POWER_OFF");
- jointStates.put((double) 245, "NOT_RESPONDING");
- jointStates.put((double) 246, "MOTOR_INIT");
- jointStates.put((double) 247, "BOOTING");
- jointStates.put((double) 248, "DUAL_CALIB_ERROR");
- jointStates.put((double) 249, "BOOTLOADER");
- jointStates.put((double) 250, "CALIBRATION_MODE");
- jointStates.put((double) 252, "FAULT");
- jointStates.put((double) 253, "RUNNING");
- jointStates.put((double) 255, "IDLE");
- String[] val = new String[RTinfo.jointmode.count()];
- int i = 0;
- while (i < RTinfo.jointmode.count()){
- // Read the code for given joint
- double code = rtm[RTinfo.jointmode.index()+i];
- // Check if the key is known in the map
- if(jointStates.containsKey(code)){
- // Read corresponding message
- val[i] = jointStates.get(code);
- }
- else{
- // If unknown code, show "unknown"
- val[i] = "UNKNOWN_CODE";
- }
- ++i;
- }
- return val;
- }
- /*
- * Example to read the actual safety limited speed scaling in percent
- */
- public double getSpeedScaling(double[] rtm){
- double scaling = rtm[RTinfo.speedscaling.index()];
- return scaling * 100; // Converted to percent
- }
- }
|