Skip to content

Commit 8d664da

Browse files
gloriacai01gloriacai01
authored
RSDK-8714: Posetracker wrappers (#65)
1 parent a1716af commit 8d664da

File tree

7 files changed

+369
-0
lines changed

7 files changed

+369
-0
lines changed

‎core/sdk/src/main/java/com/viam/sdk/core/component/posetracker/PoseTracker.java

+67
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,67 @@
1+
package com.viam.sdk.core.component.posetracker;
2+
3+
import com.google.protobuf.Struct;
4+
import com.viam.common.v1.Common;
5+
import com.viam.sdk.core.component.Component;
6+
import com.viam.sdk.core.resource.Resource;
7+
import com.viam.sdk.core.resource.Subtype;
8+
import com.viam.sdk.core.robot.RobotClient;
9+
10+
import java.util.List;
11+
import java.util.Map;
12+
13+
/**
14+
* PoseTracker represents a physical pose or motion tracking device.
15+
*/
16+
public abstract class PoseTracker extends Component {
17+
18+
public static final Subtype SUBTYPE = new Subtype(
19+
Subtype.NAMESPACE_RDK,
20+
Subtype.RESOURCE_TYPE_COMPONENT,
21+
"poseTracker");
22+
23+
public PoseTracker(final String name) {
24+
super(SUBTYPE, named(name));
25+
}
26+
27+
/**
28+
* Get the ResourceName of the component
29+
*
30+
* @param name the name of the component
31+
* @return the component's ResourceName
32+
*/
33+
public static Common.ResourceName named(final String name) {
34+
return Resource.named(SUBTYPE, name);
35+
}
36+
37+
/**
38+
* Get the component with the provided name from the provided robot.
39+
* @param robot the RobotClient
40+
* @param name the name of the component
41+
* @return the component
42+
*/
43+
public static PoseTracker fromRobot(final RobotClient robot, final String name) {
44+
return robot.getResource(PoseTracker.class, named(name));
45+
}
46+
47+
/**
48+
* Returns the current pose of each body tracked by the pose tracker.
49+
* @param bodyNames Names of the bodies whose poses are being requested. In the event this parameter is not supplied or is
50+
* an empty list, all available poses are returned.
51+
* @return the mapping of each body name to the pose representing the center of the body
52+
*/
53+
public abstract Map<String, Common.PoseInFrame> getPoses(List<String> bodyNames, Struct extra);
54+
55+
56+
/**
57+
* Returns the current pose of each body tracked by the pose tracker.
58+
* @param bodyNames Names of the bodies whose poses are being requested. In the event this parameter is not supplied or is
59+
* an empty list, all available poses are returned.
60+
* @return the mapping of each body name to the pose representing the center of the body
61+
*/
62+
public Map<String, Common.PoseInFrame> getPoses(List<String> bodyNames){
63+
return getPoses(bodyNames,Struct.getDefaultInstance());
64+
}
65+
66+
67+
}

‎core/sdk/src/main/java/com/viam/sdk/core/component/posetracker/PoseTrackerRPCClient.java

+48
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,48 @@
1+
package com.viam.sdk.core.component.posetracker;
2+
3+
import com.google.protobuf.Struct;
4+
import com.google.protobuf.Value;
5+
import com.viam.common.v1.Common;
6+
import com.viam.component.v1.PoseTracker.GetPosesRequest;
7+
import com.viam.component.v1.PoseTrackerServiceGrpc;
8+
import com.viam.sdk.core.rpc.Channel;
9+
10+
import java.util.List;
11+
import java.util.Map;
12+
import java.util.Optional;
13+
14+
public class PoseTrackerRPCClient extends PoseTracker {
15+
private final PoseTrackerServiceGrpc.PoseTrackerServiceBlockingStub client;
16+
17+
public PoseTrackerRPCClient(final String name, final Channel chan) {
18+
super(name);
19+
final PoseTrackerServiceGrpc.PoseTrackerServiceBlockingStub client = PoseTrackerServiceGrpc.newBlockingStub(chan);
20+
if (chan.getCallCredentials().isPresent()) {
21+
this.client = client.withCallCredentials(chan.getCallCredentials().get());
22+
} else {
23+
this.client = client;
24+
}
25+
}
26+
27+
@Override
28+
public Struct doCommand(final Map<String, Value> command) {
29+
return client.doCommand(Common.DoCommandRequest.newBuilder().
30+
setName(getName().getName()).
31+
setCommand(Struct.newBuilder().putAllFields(command).build()).
32+
build()).getResult();
33+
}
34+
35+
@Override
36+
public Map<String, Common.PoseInFrame> getPoses(List<String> bodyNames, Struct extra) {
37+
final GetPosesRequest request = GetPosesRequest.newBuilder().setName(getName().getName()).setExtra(extra).addAllBodyNames(bodyNames).build();
38+
return client.getPoses(request).getBodyPosesMap();
39+
}
40+
41+
@Override
42+
public List<Common.Geometry> getGeometries(Optional<Struct> extra) {
43+
final Common.GetGeometriesRequest.Builder builder = Common.GetGeometriesRequest.newBuilder().
44+
setName(getName().getName());
45+
extra.ifPresent(builder::setExtra);
46+
return client.getGeometries(builder.build()).getGeometriesList();
47+
}
48+
}

‎core/sdk/src/main/java/com/viam/sdk/core/component/posetracker/PoseTrackerRPCService.java

+62
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,62 @@
1+
package com.viam.sdk.core.component.posetracker;
2+
3+
import com.google.protobuf.Struct;
4+
import com.viam.common.v1.Common;
5+
import com.viam.component.v1.PoseTrackerServiceGrpc;
6+
import com.viam.sdk.core.resource.ResourceManager;
7+
import com.viam.sdk.core.resource.ResourceRPCService;
8+
import io.grpc.stub.StreamObserver;
9+
import com.viam.sdk.core.component.posetracker.*;
10+
11+
import java.util.List;
12+
import java.util.Map;
13+
import java.util.Optional;
14+
15+
public class PoseTrackerRPCService extends PoseTrackerServiceGrpc.PoseTrackerServiceImplBase
16+
implements ResourceRPCService<PoseTracker> {
17+
18+
private final ResourceManager manager;
19+
20+
public PoseTrackerRPCService(final ResourceManager manager) {
21+
this.manager = manager;
22+
}
23+
24+
@Override
25+
public void getPoses(com.viam.component.v1.PoseTracker.GetPosesRequest request, StreamObserver<com.viam.component.v1.PoseTracker.GetPosesResponse> responseObserver) {
26+
final PoseTracker poseTracker = getResource(PoseTracker.named(request.getName()));
27+
final Map<String, Common.PoseInFrame> result = poseTracker.getPoses(request.getBodyNamesList(), request.getExtra());
28+
responseObserver.onNext(com.viam.component.v1.PoseTracker.GetPosesResponse.newBuilder().putAllBodyPoses(result).build());
29+
responseObserver.onCompleted();
30+
}
31+
32+
@Override
33+
public void doCommand(Common.DoCommandRequest request,
34+
StreamObserver<Common.DoCommandResponse> responseObserver) {
35+
final PoseTracker poseTracker = getResource(
36+
PoseTracker.named(request.getName())
37+
);
38+
final Struct result = poseTracker.doCommand(request.getCommand().getFieldsMap());
39+
responseObserver.onNext(Common.DoCommandResponse.newBuilder().setResult(result).build());
40+
responseObserver.onCompleted();
41+
}
42+
43+
@Override
44+
public void getGeometries(Common.GetGeometriesRequest request, StreamObserver<Common.GetGeometriesResponse> responseObserver) {
45+
final PoseTracker poseTracker = getResource(
46+
PoseTracker.named(request.getName()));
47+
final List<Common.Geometry> result = poseTracker.getGeometries(Optional.of(request.getExtra()));
48+
responseObserver.onNext(Common.GetGeometriesResponse.newBuilder().addAllGeometries(result).build());
49+
responseObserver.onCompleted();
50+
}
51+
52+
@Override
53+
public Class<PoseTracker> getResourceClass() {
54+
return PoseTracker.class;
55+
}
56+
57+
@Override
58+
public ResourceManager getManager() {
59+
return manager;
60+
}
61+
62+
}

‎core/sdk/src/main/java/com/viam/sdk/core/resource/ResourceManager.java

+8
Original file line numberDiff line numberDiff line change
@@ -14,6 +14,7 @@
1414
import com.viam.component.movementsensor.v1.MovementSensorServiceGrpc;
1515
import com.viam.component.powersensor.v1.PowerSensorServiceGrpc;
1616
import com.viam.component.sensor.v1.SensorServiceGrpc;
17+
import com.viam.component.v1.PoseTrackerServiceGrpc;
1718
import com.viam.sdk.core.component.base.*;
1819
import com.viam.sdk.core.component.arm.*;
1920
import com.viam.component.servo.v1.ServoServiceGrpc;
@@ -39,6 +40,7 @@
3940
import com.viam.sdk.core.component.movementsensor.MovementSensor;
4041
import com.viam.sdk.core.component.movementsensor.MovementSensorRPCClient;
4142
import com.viam.sdk.core.component.movementsensor.MovementSensorRPCService;
43+
import com.viam.sdk.core.component.posetracker.*;
4244
import com.viam.sdk.core.component.powersensor.PowerSensor;
4345
import com.viam.sdk.core.component.powersensor.PowerSensorRPCClient;
4446
import com.viam.sdk.core.component.powersensor.PowerSensorRPCService;
@@ -131,6 +133,12 @@ public class ResourceManager implements Closeable {
131133
MovementSensorRPCService::new,
132134
MovementSensorRPCClient::new
133135
));
136+
Registry.registerSubtype(new ResourceRegistration<>(
137+
PoseTracker.SUBTYPE,
138+
PoseTrackerServiceGrpc.SERVICE_NAME,
139+
PoseTrackerRPCService::new,
140+
PoseTrackerRPCClient::new
141+
));
134142
Registry.registerSubtype(new ResourceRegistration<>(
135143
PowerSensor.SUBTYPE,
136144
PowerSensorServiceGrpc.SERVICE_NAME,

‎core/sdk/src/test/kotlin/com/viam/sdk/core/component/posetracker/PoseTrackerRPCClientTest.kt

+73
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,73 @@
1+
package com.viam.sdk.core.component.posetracker
2+
3+
import com.google.protobuf.Struct
4+
import com.google.protobuf.Value
5+
import com.viam.common.v1.Common
6+
import com.viam.common.v1.Common.Geometry
7+
import com.viam.common.v1.Common.PoseInFrame
8+
import com.viam.sdk.core.component.posetracker.*;
9+
import com.viam.sdk.core.resource.ResourceManager
10+
import com.viam.sdk.core.rpc.BasicManagedChannel
11+
import io.grpc.inprocess.InProcessChannelBuilder
12+
import io.grpc.inprocess.InProcessServerBuilder
13+
import io.grpc.testing.GrpcCleanupRule
14+
import org.junit.Rule
15+
import org.junit.jupiter.api.Assertions.assertEquals
16+
import org.junit.jupiter.api.BeforeEach
17+
import org.junit.jupiter.api.Test
18+
import org.mockito.Mockito.*
19+
import java.util.*
20+
21+
class PoseTrackerRPCClientTest {
22+
private lateinit var poseTracker: PoseTracker
23+
private lateinit var client: PoseTrackerRPCClient
24+
25+
@JvmField
26+
@Rule
27+
val grpcCleanupRule: GrpcCleanupRule = GrpcCleanupRule()
28+
29+
@BeforeEach
30+
fun setup() {
31+
poseTracker = mock(
32+
PoseTracker::class.java, withSettings().useConstructor("mock-poseTracker").defaultAnswer(
33+
CALLS_REAL_METHODS
34+
)
35+
)
36+
val resourceManager = ResourceManager(listOf(poseTracker))
37+
val service = PoseTrackerRPCService(resourceManager)
38+
val serviceName = InProcessServerBuilder.generateName()
39+
grpcCleanupRule.register(
40+
InProcessServerBuilder.forName(serviceName).directExecutor().addService(service).build().start()
41+
)
42+
val channel = grpcCleanupRule.register(InProcessChannelBuilder.forName(serviceName).directExecutor().build())
43+
client = PoseTrackerRPCClient("mock-poseTracker", BasicManagedChannel(channel))
44+
}
45+
46+
@Test
47+
fun getPoses(){
48+
val bodyNames = listOf("a", "b")
49+
val pose = Common.Pose.newBuilder().setX(1.0).setY(1.0).setZ(1.0).setOX(2.0).setOY(2.0).setOZ(2.0).setTheta(3.0).build()
50+
val poseFrames = mapOf("a" to PoseInFrame.newBuilder().setPose(pose).setReferenceFrame("0").build(),
51+
"b" to PoseInFrame.newBuilder().setPose(pose).setReferenceFrame("1").build())
52+
`when`(poseTracker.getPoses(eq(bodyNames), any(Struct::class.java) ?: Struct.getDefaultInstance())).thenReturn(poseFrames)
53+
val res = client.getPoses(bodyNames)
54+
verify(poseTracker).getPoses(bodyNames, Struct.getDefaultInstance())
55+
assertEquals(poseFrames, res)
56+
}
57+
58+
@Test
59+
fun doCommand() {
60+
val command = mapOf("foo" to Value.newBuilder().setStringValue("bar").build())
61+
doReturn(Struct.newBuilder().putAllFields(command).build()).`when`(poseTracker).doCommand(anyMap())
62+
val response = client.doCommand(command)
63+
verify(poseTracker).doCommand(command)
64+
assertEquals(command, response.fieldsMap)
65+
}
66+
67+
@Test
68+
fun getGeometries() {
69+
doReturn(listOf<Geometry>()).`when`(poseTracker).getGeometries(any())
70+
client.getGeometries(Optional.empty())
71+
verify(poseTracker).getGeometries(any())
72+
}
73+
}

‎core/sdk/src/test/kotlin/com/viam/sdk/core/component/posetracker/PoseTrackerRPCServiceTest.kt

+80
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,80 @@
1+
package com.viam.sdk.core.component.posetracker
2+
3+
import com.google.protobuf.Struct
4+
import com.google.protobuf.Value
5+
import com.viam.common.v1.Common
6+
import com.viam.common.v1.Common.Geometry
7+
import com.viam.common.v1.Common.PoseInFrame
8+
import com.viam.component.v1.PoseTrackerServiceGrpc
9+
import com.viam.component.v1.PoseTrackerServiceGrpc.PoseTrackerServiceBlockingStub
10+
import com.viam.sdk.core.resource.ResourceManager
11+
import io.grpc.inprocess.InProcessChannelBuilder
12+
import io.grpc.inprocess.InProcessServerBuilder
13+
import io.grpc.testing.GrpcCleanupRule
14+
import org.junit.Rule
15+
import org.junit.jupiter.api.Assertions.assertEquals
16+
import org.junit.jupiter.api.BeforeEach
17+
import org.junit.jupiter.api.Test
18+
import org.mockito.Mockito.*
19+
import java.util.*
20+
21+
class PoseTrackerRPCServiceTest {
22+
private lateinit var poseTracker: PoseTracker
23+
private lateinit var client: PoseTrackerServiceBlockingStub
24+
25+
@JvmField
26+
@Rule
27+
val grpcCleanupRule: GrpcCleanupRule = GrpcCleanupRule()
28+
29+
@BeforeEach
30+
fun setup() {
31+
poseTracker = mock(
32+
PoseTracker::class.java, withSettings().useConstructor("mock-poseTracker").defaultAnswer(
33+
CALLS_REAL_METHODS
34+
)
35+
)
36+
37+
val resourceManager = ResourceManager(listOf(poseTracker))
38+
val service = PoseTrackerRPCService(resourceManager)
39+
val serviceName = InProcessServerBuilder.generateName()
40+
grpcCleanupRule.register(
41+
InProcessServerBuilder.forName(serviceName).directExecutor().addService(service).build().start()
42+
)
43+
client = PoseTrackerServiceGrpc.newBlockingStub(
44+
grpcCleanupRule.register(
45+
InProcessChannelBuilder.forName(serviceName).build()
46+
)
47+
)
48+
}
49+
50+
@Test
51+
fun getPoses(){
52+
val bodyNames = listOf("a", "b")
53+
val pose = Common.Pose.newBuilder().setX(1.0).setY(1.0).setZ(1.0).setOX(2.0).setOY(2.0).setOZ(2.0).setTheta(3.0).build()
54+
val poseFrames = mapOf("a" to PoseInFrame.newBuilder().setPose(pose).setReferenceFrame("0").build(),
55+
"b" to PoseInFrame.newBuilder().setPose(pose).setReferenceFrame("1").build())
56+
`when`(poseTracker.getPoses(eq(bodyNames), any(Struct::class.java) ?: Struct.getDefaultInstance())).thenReturn(poseFrames)
57+
val request = com.viam.component.v1.PoseTracker.GetPosesRequest.newBuilder().setName(poseTracker.name.name).addAllBodyNames(bodyNames).build()
58+
val res = client.getPoses(request)
59+
verify(poseTracker).getPoses(bodyNames, Struct.getDefaultInstance())
60+
assertEquals(poseFrames, res.bodyPosesMap)
61+
}
62+
@Test
63+
fun doCommand() {
64+
val command =
65+
Struct.newBuilder().putAllFields(mapOf("foo" to Value.newBuilder().setStringValue("bar").build())).build()
66+
doReturn(command).`when`(poseTracker).doCommand(anyMap())
67+
val request = Common.DoCommandRequest.newBuilder().setName(poseTracker.name.name).setCommand(command).build()
68+
val response = client.doCommand(request)
69+
verify(poseTracker).doCommand(command.fieldsMap)
70+
assertEquals(command, response.result)
71+
}
72+
73+
@Test
74+
fun getGeometries() {
75+
doReturn(listOf<Geometry>()).`when`(poseTracker).getGeometries(any())
76+
val request = Common.GetGeometriesRequest.newBuilder().setName(poseTracker.name.name).build()
77+
client.getGeometries(request)
78+
verify(poseTracker).getGeometries(Optional.of(Struct.getDefaultInstance()))
79+
}
80+
}

‎core/sdk/src/test/kotlin/com/viam/sdk/core/component/posetracker/PoseTrackerTest.kt

+31
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,31 @@
1+
package com.viam.sdk.core.component.posetracker
2+
3+
import com.google.protobuf.Struct
4+
import com.viam.common.v1.Common
5+
import com.viam.common.v1.Common.PoseInFrame
6+
import org.junit.jupiter.api.Assertions.assertEquals
7+
import org.junit.jupiter.api.BeforeEach
8+
import org.junit.jupiter.api.Test
9+
import org.mockito.Answers
10+
import org.mockito.Mockito.*
11+
12+
class PoseTrackerTest {
13+
private lateinit var poseTracker: PoseTracker
14+
15+
@BeforeEach
16+
fun setup() {
17+
poseTracker = mock(PoseTracker::class.java, Answers.CALLS_REAL_METHODS)
18+
}
19+
20+
@Test
21+
fun getPoses(){
22+
val bodyNames = listOf("a", "b")
23+
val pose = Common.Pose.newBuilder().setX(1.0).setY(1.0).setZ(1.0).setOX(2.0).setOY(2.0).setOZ(2.0).setTheta(3.0).build()
24+
val poseFrames = mapOf("a" to PoseInFrame.newBuilder().setPose(pose).setReferenceFrame("0").build(),
25+
"b" to PoseInFrame.newBuilder().setPose(pose).setReferenceFrame("1").build())
26+
`when`(poseTracker.getPoses(eq(bodyNames), any(Struct::class.java) ?: Struct.getDefaultInstance())).thenReturn(poseFrames)
27+
val res = poseTracker.getPoses(bodyNames)
28+
verify(poseTracker).getPoses(bodyNames, Struct.getDefaultInstance())
29+
assertEquals(poseFrames, res)
30+
}
31+
}

0 commit comments

Comments
 (0)