From 86c6ac2824d8febe428c2b28f25f8b9dc053e445 Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Thu, 25 Apr 2024 09:06:12 +0200 Subject: [PATCH] format --- .../src/application/ROS2_Control.java | 24 ++--- .../src/ros2/modules/FRIManager.java | 66 ++++++------- .../src/ros2/modules/ROS2Connection.java | 98 +++++++++---------- .../src/ros2/modules/TCPConnection.java | 33 +++---- .../ros2/serialization/ControlModeParams.java | 26 ++--- .../serialization/FRIConfigurationParams.java | 4 +- ...intImpedanceControlModeExternalizable.java | 16 +-- .../ros2/serialization/MessageEncoding.java | 4 +- .../src/connection_helpers/fri_connection.cpp | 1 - .../test/test_driver_activation.py | 4 +- 10 files changed, 138 insertions(+), 138 deletions(-) diff --git a/kuka_sunrise_fri_driver/robot_application/src/application/ROS2_Control.java b/kuka_sunrise_fri_driver/robot_application/src/application/ROS2_Control.java index 98e752a7..d5fc6c6a 100755 --- a/kuka_sunrise_fri_driver/robot_application/src/application/ROS2_Control.java +++ b/kuka_sunrise_fri_driver/robot_application/src/application/ROS2_Control.java @@ -12,16 +12,16 @@ /** * Implementation of a robot application. *

- * The application provides a {@link RoboticsAPITask#initialize()} and a - * {@link RoboticsAPITask#run()} method, which will be called successively in - * the application lifecycle. The application will terminate automatically after - * the {@link RoboticsAPITask#run()} method has finished or after stopping the - * task. The {@link RoboticsAPITask#dispose()} method will be called, even if an - * exception is thrown during initialization or run. + * The application provides a {@link RoboticsAPITask#initialize()} and a + * {@link RoboticsAPITask#run()} method, which will be called successively in + * the application lifecycle. The application will terminate automatically after + * the {@link RoboticsAPITask#run()} method has finished or after stopping the + * task. The {@link RoboticsAPITask#dispose()} method will be called, even if an + * exception is thrown during initialization or run. *

- * It is imperative to call super.dispose() when overriding the - * {@link RoboticsAPITask#dispose()} method. - * + * It is imperative to call super.dispose() when overriding the + * {@link RoboticsAPITask#dispose()} method. + * * @see UseRoboticsAPIContext * @see #initialize() * @see #run() @@ -30,7 +30,7 @@ public class ROS2_Control extends RoboticsAPIApplication { @Inject private LBR _lbr; - + private TCPConnection _TCPConnection; private ROS2Connection _ROS2Connection; private FRIManager _FRIManager; @@ -41,7 +41,7 @@ public void initialize() { _TCPConnection = new TCPConnection(30000); _ROS2Connection = new ROS2Connection(); _FRIManager = new FRIManager(_lbr, getApplicationControl()); - + _FRIManager.registerROS2ConnectionModule(_ROS2Connection); _TCPConnection.registerROS2ConnectionModule(_ROS2Connection); _ROS2Connection.registerTCPConnectionModule(_TCPConnection); @@ -57,7 +57,7 @@ public void run() { _ROS2Connection.rejectCommands(); _FRIManager.close(); } - + @Override public void dispose() { getLogger().info("disposes"); diff --git a/kuka_sunrise_fri_driver/robot_application/src/ros2/modules/FRIManager.java b/kuka_sunrise_fri_driver/robot_application/src/ros2/modules/FRIManager.java index cb72826d..4dc69e58 100755 --- a/kuka_sunrise_fri_driver/robot_application/src/ros2/modules/FRIManager.java +++ b/kuka_sunrise_fri_driver/robot_application/src/ros2/modules/FRIManager.java @@ -32,7 +32,7 @@ public enum CommandResult{ ERRORED; } private ROS2Connection _ROS2Connection; - + private State _currentState; private LBR _lbr; private FRISession _FRISession; @@ -42,9 +42,9 @@ public enum CommandResult{ private IMotionContainer _motionContainer; private FRIMotionErrorHandler _friMotionErrorHandler = new FRIMotionErrorHandler(); //private IApplicationControl _applicationControl; - + private static double[] stiffness_ = new double[7]; - + public FRIManager(LBR lbr, IApplicationControl applicationControl){ _currentState = new InactiveState(); _lbr = lbr; @@ -57,11 +57,11 @@ public FRIManager(LBR lbr, IApplicationControl applicationControl){ //_applicationControl = applicationControl; applicationControl.registerMoveAsyncErrorHandler(_friMotionErrorHandler); } - + public void registerROS2ConnectionModule(ROS2Connection ros2ConnectionModule){ _ROS2Connection = ros2ConnectionModule; } - + public void close(){ if(_currentState instanceof ControlActiveState){ deactivateControl(); @@ -70,36 +70,36 @@ public void close(){ endFRI();//TODO: handle error } } - + public FRIConfigurationParams getFRIConfig(){ FRIConfigurationParams friConfigurationParams = new FRIConfigurationParams(_FRIConfiguration); return friConfigurationParams; } - + public ClientCommandMode getClientCommandMode(){ return _clientCommandMode; } - + public IMotionControlMode getControlMode(){ return _controlMode; } - + public FRISessionState getFRISessionState(){ return _FRISession.getFRIChannelInformation().getFRISessionState(); } - + /* * The Following commands change the state of the FRI Manager, and thus are forwarded to the state machine for validation * */ - + public CommandResult setControlMode(IMotionControlMode controlMode){ return _currentState.setControlMode(controlMode); } - + public CommandResult setCommandMode(ClientCommandMode clientCommandMode){ return _currentState.setCommandMode(clientCommandMode); } - + public CommandResult setFRIConfig(FRIConfigurationParams friConfigurationParams){ return _currentState.setFRIConfig(friConfigurationParams); } @@ -111,7 +111,7 @@ public CommandResult startFRI(){ } return commandResult; } - + public CommandResult endFRI(){ CommandResult commandResult = _currentState.endFRI(); if(commandResult == CommandResult.EXECUTED){ @@ -119,7 +119,7 @@ public CommandResult endFRI(){ } return commandResult; } - + public CommandResult activateControl(){ CommandResult commandResult = _currentState.activateControl(); if(commandResult == CommandResult.EXECUTED){ @@ -127,7 +127,7 @@ public CommandResult activateControl(){ } return commandResult; } - + public CommandResult deactivateControl(){ CommandResult commandResult = _currentState.deactivateControl(); if(commandResult == CommandResult.EXECUTED){ @@ -135,38 +135,38 @@ public CommandResult deactivateControl(){ } return commandResult; } - + private class State{ /* By default reject all commands */ public CommandResult startFRI(){ return CommandResult.REJECTED; } - + public CommandResult endFRI(){ return CommandResult.REJECTED; } - + public CommandResult activateControl(){ return CommandResult.REJECTED; } - + public CommandResult deactivateControl(){ return CommandResult.REJECTED; } - + public CommandResult setFRIConfig(FRIConfigurationParams friConfigurationParams){ return CommandResult.REJECTED; } - + public CommandResult setControlMode(IMotionControlMode controlMode){ return CommandResult.REJECTED; } - + public CommandResult setCommandMode(ClientCommandMode clientCommandMode){ return CommandResult.REJECTED; } } - + private class InactiveState extends State{ @Override public CommandResult startFRI(){ @@ -177,7 +177,7 @@ public CommandResult startFRI(){ FRIManager.this._FRISession.close(); return CommandResult.ERRORED; } - + return CommandResult.EXECUTED; } @Override @@ -196,7 +196,7 @@ public CommandResult setCommandMode(ClientCommandMode clientCommandMode){ return CommandResult.EXECUTED; } } - + private class FRIActiveState extends State { @Override public CommandResult endFRI(){ @@ -209,11 +209,11 @@ public CommandResult endFRI(){ } @Override public CommandResult activateControl(){ - FRIJointOverlay friJointOverlay = + FRIJointOverlay friJointOverlay = new FRIJointOverlay(FRIManager.this._FRISession, FRIManager.this._clientCommandMode); - PositionHold motion = + PositionHold motion = new PositionHold(FRIManager.this._controlMode, -1, null); - FRIManager.this._motionContainer = + FRIManager.this._motionContainer = FRIManager.this._lbr.moveAsync(motion.addMotionOverlay(friJointOverlay)); return CommandResult.EXECUTED; } @@ -228,7 +228,7 @@ public CommandResult setCommandMode(ClientCommandMode clientCommandMode){ return CommandResult.EXECUTED; } } - + private class ControlActiveState extends State { @Override public CommandResult deactivateControl(){ @@ -236,7 +236,7 @@ public CommandResult deactivateControl(){ return CommandResult.EXECUTED; } } - + private class FRIMotionErrorHandler implements IErrorHandler{ @Override @@ -260,7 +260,7 @@ public ErrorHandlingAction handleError(Device device, System.out.println("Cancelled containers: " + canceledContainers.toString()); return ErrorHandlingAction.Ignore; } - + } - + } diff --git a/kuka_sunrise_fri_driver/robot_application/src/ros2/modules/ROS2Connection.java b/kuka_sunrise_fri_driver/robot_application/src/ros2/modules/ROS2Connection.java index 13e2c338..c44e569c 100755 --- a/kuka_sunrise_fri_driver/robot_application/src/ros2/modules/ROS2Connection.java +++ b/kuka_sunrise_fri_driver/robot_application/src/ros2/modules/ROS2Connection.java @@ -20,37 +20,37 @@ public class ROS2Connection { private FRIManager _FRIManager; private boolean _acceptingCommands = false; private boolean _disconnect = false; - + public void registerTCPConnectionModule(TCPConnection tcpConnectionModule){ _TCPConnection = tcpConnectionModule; } - + public void registerFRIManagerModule(FRIManager friManagerModule){ _FRIManager = friManagerModule; } - + public void acceptCommands(){ _acceptingCommands = true; } - + public void rejectCommands(){ _acceptingCommands = false; } - + private enum CommandState{ ACCEPTED((byte)1), REJECTED((byte)2), UNKNOWN((byte)3), ERROR_CONTROL_ENDED((byte)4), ERROR_FRI_ENDED((byte)5); - + private final byte value; - + private CommandState(byte value){ this.value = value; } } - + private enum CommandID{ CONNECT( (byte)1), DISCONNECT( (byte)2), @@ -64,14 +64,14 @@ private enum CommandID{ SET_CONTROL_MODE( (byte)10), GET_COMMAND_MODE( (byte)11), SET_COMMAND_MODE( (byte)12); - + private final byte value; - + private CommandID(byte value){ this.value = value; - } + } + - public static CommandID fromByte(byte value){ for(CommandID id : CommandID.values()){ if(value == id.value){ @@ -81,28 +81,28 @@ public static CommandID fromByte(byte value){ throw new RuntimeException("Byte " + value + " does not represent an InMessageID"); } } - + private enum SuccessSignalID { SUCCESS((byte)1), NO_SUCCESS((byte)2); - + private final byte value; - + private SuccessSignalID(byte value){ this.value = value; } } - + private enum ControlModeID{ POSITION( (byte)1), JOINT_IMPEDANCE((byte)2); - + public final byte value; - + ControlModeID(byte value){ this.value = value; } - + public static ControlModeID fromByte(byte value){ for(ControlModeID id : ControlModeID.values()){ if(value == id.value){ @@ -112,7 +112,7 @@ public static ControlModeID fromByte(byte value){ throw new RuntimeException("Byte " + value + " does not represent a ControlModeID"); } } - + public void handleMessageFromROS(byte[] inMessage){ CommandID command = null; try{ @@ -122,18 +122,18 @@ public void handleMessageFromROS(byte[] inMessage){ feedbackCommandRejected(command, new String("Not accepting commands").getBytes()); return; } - + } catch(RuntimeException e){ System.out.println(e.getMessage()); feedbackCommandUnknown(e.getMessage().getBytes()); return; } - + try{ byte[] inMessageData = Arrays.copyOfRange(inMessage, 1, inMessage.length); byte[] feedbackData = null; System.out.println("Command received: " + command.toString()); - + switch(command){ case CONNECT: feedbackData = connect(inMessageData); @@ -178,9 +178,9 @@ public void handleMessageFromROS(byte[] inMessage){ System.out.println(e.getMessage()); feedbackCommandNoSuccess(command, e.getMessage().getBytes()); return; - } + } } - + public void handleConnectionLost(){ _FRIManager.deactivateControl(); _FRIManager.endFRI(); @@ -193,38 +193,38 @@ public void handleControlEndedError(){ System.out.println("Error: control ended"); _TCPConnection.sendBytes(message); } - + public void handleFRIEndedError(){ byte[] message = {CommandState.ERROR_FRI_ENDED.value}; System.out.println("Error: session ended"); _TCPConnection.sendBytes(message); } - + private void feedbackCommandRejected(CommandID command, byte[] feedbackData){ byte[] message = appendByte(command.value, feedbackData); message = appendByte(CommandState.REJECTED.value, message); _TCPConnection.sendBytes(message); } - + private void feedbackCommandUnknown(byte[] feedbackData){ byte[] message = appendByte(CommandState.UNKNOWN.value, feedbackData); _TCPConnection.sendBytes(message); } - + private void feedbackCommandSuccess(CommandID command, byte[] feedbackData){ byte[] message = appendByte(SuccessSignalID.SUCCESS.value, feedbackData); message = appendByte(command.value, message); message = appendByte(CommandState.ACCEPTED.value, message); _TCPConnection.sendBytes(message); } - + private void feedbackCommandNoSuccess(CommandID command, byte[] feedbackData){ byte[] message = appendByte(SuccessSignalID.NO_SUCCESS.value, feedbackData); message = appendByte(command.value, message); message = appendByte(CommandState.ACCEPTED.value, message); _TCPConnection.sendBytes(message); } - + private byte[] appendByte(byte id, byte[] data){ byte[] message = null; if(data == null){ @@ -236,18 +236,18 @@ private byte[] appendByte(byte id, byte[] data){ } return message; } - + private byte[] connect(byte[] cmdData){ return null; } - + private byte[] disconnect(byte[] cmdData){ _FRIManager.close(); _disconnect = true; //_TCPConnection.closeConnection(); //TODO: close connection after feedback was sent return null; } - + private byte[] startFRI(byte[] cmdData){ FRIManager.CommandResult commandResult = _FRIManager.startFRI(); if(commandResult == FRIManager.CommandResult.REJECTED){ @@ -258,7 +258,7 @@ private byte[] startFRI(byte[] cmdData){ } return null; } - + private byte[] endFRI(byte[] cmdData){ FRIManager.CommandResult commandResult = _FRIManager.endFRI(); if(commandResult == FRIManager.CommandResult.REJECTED){ @@ -269,7 +269,7 @@ private byte[] endFRI(byte[] cmdData){ } return null; } - + private byte[] activateControl(byte[] cmdData){ FRIManager.CommandResult commandResult = _FRIManager.activateControl(); if(commandResult == FRIManager.CommandResult.REJECTED){ @@ -280,7 +280,7 @@ private byte[] activateControl(byte[] cmdData){ } return null; } - + private byte[] deactivateControl(byte[] cmdData){ FRIManager.CommandResult commandResult = _FRIManager.deactivateControl(); if(commandResult == FRIManager.CommandResult.REJECTED){ @@ -291,16 +291,16 @@ private byte[] deactivateControl(byte[] cmdData){ } return null; } - + private byte[] getFRIConfig(byte[] cmdData){ FRIConfigurationParams friConfigurationParams = _FRIManager.getFRIConfig(); byte[] feedbackData = MessageEncoding.Encode(friConfigurationParams, FRIConfigurationParams.length); return feedbackData; } - + private byte[] setFRIConfig(byte[] cmdData) { ensureArrayLength(cmdData, FRIConfigurationParams.length + 6); - + FRIConfigurationParams friConfigurationParams = new FRIConfigurationParams(); MessageEncoding.Decode(cmdData, friConfigurationParams); FRIManager.CommandResult commandResult = _FRIManager.setFRIConfig(friConfigurationParams); @@ -312,7 +312,7 @@ private byte[] setFRIConfig(byte[] cmdData) { } return null; } - + private byte[] getControlMode(byte[] cmdData){ IMotionControlMode controlMode = _FRIManager.getControlMode(); ControlModeID controlModeID; @@ -329,13 +329,13 @@ private byte[] getControlMode(byte[] cmdData){ byte[] message = appendByte(controlModeID.value, controlModeData); return message; } - + private byte[] setControlMode(byte[] cmdData){ ensureArrayLength(cmdData, 1); ControlModeID controlModeID = ControlModeID.fromByte(cmdData[0]); - + byte[] controlModeData = Arrays.copyOfRange(cmdData, 1, cmdData.length); - + IMotionControlMode controlMode = null; switch(controlModeID){ case POSITION: @@ -359,14 +359,14 @@ private byte[] setControlMode(byte[] cmdData){ } return null; } - + private byte[] getCommandMode(byte[] cmdData){ ClientCommandMode clientCommandMode = _FRIManager.getClientCommandMode(); byte[] commandModeData = new byte[1]; commandModeData[0] = (byte)clientCommandMode.ordinal();//TODO: check if ordinal == value return commandModeData; } - + private byte[] setCommandMode(byte[] cmdData){ ensureArrayLength(cmdData, 1); ClientCommandMode clientCommandMode = ClientCommandMode.intToVal((int)cmdData[0]); @@ -382,7 +382,7 @@ private byte[] setCommandMode(byte[] cmdData){ } return null; } - + private void ensureArrayLength(byte[] array, int requiredLength){ if(array == null){ throw new RuntimeException("Array is null"); @@ -391,5 +391,5 @@ private void ensureArrayLength(byte[] array, int requiredLength){ throw new RuntimeException("Array does not satisfy length requirement. Required length: " + requiredLength + ", actual length: " + array.length); } } - -} \ No newline at end of file + +} diff --git a/kuka_sunrise_fri_driver/robot_application/src/ros2/modules/TCPConnection.java b/kuka_sunrise_fri_driver/robot_application/src/ros2/modules/TCPConnection.java index eedcc703..f39018f5 100755 --- a/kuka_sunrise_fri_driver/robot_application/src/ros2/modules/TCPConnection.java +++ b/kuka_sunrise_fri_driver/robot_application/src/ros2/modules/TCPConnection.java @@ -14,7 +14,7 @@ public class TCPConnection{ private boolean _closeRequested; private byte[] _incomingData; private ROS2Connection _ROS2Connection; - + public TCPConnection(int tcpServerPort){ _tcpServerPort = tcpServerPort; _tcpServer = null; @@ -23,11 +23,11 @@ public TCPConnection(int tcpServerPort){ _closeRequested = false; _incomingData = null; } - + public void registerROS2ConnectionModule(ROS2Connection ros2ConnectionModule){ _ROS2Connection = ros2ConnectionModule; } - + public void waitUntilDisconnected(){ try { _tcpConnectionThread.join(); @@ -36,7 +36,7 @@ public void waitUntilDisconnected(){ e.printStackTrace(); } } - + private class ConnectionHandler implements Runnable{ public void run(){ try{ @@ -68,7 +68,7 @@ public void run(){ _closeRequested = false; } } - + public void openConnection() { try { _tcpServer = new ServerSocket(_tcpServerPort); @@ -78,7 +78,7 @@ public void openConnection() { } _tcpConnectionThread.start(); } - + public void sendString(String s){ if(_tcpClient != null && _tcpClient.isConnected() && !_tcpClient.isClosed()){ try{ @@ -93,7 +93,7 @@ public void sendString(String s){ System.out.println("TCP client not connected."); } } - + public void sendBytes(byte[] message){ if(_tcpClient != null && _tcpClient.isConnected() && !_tcpClient.isClosed()){ try{ @@ -107,13 +107,13 @@ public void sendBytes(byte[] message){ System.out.println("TCP client not connected."); } } - + public byte[] getReceivedData(){ byte[] dataCopy = _incomingData; _incomingData = null; return dataCopy; } - + public void closeConnection(){ _closeRequested = true; try{ @@ -129,7 +129,7 @@ public void closeConnection(){ e.printStackTrace(); } } - + private void waitForConnection() throws Exception { System.out.println("Waiting for connection..."); try { @@ -142,10 +142,10 @@ private void waitForConnection() throws Exception { throw e; } - } + } System.out.println("Connection established."); } - + private void handleIncomingData() throws IOException{ BufferedReader inFromClient = new BufferedReader(new InputStreamReader(_tcpClient.getInputStream())); while(_tcpClient.isClosed() == false){ @@ -168,7 +168,7 @@ private void handleIncomingData() throws IOException{ byte[] byteArray = Arrays.copyOf(new String(charArray).getBytes(), dataLength); String byteHexString = DatatypeConverter.printHexBinary(byteArray); - + if(_incomingData != null){ System.out.println("ERROR: Previous data not yet processed! Skipping data: " + byteHexString); } @@ -180,8 +180,7 @@ private void handleIncomingData() throws IOException{ } } } - - - -} + + +} diff --git a/kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/ControlModeParams.java b/kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/ControlModeParams.java index 397fca5a..4872d1ca 100755 --- a/kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/ControlModeParams.java +++ b/kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/ControlModeParams.java @@ -13,17 +13,17 @@ public abstract class ControlModeParams implements Externalizable{ public static int length = 0; - + private enum ControlModeID{ POSITION( (byte)1), JOINT_IMPEDANCE((byte)2); - + public final byte value; - + ControlModeID(byte value){ this.value = value; } - + public static ControlModeID fromByte(byte value){ for(ControlModeID id : ControlModeID.values()){ if(value == id.value){ @@ -33,7 +33,7 @@ public static ControlModeID fromByte(byte value){ throw new RuntimeException("Byte " + value + " does not represent an ControlModeID"); } } - + public static ControlModeParams fromSerialData(byte[] serialData){ if(serialData.length == 0){ throw new RuntimeException("serialData is empty"); @@ -52,7 +52,7 @@ public static ControlModeParams fromSerialData(byte[] serialData){ MessageEncoding.Decode(serialData, controlModeParams); return controlModeParams; } - + public static ControlModeParams fromMotionControlMode(IMotionControlMode controlMode){ if(controlMode == null){ throw new RuntimeException("ControlMode is null"); @@ -67,19 +67,19 @@ public static ControlModeParams fromMotionControlMode(IMotionControlMode control } return controlModeParams; } - + @Override public void writeExternal(ObjectOutput out) throws IOException { - - + + } @Override public void readExternal(ObjectInput in) throws IOException, ClassNotFoundException { - + } - + } class PositionControlModeParams extends ControlModeParams{ @@ -89,9 +89,9 @@ class PositionControlModeParams extends ControlModeParams{ class JointImpedanceControlModeParams extends ControlModeParams{ public JointImpedanceControlModeParams(){ - + } public JointImpedanceControlModeParams(JointImpedanceControlMode controlMode){ - + } } diff --git a/kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/FRIConfigurationParams.java b/kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/FRIConfigurationParams.java index 96090dcb..d7721cb1 100755 --- a/kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/FRIConfigurationParams.java +++ b/kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/FRIConfigurationParams.java @@ -32,7 +32,7 @@ public void readExternal(ObjectInput in) throws IOException, _sendPeriodMilliSec = in.readInt(); _receiveMultiplier = in.readInt(); _remoteIP = "192.168.38.6"; - + int ip = in.readInt(); _remoteIP = String.format("%d.%d.%d.%d", (ip & 0xff),(ip >> 8 & 0xff), (ip >> 16 & 0xff), (ip >> 24 & 0xff)); @@ -60,4 +60,4 @@ public FRIConfiguration toFRIConfiguration(Device device){ friConfiguration.setReceiveMultiplier(_receiveMultiplier); return friConfiguration; } -} \ No newline at end of file +} diff --git a/kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/JointImpedanceControlModeExternalizable.java b/kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/JointImpedanceControlModeExternalizable.java index 34921c99..618205a2 100755 --- a/kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/JointImpedanceControlModeExternalizable.java +++ b/kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/JointImpedanceControlModeExternalizable.java @@ -13,20 +13,20 @@ public class JointImpedanceControlModeExternalizable extends JointImpedanceControlMode implements Externalizable{ public final static int length = 112; - + public JointImpedanceControlModeExternalizable(){ super(1000, 1000, 1000, 1000, 1000, 1000, 1000); } - + public JointImpedanceControlModeExternalizable(JointImpedanceControlMode other){ super(other); } - + public IMotionControlMode toControlMode(){ JointImpedanceControlMode controlMode = new JointImpedanceControlMode((JointImpedanceControlMode)this); return (IMotionControlMode)controlMode; } - + @Override public void writeExternal(ObjectOutput out) throws IOException { for(double jointStiffness : getStiffness()){ @@ -40,18 +40,18 @@ public void writeExternal(ObjectOutput out) throws IOException { @Override public void readExternal(ObjectInput in) throws IOException, ClassNotFoundException { - double[] jointStiffness = new double[getStiffness().length]; + double[] jointStiffness = new double[getStiffness().length]; for(int i = 0; i < getStiffness().length; i++){ jointStiffness[i] = in.readDouble(); } setStiffness(jointStiffness); - - double[] jointDamping = new double[getDamping().length]; + + double[] jointDamping = new double[getDamping().length]; for(int i = 0; i < getDamping().length; i++){ jointDamping[i] = in.readDouble(); } setDamping(jointDamping); - + } diff --git a/kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/MessageEncoding.java b/kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/MessageEncoding.java index 8a710ac6..7083509d 100755 --- a/kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/MessageEncoding.java +++ b/kuka_sunrise_fri_driver/robot_application/src/ros2/serialization/MessageEncoding.java @@ -22,7 +22,7 @@ public static byte[] Encode(Externalizable objectIn, int serialLength){ } return serialDataOut; } - + public static void Decode(byte[] serialDataIn, Externalizable objectOut) throws RuntimeException{ try{ ByteArrayInputStream serialDataStream = new ByteArrayInputStream(serialDataIn); @@ -31,7 +31,7 @@ public static void Decode(byte[] serialDataIn, Externalizable objectOut) throws objectStream.close(); } catch(IOException e){ e.printStackTrace(); - throw new RuntimeException("IO Exception occured"); + throw new RuntimeException("IO Exception occurred"); } catch (ClassNotFoundException e) { throw new RuntimeException("Message could not be decoded"); } diff --git a/kuka_sunrise_fri_driver/src/connection_helpers/fri_connection.cpp b/kuka_sunrise_fri_driver/src/connection_helpers/fri_connection.cpp index 8cea24e2..8f1b0f5c 100644 --- a/kuka_sunrise_fri_driver/src/connection_helpers/fri_connection.cpp +++ b/kuka_sunrise_fri_driver/src/connection_helpers/fri_connection.cpp @@ -20,7 +20,6 @@ #include "rclcpp/logging.hpp" #include -#include #include "kuka_sunrise_fri_driver/fri_connection.hpp" #include "kuka_sunrise_fri_driver/serialization.hpp" diff --git a/kuka_sunrise_fri_driver/test/test_driver_activation.py b/kuka_sunrise_fri_driver/test/test_driver_activation.py index baa67a00..80705852 100644 --- a/kuka_sunrise_fri_driver/test/test_driver_activation.py +++ b/kuka_sunrise_fri_driver/test/test_driver_activation.py @@ -83,4 +83,6 @@ def test_read_stdout(self, proc_output): proc_output.assertWaitFor( "Successful 'configure' of hardware 'lbr_iiwa14_r820'", timeout=15 ) - proc_output.assertWaitFor("Successful 'activate' of hardware 'lbr_iiwa14_r820'", timeout=20) + proc_output.assertWaitFor( + "Successful 'activate' of hardware 'lbr_iiwa14_r820'", timeout=20 + )