Skip to content

Commit

Permalink
Update Detector object for sensor configuration file changes
Browse files Browse the repository at this point in the history
  • Loading branch information
@paulbourelly999
paulbourelly999 committed Aug 29, 2024
1 parent 9739e4c commit 8f8d2f7
Show file tree
Hide file tree
Showing 13 changed files with 437 additions and 205 deletions.
66 changes: 38 additions & 28 deletions ...saic-carla/src/main/java/org/eclipse/mosaic/fed/carla/carlaconnect/CarlaXmlRpcClient.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,21 +30,21 @@
import com.google.gson.Gson;

/**
* This is a class that uses xmlrpc to connect with CARLA CDASim Adapter. It includes calls
* This is a class that uses xmlrpc to connect with CARLA CDASim Adapter. It
* includes calls
* to dynamically create sensors and get detected objects from created sensors.
*/
public class CarlaXmlRpcClient{
public class CarlaXmlRpcClient {

private static final String CREATE_SENSOR = "create_simulated_semantic_lidar_sensor";
private static final String GET_DETECTED_OBJECTS = "get_detected_objects";
private static final String CONNECT ="connect";
private static final String CONNECT = "connect";

private XmlRpcClient client;
private final Logger log = LoggerFactory.getLogger(this.getClass());


public CarlaXmlRpcClient(URL xmlRpcServerUrl) {
XmlRpcClientConfigImpl config = new XmlRpcClientConfigImpl();
XmlRpcClientConfigImpl config = new XmlRpcClientConfigImpl();
config.setServerURL(xmlRpcServerUrl);
// Set reply and connection timeout (both in ms)
config.setReplyTimeout(6000);
Expand All @@ -53,56 +53,66 @@ public CarlaXmlRpcClient(URL xmlRpcServerUrl) {
client.setConfig(config);
}


public void connect(int retryAttempts) throws XmlRpcException, InterruptedException{
public void connect(int retryAttempts) throws XmlRpcException, InterruptedException {
boolean connected = false;
int currentAttempt = 1;
while( !connected && retryAttempts >= currentAttempt ) {
while (!connected && retryAttempts >= currentAttempt) {
try {
log.info("Attempting to connect to CARLA CDA Sim Adapter ... ");
Object[] params = new Object[]{};
Object[] params = new Object[] {};
client.execute(CONNECT, params);
connected = true;
}
catch(XmlRpcException e) {
} catch (XmlRpcException e) {
log.error("Connection attempt {} to connect to CARLA CDA Sim Adapter failed!", currentAttempt, e);
// Sleep for 1 second betweeen attempts
Thread.sleep(1000);
currentAttempt++;
}
}
}
if (!connected) {
throw new XmlRpcException("Failed to connect to XML RPC Server with config " + client.getConfig() + " !");
}
log.info("Connected successfully to CARLA CDA Sim Adapter!");
log.info("Connected successfully to CARLA CDA Sim Adapter!");
}

/**
* Calls CARLA CDA Sim Adapter create_sensor XMLRPC method and logs sensor ID of created sensor.
* Calls CARLA CDA Sim Adapter create_sensor XMLRPC method and logs sensor ID of
* created sensor.
*
* @param registration DetectorRegistration interaction used to create sensor.
* @throws XmlRpcException if XMLRPC call fails or connection is lost.
*/
public void createSensor(DetectorRegistration registration) throws XmlRpcException{
List<Double> location = Arrays.asList(registration.getDetector().getLocation().getX(), registration.getDetector().getLocation().getY(), registration.getDetector().getLocation().getZ());
List<Double> orientation = Arrays.asList(registration.getDetector().getOrientation().getPitch(), registration.getDetector().getOrientation().getRoll(), registration.getDetector().getOrientation().getYaw());
Object[] params = new Object[]{registration.getInfrastructureId(), registration.getDetector().getSensorId(), location, orientation};
public void createSensor(DetectorRegistration registration) throws XmlRpcException {
List<Double> location = Arrays.asList(registration.getDetector().getRef().getLocation().getX(),
registration.getDetector().getRef().getLocation().getY(),
registration.getDetector().getRef().getLocation().getZ());
List<Double> orientation = Arrays.asList(registration.getDetector().getRef().getOrientation().getPitch(),
registration.getDetector().getRef().getOrientation().getRoll(),
registration.getDetector().getRef().getOrientation().getYaw());
Object[] params = new Object[] { registration.getInfrastructureId(), registration.getDetector().getSensorId(),
location, orientation };
Object result = client.execute(CREATE_SENSOR, params);
log.info((String)result);
log.info((String) result);

}

/**
* Calls CARLA CDA Sim Adapter get_detected_objects XMLRPC method and returns an array of DetectedObject.
* @param infrastructureId String infrastructure ID of sensor to get detections from.
* @param sensorId String sensor ID of sensor to get detections from
* Calls CARLA CDA Sim Adapter get_detected_objects XMLRPC method and returns an
* array of DetectedObject.
*
* @param infrastructureId String infrastructure ID of sensor to get detections
* from.
* @param sensorId String sensor ID of sensor to get detections from
* @return DetectedObject[] from given sensor.
* @throws XmlRpcException if XMLRPC call fails or connection is lost.
*/
public DetectedObject[] getDetectedObjects(String infrastructureId ,String sensorId) throws XmlRpcException{
Object[] params = new Object[]{infrastructureId, sensorId};
public DetectedObject[] getDetectedObjects(String infrastructureId, String sensorId) throws XmlRpcException {
Object[] params = new Object[] { infrastructureId, sensorId };
Object result = client.execute(GET_DETECTED_OBJECTS, params);
log.debug("Detections from infrastructure {} sensor {} : {}", infrastructureId, sensorId, result);
String jsonResult = (String)result;
String jsonResult = (String) result;
Gson gson = new Gson();
return gson.fromJson(jsonResult,DetectedObject[].class);
return gson.fromJson(jsonResult, DetectedObject[].class);

}
}
109 changes: 66 additions & 43 deletions ...saic-carla/src/test/java/org/eclipse/mosaic/fed/carla/ambassador/CarlaAmbassadorTest.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,7 +26,9 @@
import org.eclipse.mosaic.lib.objects.detector.DetectedObject;
import org.eclipse.mosaic.lib.objects.detector.DetectionType;
import org.eclipse.mosaic.lib.objects.detector.Detector;
import org.eclipse.mosaic.lib.objects.detector.DetectorReferenceLocation;
import org.eclipse.mosaic.lib.objects.detector.DetectorType;
import org.eclipse.mosaic.lib.objects.detector.LocationDataType;
import org.eclipse.mosaic.lib.objects.detector.Orientation;
import org.eclipse.mosaic.lib.objects.detector.Size;
import org.eclipse.mosaic.lib.util.junit.TestFileRule;
Expand All @@ -53,9 +55,6 @@
import static org.mockito.Mockito.verify;
import static org.mockito.Mockito.when;




/**
* Tests for {@link CarlaAmbassador}.
*/
Expand All @@ -79,7 +78,7 @@ public void setup() throws IOException, NoSuchFieldException {

rtiMock = mock(RtiAmbassador.class);

carlaXmlRpcClientMock = mock(CarlaXmlRpcClient.class);
carlaXmlRpcClientMock = mock(CarlaXmlRpcClient.class);

FederateDescriptor handleMock = mock(FederateDescriptor.class);

Expand All @@ -100,33 +99,38 @@ public void setup() throws IOException, NoSuchFieldException {

ambassador.setFederateDescriptor(handleMock);

FieldSetter.setField(ambassador, ambassador.getClass().getDeclaredField("carlaXmlRpcClient"), carlaXmlRpcClientMock);


FieldSetter.setField(ambassador, ambassador.getClass().getDeclaredField("carlaXmlRpcClient"),
carlaXmlRpcClientMock);

}

@Test
public void initialize() throws Throwable {
CarlaConfiguration config = new CarlaConfiguration();
config.carlaCDASimAdapterUrl="https://testing/something";
config.carlaCDASimAdapterUrl = "https://testing/something";
FieldSetter.setField(ambassador, ambassador.getClass().getDeclaredField("carlaConfig"), config);

// RUN
ambassador.initialize(0, 100 * TIME.SECOND);
// ASSERT
verify(rtiMock, times(1)).requestAdvanceTime(eq(0L), eq(0L), eq((byte) 1));


}

@Test
public void processTimeAdvanceGrant() throws InternalFederateException, NoSuchFieldException, SecurityException, XmlRpcException, IllegalValueException {
public void processTimeAdvanceGrant() throws InternalFederateException, NoSuchFieldException, SecurityException,
XmlRpcException, IllegalValueException {
List<DetectorRegistration> registeredDetectors = new ArrayList<>();
Detector detector = new Detector("sensorID1", DetectorType.SEMANTIC_LIDAR, new Orientation( 0.0,0.0,0.0), CartesianPoint.ORIGO);
Detector detector = new Detector(
"sensorId",
DetectorType.SEMANTIC_LIDAR,
new DetectorReferenceLocation(LocationDataType.CARTESIAN,
CartesianPoint.xyz(0, 0, 0),
new Orientation(0, 0, 0)));
DetectorRegistration registration = new DetectorRegistration(0, detector, "rsu_2");
registeredDetectors.add( registration);
FieldSetter.setField(ambassador, ambassador.getClass().getDeclaredField("registeredDetectors"), registeredDetectors);
registeredDetectors.add(registration);
FieldSetter.setField(ambassador, ambassador.getClass().getDeclaredField("registeredDetectors"),
registeredDetectors);

// Setup Get detected objects return
// Object 1 is CAR
Expand All @@ -141,65 +145,81 @@ public void processTimeAdvanceGrant() throws InternalFederateException, NoSuchFi
new Vector3d(.1, .2, .3),
new Size(2, 1, .5),
100);
Double[][] covarianceMatrix = { {1.0, 0.0, 0.0} , {1.0, 0.0, 0.0} , {1.0, 0.0, 0.0}};
Double[][] covarianceMatrix = { { 1.0, 0.0, 0.0 }, { 1.0, 0.0, 0.0 }, { 1.0, 0.0, 0.0 } };
predictedCar.setPositionCovariance(covarianceMatrix);
predictedCar.setVelocityCovariance(covarianceMatrix);
predictedCar.setAngularVelocityCovariance(covarianceMatrix);
DetectedObject predictedBus = new DetectedObject(
DetectionType.BUS,
0.5,
"sensorID1",
"projection String",
101,
CartesianPoint.xyz(1.1, 2, 3.2),
new Vector3d(0, 0, 0),
new Vector3d(),
new Size(0, 0, 0),
100);
Double[][] bus_covarianceMatrix = { {0.0, 0.0, 0.0} , {0.0, 0.0, 0.0} , {0.0, 0.0, 0.0}};
DetectionType.BUS,
0.5,
"sensorID1",
"projection String",
101,
CartesianPoint.xyz(1.1, 2, 3.2),
new Vector3d(0, 0, 0),
new Vector3d(),
new Size(0, 0, 0),
100);
Double[][] bus_covarianceMatrix = { { 0.0, 0.0, 0.0 }, { 0.0, 0.0, 0.0 }, { 0.0, 0.0, 0.0 } };
predictedBus.setPositionCovariance(bus_covarianceMatrix);
predictedBus.setVelocityCovariance(bus_covarianceMatrix);
predictedBus.setAngularVelocityCovariance(bus_covarianceMatrix);

DetectedObject[] detectedObjects = {predictedBus, predictedCar};
when(carlaXmlRpcClientMock.getDetectedObjects(registration.getInfrastructureId(), registration.getDetector().getSensorId() )).thenReturn(detectedObjects);
// Set is simulation timestep to true
DetectedObject[] detectedObjects = { predictedBus, predictedCar };
when(carlaXmlRpcClientMock.getDetectedObjects(registration.getInfrastructureId(),
registration.getDetector().getSensorId())).thenReturn(detectedObjects);
// Set is simulation timestep to true
FieldSetter.setField(ambassador, ambassador.getClass().getDeclaredField("isSimulationStep"), true);

ambassador.processTimeAdvanceGrant(100);

verify(carlaXmlRpcClientMock, times(1)).getDetectedObjects(registration.getInfrastructureId(), registration.getDetector().getSensorId());
verify(carlaXmlRpcClientMock, times(1)).getDetectedObjects(registration.getInfrastructureId(),
registration.getDetector().getSensorId());
verify(rtiMock, times(2)).triggerInteraction(any(DetectedObjectInteraction.class));
}

@Test
public void processTimeAdvanceGrantException() throws InternalFederateException, NoSuchFieldException, SecurityException, XmlRpcException, IllegalValueException {
public void processTimeAdvanceGrantException() throws InternalFederateException, NoSuchFieldException,
SecurityException, XmlRpcException, IllegalValueException {
List<DetectorRegistration> registeredDetectors = new ArrayList<>();
Detector detector = new Detector("sensorID1", DetectorType.SEMANTIC_LIDAR, new Orientation( 0.0,0.0,0.0), CartesianPoint.ORIGO);
Detector detector = new Detector(
"sensorId",
DetectorType.SEMANTIC_LIDAR,
new DetectorReferenceLocation( LocationDataType.CARTESIAN,
CartesianPoint.xyz(0, 0, 0),
new Orientation(0, 0, 0)));
DetectorRegistration registration = new DetectorRegistration(0, detector, "rsu_2");
registeredDetectors.add( registration);
FieldSetter.setField(ambassador, ambassador.getClass().getDeclaredField("registeredDetectors"), registeredDetectors);
registeredDetectors.add(registration);
FieldSetter.setField(ambassador, ambassador.getClass().getDeclaredField("registeredDetectors"),
registeredDetectors);


when(carlaXmlRpcClientMock.getDetectedObjects(registration.getInfrastructureId(), registration.getDetector().getSensorId() )).thenThrow(XmlRpcException.class);
// Set is simulation timestep to true
when(carlaXmlRpcClientMock.getDetectedObjects(registration.getInfrastructureId(),
registration.getDetector().getSensorId())).thenThrow(XmlRpcException.class);
// Set is simulation timestep to true
FieldSetter.setField(ambassador, ambassador.getClass().getDeclaredField("isSimulationStep"), true);
// Verify that when exceptiopn is thrown by CarlaXmlRpcClient, no interactions are trigger and exception is caught
// Verify that when exceptiopn is thrown by CarlaXmlRpcClient, no interactions
// are trigger and exception is caught
try {
ambassador.processTimeAdvanceGrant(0);
}catch (Exception e) {
} catch (Exception e) {
assertEquals(InternalFederateException.class, e.getClass());
assertEquals(XmlRpcException.class, e.getCause().getClass());
}

verify(carlaXmlRpcClientMock, times(1)).getDetectedObjects(registration.getInfrastructureId(), registration.getDetector().getSensorId());
verify(carlaXmlRpcClientMock, times(1)).getDetectedObjects(registration.getInfrastructureId(),
registration.getDetector().getSensorId());
verify(rtiMock, times(0)).triggerInteraction(any(DetectedObjectInteraction.class));

}

@Test
public void processDetectorRegistrationInteraction() throws XmlRpcException {
Detector detector = new Detector("sensorID1", DetectorType.SEMANTIC_LIDAR, new Orientation( 0.0,0.0,0.0), CartesianPoint.ORIGO);
Detector detector = new Detector(
"sensorId",
DetectorType.SEMANTIC_LIDAR,
new DetectorReferenceLocation( LocationDataType.CARTESIAN,
CartesianPoint.xyz(0, 0, 0),
new Orientation(0, 0, 0)));
DetectorRegistration registration = new DetectorRegistration(0, detector, "rsu_2");

ambassador.processInteraction(registration);
Expand All @@ -210,7 +230,12 @@ public void processDetectorRegistrationInteraction() throws XmlRpcException {

@Test
public void processDetectorRegistrationInteractionException() throws XmlRpcException {
Detector detector = new Detector("sensorID1", DetectorType.SEMANTIC_LIDAR, new Orientation( 0.0,0.0,0.0), CartesianPoint.ORIGO);
Detector detector = new Detector(
"sensorId",
DetectorType.SEMANTIC_LIDAR,
new DetectorReferenceLocation( LocationDataType.CARTESIAN,
CartesianPoint.xyz(0, 0, 0),
new Orientation(0, 0, 0)));
DetectorRegistration registration = new DetectorRegistration(0, detector, "rsu_2");

doThrow(new XmlRpcException("")).when(carlaXmlRpcClientMock).createSensor(registration);
Expand All @@ -219,6 +244,4 @@ public void processDetectorRegistrationInteractionException() throws XmlRpcExcep
verify(carlaXmlRpcClientMock, times(1)).createSensor(registration);
}



}
13 changes: 10 additions & 3 deletions ...-carla/src/test/java/org/eclipse/mosaic/fed/carla/carlaconnect/CarlaXmlRpcClientTest.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,9 @@
import org.eclipse.mosaic.lib.objects.detector.DetectedObject;
import org.eclipse.mosaic.lib.objects.detector.DetectionType;
import org.eclipse.mosaic.lib.objects.detector.Detector;
import org.eclipse.mosaic.lib.objects.detector.DetectorReferenceLocation;
import org.eclipse.mosaic.lib.objects.detector.DetectorType;
import org.eclipse.mosaic.lib.objects.detector.LocationDataType;
import org.eclipse.mosaic.lib.objects.detector.Orientation;
import org.eclipse.mosaic.lib.objects.detector.Size;
import org.junit.Before;
Expand Down Expand Up @@ -60,11 +62,16 @@ public void setup() throws NoSuchFieldException, MalformedURLException, XmlRpcEx
@Test
public void testCreateSensor() throws XmlRpcException {
// Create Detector Registration
Detector detector = new Detector("sensorID1", DetectorType.SEMANTIC_LIDAR, new Orientation( 0.0,0.0,0.0), CartesianPoint.ORIGO);
Detector detector = new Detector(
"sensor1",
DetectorType.SEMANTIC_LIDAR,
new DetectorReferenceLocation( LocationDataType.CARTESIAN,
CartesianPoint.xyz(0, 0, 0),
new Orientation(0, 0, 0)));
DetectorRegistration registration = new DetectorRegistration(0, detector, "rsu_2");
// Create request params
List<Double> location = Arrays.asList(registration.getDetector().getLocation().getX(), registration.getDetector().getLocation().getY(), registration.getDetector().getLocation().getZ());
List<Double> orientation = Arrays.asList(registration.getDetector().getOrientation().getPitch(), registration.getDetector().getOrientation().getRoll(), registration.getDetector().getOrientation().getYaw());
List<Double> location = Arrays.asList(registration.getDetector().getRef().getLocation().getX(), registration.getDetector().getRef().getLocation().getY(), registration.getDetector().getRef().getLocation().getZ());
List<Double> orientation = Arrays.asList(registration.getDetector().getRef().getOrientation().getPitch(), registration.getDetector().getRef().getOrientation().getRoll(), registration.getDetector().getRef().getOrientation().getYaw());
Object[] params = new Object[]{registration.getInfrastructureId(), registration.getDetector().getSensorId(), location, orientation};
// Tell mock to return sensor ID when following method is called with following parameters
when( mockClient.execute("create_simulated_semantic_lidar_sensor", params)).thenReturn(registration.getSenderId());
Expand Down
Loading

0 comments on commit 8f8d2f7

Please sign in to comment.