Skip to content
GitLab
Menu
Projects
Groups
Snippets
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
JastAdd
ros2rag
Commits
8430dfc7
Commit
8430dfc7
authored
Jun 04, 2020
by
René Schöne
Browse files
Use JSON config for setup (both starter and receiver).
- Starter: Send initial DataConfig - Starter: Always send complete RobotConfig
parent
bd6c3376
Pipeline
#6886
failed with stage
in 2 minutes and 56 seconds
Changes
8
Pipelines
2
Hide whitespace changes
Inline
Side-by-side
ros2rag.base/src/main/java/org/jastadd/ros2rag/compiler/SimpleMain.java
View file @
8430dfc7
...
@@ -7,6 +7,7 @@ import org.jastadd.ros2rag.scanner.Ros2RagScanner;
...
@@ -7,6 +7,7 @@ import org.jastadd.ros2rag.scanner.Ros2RagScanner;
import
java.io.BufferedReader
;
import
java.io.BufferedReader
;
import
java.io.IOException
;
import
java.io.IOException
;
import
java.nio.ByteBuffer
;
import
java.nio.file.Files
;
import
java.nio.file.Files
;
import
java.nio.file.Path
;
import
java.nio.file.Path
;
import
java.nio.file.Paths
;
import
java.nio.file.Paths
;
...
@@ -17,34 +18,95 @@ import java.nio.file.Paths;
...
@@ -17,34 +18,95 @@ import java.nio.file.Paths;
* @author rschoene - Initial contribution
* @author rschoene - Initial contribution
*/
*/
public
class
SimpleMain
{
public
class
SimpleMain
{
// --- just testing byte[] conversion ---
public
static
void
testing
()
{
byte
[]
bytes
;
int
i
=
1
;
double
d
=
2.3d
;
float
f
=
4.2f
;
short
sh
=
13
;
long
l
=
7L
;
String
s
=
"Hello"
;
char
c
=
'a'
;
Integer
ii
=
Integer
.
valueOf
(
1
);
if
(!
ii
.
equals
(
i
))
throw
new
AssertionError
(
"Ints not equal"
);
// int to byte
ByteBuffer
i2b
=
ByteBuffer
.
allocate
(
4
);
i2b
.
putInt
(
i
);
bytes
=
i2b
.
array
();
// byte to int
ByteBuffer
b2i
=
ByteBuffer
.
wrap
(
bytes
);
int
actual_i
=
b2i
.
getInt
();
if
(
i
!=
actual_i
)
throw
new
AssertionError
(
"Ints not equal"
);
// double to byte
ByteBuffer
d2b
=
ByteBuffer
.
allocate
(
8
);
d2b
.
putDouble
(
d
);
bytes
=
d2b
.
array
();
// byte to double
ByteBuffer
b2d
=
ByteBuffer
.
wrap
(
bytes
);
double
actual_d
=
b2d
.
getDouble
();
if
(
d
!=
actual_d
)
throw
new
AssertionError
(
"Doubles not equal"
);
// float to byte
ByteBuffer
f2b
=
ByteBuffer
.
allocate
(
4
);
f2b
.
putFloat
(
f
);
bytes
=
f2b
.
array
();
// byte to float
ByteBuffer
b2f
=
ByteBuffer
.
wrap
(
bytes
);
float
actual_f
=
b2f
.
getFloat
();
if
(
f
!=
actual_f
)
throw
new
AssertionError
(
"Floats not equal"
);
// short to byte
ByteBuffer
sh2b
=
ByteBuffer
.
allocate
(
2
);
sh2b
.
putShort
(
sh
);
bytes
=
sh2b
.
array
();
// byte to short
ByteBuffer
b2sh
=
ByteBuffer
.
wrap
(
bytes
);
short
actual_sh
=
b2sh
.
getShort
();
if
(
sh
!=
actual_sh
)
throw
new
AssertionError
(
"Shorts not equal"
);
// long to byte
ByteBuffer
l2b
=
ByteBuffer
.
allocate
(
8
);
l2b
.
putLong
(
l
);
bytes
=
l2b
.
array
();
// byte to long
ByteBuffer
b2l
=
ByteBuffer
.
wrap
(
bytes
);
long
actual_l
=
b2l
.
getLong
();
if
(
l
!=
actual_l
)
throw
new
AssertionError
(
"Longs not equal"
);
// String to byte
bytes
=
s
.
getBytes
();
// byte to String
String
actual_s
=
new
String
(
bytes
);
if
(!
s
.
equals
(
actual_s
))
throw
new
AssertionError
(
"Strings not equal"
);
// char to byte
ByteBuffer
c2b
=
ByteBuffer
.
allocate
(
2
);
c2b
.
putChar
(
c
);
bytes
=
c2b
.
array
();
// byte to char
ByteBuffer
b2c
=
ByteBuffer
.
wrap
(
bytes
);
char
actual_c
=
b2c
.
getChar
();
if
(
c
!=
actual_c
)
throw
new
AssertionError
(
"Floats not equal"
);
}
public
static
void
main
(
String
[]
args
)
{
public
static
void
main
(
String
[]
args
)
{
/*
testing
();
// as soon as the cache of isInSafetyZone is invalidated, update the value of Robot.ShouldUseLowSpeed with its value
// createManualAST();
[always] update Robot.ShouldUseLowSpeed with isInSafetyZone() using transformation();
}
// when a (new?) value for ShouldUseLowSpeed is set, send it over via mqtt
private
static
void
createManualAST
()
{
[always] write Robot.ShouldUseLowSpeed;
// when an update of pose is read via mqtt, then update current position
[always] read Joint.CurrentPosition using PoseToPosition;
// PBPose is a datatype defined in protobuf
PoseToPosition: map PBPose to Position using
pose.position.x += sqrt(.5 * size.x)
MAP round(2)
x = x / 100
IGNORE_IF_SAME
;
--- using generated methods ---
Joint j;
j.getCurrentPosition().connectTo("/robot/joint2/pos");
RobotArm r;
// this should not be required
r.getShouldUseLowSpeed().addObserver(j.getCurrentPosition());
r.getShouldUseLowSpeed().connectTo("/robot/config/speed");
*/
Ros2Rag
model
=
new
Ros2Rag
();
Ros2Rag
model
=
new
Ros2Rag
();
Program
program
=
parseProgram
(
Paths
.
get
(
"src"
,
"test"
,
"resources"
,
"Example.relast"
));
Program
program
=
parseProgram
(
Paths
.
get
(
"src"
,
"test"
,
"resources"
,
"Example.relast"
));
model
.
setProgram
(
program
);
model
.
setProgram
(
program
);
...
...
ros2rag.receiverstub/src/main/java/de/tudresden/inf/st/ros2rag/receiverstub/Main.java
View file @
8430dfc7
package
de.tudresden.inf.st.ros2rag.receiverstub
;
package
de.tudresden.inf.st.ros2rag.receiverstub
;
import
com.beust.jcommander.JCommander
;
import
com.beust.jcommander.Parameter
;
import
com.fasterxml.jackson.databind.ObjectMapper
;
import
com.google.protobuf.InvalidProtocolBufferException
;
import
com.google.protobuf.InvalidProtocolBufferException
;
import
config.Dataconfig
;
import
config.Dataconfig.DataConfig
;
import
config.Robotconfig.RobotConfig
;
import
config.Robotconfig.RobotConfig
;
import
de.tudresden.inf.st.ros2rag.starter.ast.MqttUpdater
;
import
de.tudresden.inf.st.ros2rag.starter.ast.MqttUpdater
;
import
de.tudresden.inf.st.ros2rag.starter.data.DataConfiguration
;
import
de.tudresden.inf.st.ros2rag.starter.data.DataJoint
;
import
org.apache.logging.log4j.LogManager
;
import
org.apache.logging.log4j.LogManager
;
import
org.apache.logging.log4j.Logger
;
import
org.apache.logging.log4j.Logger
;
import
panda.Linkstate.PandaLinkState
;
import
panda.Linkstate.PandaLinkState
;
import
java.io.File
;
import
java.io.IOException
;
import
java.nio.file.Paths
;
import
java.util.ArrayList
;
import
java.util.List
;
import
java.util.concurrent.CountDownLatch
;
import
java.util.concurrent.TimeUnit
;
import
java.util.concurrent.TimeUnit
;
import
java.util.concurrent.locks.Condition
;
import
java.util.concurrent.locks.Condition
;
import
java.util.concurrent.locks.Lock
;
import
java.util.concurrent.locks.Lock
;
...
@@ -17,83 +30,103 @@ public class Main {
...
@@ -17,83 +30,103 @@ public class Main {
private
static
final
Logger
logger
=
LogManager
.
getLogger
(
Main
.
class
);
private
static
final
Logger
logger
=
LogManager
.
getLogger
(
Main
.
class
);
public
static
void
main
(
String
[]
args
)
throws
Exception
{
public
static
void
main
(
String
[]
args
)
throws
Exception
{
String
jointTopic
,
configTopic
;
Main
main
=
new
Main
();
if
(
args
.
length
<
2
)
{
ObjectMapper
mapper
=
new
ObjectMapper
();
jointTopic
=
"robot/joint1"
;
File
configFile
=
new
File
(
args
[
0
]);
configTopic
=
"robot/config"
;
System
.
out
.
println
(
"Using config file: "
+
configFile
.
getAbsolutePath
());
}
else
{
DataConfiguration
config
=
mapper
.
readValue
(
configFile
,
DataConfiguration
.
class
);
jointTopic
=
args
[
0
];
main
.
run
(
config
);
configTopic
=
args
[
1
];
}
}
final
Lock
finishLock
=
new
ReentrantLock
();
private
void
run
(
DataConfiguration
config
)
throws
IOException
,
InterruptedException
{
final
Co
ndition
finishCondition
=
finishLock
.
newCo
ndition
(
);
final
Co
untDownLatch
finish
=
new
Co
untDownLatch
(
1
);
MqttUpdater
receiver
=
new
MqttUpdater
(
"receiver stub"
);
MqttUpdater
receiver
=
new
MqttUpdater
(
"receiver stub"
);
receiver
.
setHost
(
"localhost"
,
1883
);
receiver
.
setHost
(
config
.
mqttHost
);
receiver
.
waitUntilReady
(
2
,
TimeUnit
.
SECONDS
);
receiver
.
waitUntilReady
(
2
,
TimeUnit
.
SECONDS
);
receiver
.
newConnection
(
configTopic
,
bytes
->
{
receiver
.
newConnection
(
config
.
robotConfigTopic
,
this
::
printRobotConfig
);
try
{
receiver
.
newConnection
(
config
.
dataConfigTopic
,
this
::
printDataConfig
);
logger
.
debug
(
"Got a config message, parsing ..."
);
for
(
DataJoint
joint
:
config
.
joints
)
{
RobotConfig
robotConfig
=
RobotConfig
.
parseFrom
(
bytes
);
receiver
.
newConnection
(
joint
.
topic
,
this
::
printPandaLinkState
);
logger
.
info
(
"robotConfig: speed = {}, loopTrajectory = {}, planningMode = {}"
,
}
robotConfig
.
getSpeed
(),
robotConfig
.
getLoopTrajectory
(),
robotConfig
.
getPlanningMode
().
toString
());
}
catch
(
InvalidProtocolBufferException
e
)
{
e
.
printStackTrace
();
}
});
receiver
.
newConnection
(
jointTopic
,
bytes
->
{
try
{
logger
.
debug
(
"Got a joint message, parsing ..."
);
PandaLinkState
pls
=
PandaLinkState
.
parseFrom
(
bytes
);
PandaLinkState
.
Position
tmpPosition
=
pls
.
getPos
();
PandaLinkState
.
Orientation
tmpOrientation
=
pls
.
getOrient
();
PandaLinkState
.
TwistLinear
tmpTwistLinear
=
pls
.
getTl
();
PandaLinkState
.
TwistAngular
tmpTwistAngular
=
pls
.
getTa
();
logger
.
info
(
"{}: pos({},{},{}), orient({},{},{},{}),"
+
" twist-linear({},{},{}), twist-angular({},{},{})"
,
pls
.
getName
(),
tmpPosition
.
getPositionX
(),
tmpPosition
.
getPositionY
(),
tmpPosition
.
getPositionZ
(),
tmpOrientation
.
getOrientationX
(),
tmpOrientation
.
getOrientationY
(),
tmpOrientation
.
getOrientationZ
(),
tmpOrientation
.
getOrientationW
(),
tmpTwistLinear
.
getTwistLinearX
(),
tmpTwistLinear
.
getTwistLinearY
(),
tmpTwistLinear
.
getTwistLinearZ
(),
tmpTwistAngular
.
getTwistAngularX
(),
tmpTwistAngular
.
getTwistAngularY
(),
tmpTwistAngular
.
getTwistAngularZ
());
}
catch
(
InvalidProtocolBufferException
e
)
{
e
.
printStackTrace
();
}
});
receiver
.
newConnection
(
"components"
,
bytes
->
{
receiver
.
newConnection
(
"components"
,
bytes
->
{
String
message
=
new
String
(
bytes
);
String
message
=
new
String
(
bytes
);
logger
.
info
(
"Components: {}"
,
message
);
logger
.
info
(
"Components: {}"
,
message
);
});
});
receiver
.
newConnection
(
"receiver"
,
bytes
->
{
receiver
.
newConnection
(
"receiver"
,
bytes
->
{
String
message
=
new
String
(
bytes
);
String
message
=
new
String
(
bytes
);
if
(
message
.
equals
(
"exit"
))
{
if
(
message
.
equals
(
"exit"
))
{
try
{
finish
.
countDown
();
finishLock
.
lock
();
finishCondition
.
signal
();
}
finally
{
finishLock
.
unlock
();
}
}
}
});
});
try
{
finishLock
.
lock
();
Runtime
.
getRuntime
().
addShutdownHook
(
new
Thread
(
receiver:
:
close
));
finishCondition
.
await
();
if
(
config
.
exitAfterSeconds
>
0
)
{
}
finally
{
finish
.
await
(
config
.
exitAfterSeconds
,
TimeUnit
.
SECONDS
);
finishLock
.
unlock
();
}
else
{
finish
.
await
();
}
}
receiver
.
close
();
receiver
.
close
();
Runtime
.
getRuntime
().
addShutdownHook
(
new
Thread
(
receiver:
:
close
));
}
}
private
void
printPandaLinkState
(
byte
[]
bytes
)
{
try
{
logger
.
debug
(
"Got a joint message, parsing ..."
);
PandaLinkState
pls
=
PandaLinkState
.
parseFrom
(
bytes
);
PandaLinkState
.
Position
tmpPosition
=
pls
.
getPos
();
PandaLinkState
.
Orientation
tmpOrientation
=
pls
.
getOrient
();
PandaLinkState
.
TwistLinear
tmpTwistLinear
=
pls
.
getTl
();
PandaLinkState
.
TwistAngular
tmpTwistAngular
=
pls
.
getTa
();
logger
.
info
(
"{}: pos({},{},{}), orient({},{},{},{}),"
+
" twist-linear({},{},{}), twist-angular({},{},{})"
,
pls
.
getName
(),
tmpPosition
.
getPositionX
(),
tmpPosition
.
getPositionY
(),
tmpPosition
.
getPositionZ
(),
tmpOrientation
.
getOrientationX
(),
tmpOrientation
.
getOrientationY
(),
tmpOrientation
.
getOrientationZ
(),
tmpOrientation
.
getOrientationW
(),
tmpTwistLinear
.
getTwistLinearX
(),
tmpTwistLinear
.
getTwistLinearY
(),
tmpTwistLinear
.
getTwistLinearZ
(),
tmpTwistAngular
.
getTwistAngularX
(),
tmpTwistAngular
.
getTwistAngularY
(),
tmpTwistAngular
.
getTwistAngularZ
());
}
catch
(
InvalidProtocolBufferException
e
)
{
e
.
printStackTrace
();
}
}
private
void
printRobotConfig
(
byte
[]
bytes
)
{
try
{
logger
.
debug
(
"Got a robotConfig message, parsing ..."
);
RobotConfig
robotConfig
=
RobotConfig
.
parseFrom
(
bytes
);
logger
.
info
(
"robotConfig: speed = {}, loopTrajectory = {}, planningMode = {}"
,
robotConfig
.
getSpeed
(),
robotConfig
.
getLoopTrajectory
(),
robotConfig
.
getPlanningMode
().
toString
());
}
catch
(
InvalidProtocolBufferException
e
)
{
e
.
printStackTrace
();
}
}
private
void
printDataConfig
(
byte
[]
bytes
)
{
try
{
logger
.
debug
(
"Got a dataConfig message, parsing ..."
);
DataConfig
dataConfig
=
DataConfig
.
parseFrom
(
bytes
);
logger
.
info
(
"dataConfig: position = {}, orientation = {}, twistLinear = {}, twistAngular = {}, publishRate = {}"
,
dataConfig
.
getEnablePosition
(),
dataConfig
.
getEnableOrientation
(),
dataConfig
.
getEnableTwistLinear
(),
dataConfig
.
getEnableTwistAngular
(),
dataConfig
.
getPublishRate
());
}
catch
(
InvalidProtocolBufferException
e
)
{
e
.
printStackTrace
();
}
}
}
}
ros2rag.starter/build.gradle
View file @
8430dfc7
...
@@ -4,7 +4,7 @@ apply plugin: 'com.google.protobuf'
...
@@ -4,7 +4,7 @@ apply plugin: 'com.google.protobuf'
sourceCompatibility
=
1.8
sourceCompatibility
=
1.8
mainClassName
=
'de.tudresden.inf.st.ros2rag.starter.Main'
mainClassName
=
'de.tudresden.inf.st.ros2rag.starter.
Starter
Main'
repositories
{
repositories
{
jcenter
()
jcenter
()
...
@@ -23,7 +23,7 @@ configurations {
...
@@ -23,7 +23,7 @@ configurations {
}
}
sourceSets
.
main
.
java
.
srcDir
"src/gen/java"
sourceSets
.
main
.
java
.
srcDir
"src/gen/java"
jar
.
manifest
.
attributes
(
'Main-Class'
:
'de.tudresden.inf.st.ros2rag.starter.Main'
)
jar
.
manifest
.
attributes
(
'Main-Class'
:
'de.tudresden.inf.st.ros2rag.starter.
Starter
Main'
)
dependencies
{
dependencies
{
implementation
project
(
':ros2rag.base'
)
implementation
project
(
':ros2rag.base'
)
...
...
ros2rag.starter/src/main/jastadd/Definitions.ros2rag
View file @
8430dfc7
...
@@ -26,5 +26,7 @@ LinkStateToIntPosition maps panda.Linkstate.PandaLinkState pls to IntPosition {:
...
@@ -26,5 +26,7 @@ LinkStateToIntPosition maps panda.Linkstate.PandaLinkState pls to IntPosition {:
CreateSpeedMessage maps double speed to config.Robotconfig.RobotConfig {:
CreateSpeedMessage maps double speed to config.Robotconfig.RobotConfig {:
return config.Robotconfig.RobotConfig.newBuilder()
return config.Robotconfig.RobotConfig.newBuilder()
.setSpeed(speed)
.setSpeed(speed)
.setLoopTrajectory(true)
.setPlanningMode(config.Robotconfig.RobotConfig.PlanningMode.CARTESIAN)
.build();
.build();
:}
:}
ros2rag.starter/src/main/java/de/tudresden/inf/st/ros2rag/starter/Main.java
→
ros2rag.starter/src/main/java/de/tudresden/inf/st/ros2rag/starter/
Starter
Main.java
View file @
8430dfc7
package
de.tudresden.inf.st.ros2rag.starter
;
package
de.tudresden.inf.st.ros2rag.starter
;
import
com.fasterxml.jackson.databind.ObjectMapper
;
import
config.Dataconfig
;
import
de.tudresden.inf.st.ros2rag.starter.ast.*
;
import
de.tudresden.inf.st.ros2rag.starter.ast.*
;
import
de.tudresden.inf.st.ros2rag.starter.data.DataConfiguration
;
import
de.tudresden.inf.st.ros2rag.starter.data.DataJoint
;
import
org.apache.logging.log4j.LogManager
;
import
org.apache.logging.log4j.LogManager
;
import
org.apache.logging.log4j.Logger
;
import
org.apache.logging.log4j.Logger
;
import
java.io.File
;
import
java.io.IOException
;
import
java.io.IOException
;
import
java.util.concurrent.CountDownLatch
;
import
java.util.concurrent.CountDownLatch
;
import
java.util.concurrent.TimeUnit
;
import
java.util.concurrent.TimeUnit
;
...
@@ -13,47 +18,29 @@ import java.util.concurrent.TimeUnit;
...
@@ -13,47 +18,29 @@ import java.util.concurrent.TimeUnit;
*
*
* @author rschoene - Initial contribution
* @author rschoene - Initial contribution
*/
*/
public
class
Main
{
public
class
Starter
Main
{
private
static
final
Logger
logger
=
LogManager
.
getLogger
(
Main
.
class
);
private
static
final
Logger
logger
=
LogManager
.
getLogger
(
StarterMain
.
class
);
private
MqttUpdater
mainHandler
;
private
Model
model
;
public
static
void
main
(
String
[]
args
)
throws
IOException
,
InterruptedException
{
public
void
run
(
String
[]
args
)
throws
IOException
,
InterruptedException
{
// parse arguments
File
configFile
=
new
File
(
args
[
0
]);
final
String
mqttHost
;
if
(
args
.
length
>
0
)
{
mqttHost
=
args
[
0
];
}
else
{
mqttHost
=
"localhost"
;
}
/* assumed configuration:
panda_ground: ground-plate
panda_link_0: link_0 of the panda
panda_link_1: link_1 of the panda
panda_link_2: link_2 of the panda
panda_link_3: link_3 of the panda
panda_link_4: link_4 of the panda
panda_link_5: link_5 of the panda
panda_link_6: link_6 of the panda
panda_link_7: link_7 of the panda (end effector)
panda_link_8: link_8 of the panda (left finger)
panda_link_9: link_9 of the panda (right finger)
*/
final
int
numberOfJoints
=
10
;
final
int
endEffectorIndex
=
7
;
final
String
jointTopicFormat
=
"panda_link_%d"
;
final
String
configTopic
=
"robotconfig"
;
final
int
[][]
safetyZoneCoordinates
=
{
final
int
[][]
safetyZoneCoordinates
=
{
{
0
,
0
,
0
},
{
0
,
0
,
0
},
{-
1
,
0
,
0
},
{-
1
,
0
,
0
},
{
1
,
0
,
0
}
{
1
,
0
,
0
}
};
};
// --- No configuration below this line ---
// --- No configuration below this line ---
Model
model
=
new
Model
();
ObjectMapper
mapper
=
new
ObjectMapper
();
model
.
MqttSetHost
(
mqttHost
);
System
.
out
.
println
(
"Using config file: "
+
configFile
.
getAbsolutePath
());
DataConfiguration
config
=
mapper
.
readValue
(
configFile
,
DataConfiguration
.
class
);
model
=
new
Model
();
model
.
MqttSetHost
(
config
.
mqttHost
);
ZoneModel
zoneModel
=
new
ZoneModel
();
ZoneModel
zoneModel
=
new
ZoneModel
();
zoneModel
.
setSize
(
makePosition
(
1
,
1
,
1
));
zoneModel
.
setSize
(
makePosition
(
1
,
1
,
1
));
...
@@ -69,45 +56,58 @@ public class Main {
...
@@ -69,45 +56,58 @@ public class Main {
RobotArm
robotArm
=
new
RobotArm
();
RobotArm
robotArm
=
new
RobotArm
();
model
.
setRobotArm
(
robotArm
);
model
.
setRobotArm
(
robotArm
);
for
(
int
jointIndex
=
0
;
jointIndex
<
numberOfJoints
;
jointIndex
++
)
{
for
(
DataJoint
dataJoint
:
config
.
joints
)
{
final
Joint
jointOrEndEffector
;
final
Joint
jointOrEndEffector
;
if
(
jointIndex
==
e
ndEffector
Index
)
{
if
(
dataJoint
.
isE
ndEffector
)
{
EndEffector
endEffector
=
new
EndEffector
();
EndEffector
endEffector
=
new
EndEffector
();
endEffector
.
setName
(
"gripper"
);
robotArm
.
setEndEffector
(
endEffector
);
robotArm
.
setEndEffector
(
endEffector
);
jointOrEndEffector
=
endEffector
;
jointOrEndEffector
=
endEffector
;
}
else
{
}
else
{
Joint
joint
=
new
Joint
();
Joint
joint
=
new
Joint
();
joint
.
setName
(
"joint"
+
jointIndex
);
robotArm
.
addJoint
(
joint
);
robotArm
.
addJoint
(
joint
);
jointOrEndEffector
=
joint
;
jointOrEndEffector
=
joint
;
}
}
jointOrEndEffector
.
setName
(
dataJoint
.
name
);
jointOrEndEffector
.
setCurrentPosition
(
makePosition
(
0
,
0
,
0
));
jointOrEndEffector
.
setCurrentPosition
(
makePosition
(
0
,
0
,
0
));
robotArm
.
addDependency1
(
jointOrEndEffector
);
robotArm
.
addDependency1
(
jointOrEndEffector
);
jointOrEndEffector
.
connectCurrentPosition
(
String
.
format
(
j
oint
T
opic
Format
,
jointIndex
)
);
jointOrEndEffector
.
connectCurrentPosition
(
dataJ
oint
.
t
opic
);
}
}
robotArm
.
connectAppropriateSpeed
(
configTopic
,
true
);
robotArm
.
connectAppropriateSpeed
(
config
.
robotConfig
Topic
,
true
);
logStatus
(
"Start"
,
robotArm
);
logStatus
(
"Start"
,
robotArm
);
CountDownLatch
exitCondition
=
new
CountDownLatch
(
1
);
CountDownLatch
exitCondition
=
new
CountDownLatch
(
1
);
logger
.
info
(
"To print the current model states, send a message to the topic 'model'."
);
logger
.
info
(
"To print the current model states, send a message to the topic 'model'."
);
logger
.
info
(
"To exit the system cleanly, send a message to the topic 'exit'."
);
logger
.
info
(
"To exit the system cleanly, send a message to the topic 'exit'
, or use Ctrl+C
."
);
MqttUpdater
mainHandler
=
new
MqttUpdater
(
"mainHandler"
);
mainHandler
=
new
MqttUpdater
(
"mainHandler"
);
mainHandler
.
setHost
(
mqttHost
,
1883
);
mainHandler
.
setHost
(
config
.
mqttHost
);
mainHandler
.
waitUntilReady
(
2
,
TimeUnit
.
SECONDS
);
mainHandler
.
waitUntilReady
(
2
,
TimeUnit
.
SECONDS
);
mainHandler
.
newConnection
(
"exit"
,
bytes
->
exitCondition
.
countDown
());
mainHandler
.
newConnection
(
"exit"
,
bytes
->
exitCondition
.
countDown
());
mainHandler
.
newConnection
(
"model"
,
bytes
->
logStatus
(
new
String
(
bytes
),
robotArm
));
mainHandler
.
newConnection
(
"model"
,
bytes
->
logStatus
(
new
String
(
bytes
),
robotArm
));
sendInitialDataConfig
(
mainHandler
,
config
.
dataConfigTopic
);
Runtime
.
getRuntime
().
addShutdownHook
(
new
Thread
(
this
::
close
));
exitCondition
.
await
();
exitCondition
.
await
();
mainHandler
.
close
();
this
.
close
();
model
.
MqttCloseConnections
();
}
}
private
static
void
logStatus
(
String
prefix
,
RobotArm
robotArm
)
{
private
void
sendInitialDataConfig
(
MqttUpdater
mainHandler
,
String
dataConfigTopic
)
{
Dataconfig
.
DataConfig
dataConfig
=
Dataconfig
.
DataConfig
.
newBuilder
()
.
setEnablePosition
(
true
)
.
setEnableOrientation
(
false
)
.
setEnableTwistAngular
(
false
)
.
setEnableTwistLinear
(
false
)