- * 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