void testConvexHullProjectionConstraint()
{
// Check that given an "easy" problem the constraint works fine
iDynTree::ConvexHullProjectionConstraint projectionConstraint;
// Add a nice big square convex hull
iDynTree::Polygon notProjectedConvexHull;
notProjectedConvexHull.m_vertices.push_back(iDynTree::Position( 1.0, 1.0, 5.0));
notProjectedConvexHull.m_vertices.push_back(iDynTree::Position(-1.0, 1.0, 10.0));
notProjectedConvexHull.m_vertices.push_back(iDynTree::Position(-1.0, -1.0, -10.0));
notProjectedConvexHull.m_vertices.push_back(iDynTree::Position( 1.0, -1.0, -15.0));
std::vector<iDynTree::Polygon> polygons;
polygons.resize(1);
polygons[0] = notProjectedConvexHull;
// Add a smaller polygon inside the other one
iDynTree::Polygon notProjectedConvexHullSmall;
notProjectedConvexHullSmall.m_vertices.push_back(iDynTree::Position( 0.5, 0.5, 5.0));
notProjectedConvexHullSmall.m_vertices.push_back(iDynTree::Position(-0.5, 0.5, 10.0));
notProjectedConvexHullSmall.m_vertices.push_back(iDynTree::Position(-0.5, -0.5, -10.0));
notProjectedConvexHullSmall.m_vertices.push_back(iDynTree::Position( 0.5, -0.5, -15.0));
polygons.push_back(notProjectedConvexHullSmall);
std::vector<iDynTree::Transform> transforms;
transforms.push_back(iDynTree::Transform::Identity());
transforms.push_back(iDynTree::Transform::Identity());
iDynTree::Direction xAxis(1.0, 0.0, 0.0);
iDynTree::Direction yAxis(0.0, 1.0, 0.0);
bool ok = projectionConstraint.buildConvexHull(xAxis,yAxis,iDynTree::Position::Zero(),polygons,transforms);
ASSERT_IS_TRUE(ok);
ASSERT_IS_TRUE(projectionConstraint.projectedConvexHull.getNrOfVertices() == 4);
// First point
ASSERT_EQUAL_DOUBLE(projectionConstraint.projectedConvexHull(0)(0), -1.0);
ASSERT_EQUAL_DOUBLE(projectionConstraint.projectedConvexHull(0)(1), -1.0);
// Second point
ASSERT_EQUAL_DOUBLE(projectionConstraint.projectedConvexHull(1)(0), 1.0);
ASSERT_EQUAL_DOUBLE(projectionConstraint.projectedConvexHull(1)(1),-1.0);
// Third point
ASSERT_EQUAL_DOUBLE(projectionConstraint.projectedConvexHull(2)(0), 1.0);
ASSERT_EQUAL_DOUBLE(projectionConstraint.projectedConvexHull(2)(1), 1.0);
// Fourth point
ASSERT_EQUAL_DOUBLE(projectionConstraint.projectedConvexHull(3)(0),-1.0);
ASSERT_EQUAL_DOUBLE(projectionConstraint.projectedConvexHull(3)(1), 1.0);
}
void threeLinksReducedTest()
{
// Check visualizer of simple model
iDynTree::ModelLoader mdlLoader, mdlLoaderReduced;
// Load full model
bool ok = mdlLoader.loadModelFromFile(getAbsModelPath("threeLinks.urdf"));
ASSERT_IS_TRUE(ok);
// Check visualization of full model
checkVizLoading(mdlLoader.model());
// Load reduced model
std::vector<std::string> consideredJoints;
consideredJoints.push_back("joint_2_3");
ok = mdlLoaderReduced.loadReducedModelFromFullModel(mdlLoader.model(),consideredJoints);
ASSERT_IS_TRUE(ok);
// Check vizualization for reduced model
checkVizLoading(mdlLoaderReduced.model());
}
void checkVizLoading(const iDynTree::Model & model)
{
// Open visualizer
iDynTree::Visualizer viz;
bool ok = viz.addModel(model,"model");
ASSERT_IS_TRUE(ok);
for(int i=0; i < 5; i++)
{
viz.draw();
}
viz.close();
}
std::string BasicTestF::execute()
{
std::string value = "";
bool a=true;
std::string term = ASSERT_IS_TRUE(a);
if(term == "OK")
{
value = "OK";
}
else
{
value += term;
}
return value;
}
void testConvexHullProjectionWithGravity()
{
// For this test, we will use a simple convex hull of side 1 meter
// centered in the origin of the world and laying on the XY plane
iDynTree::ConvexHullProjectionConstraint projectionConstraint;
iDynTree::Polygon convexHull;
convexHull.m_vertices.push_back(iDynTree::Position( 0.5, 0.5, 0.0));
convexHull.m_vertices.push_back(iDynTree::Position(-0.5, 0.5, 0.0));
convexHull.m_vertices.push_back(iDynTree::Position(-0.5, -0.5, 0.0));
convexHull.m_vertices.push_back(iDynTree::Position( 0.5, -0.5, 0.0));
std::vector<iDynTree::Polygon> polygons;
polygons.resize(1);
polygons[0] = convexHull;
// The convex hull is already expressed in the world
std::vector<iDynTree::Transform> transforms;
transforms.push_back(iDynTree::Transform::Identity());
iDynTree::Direction xAxis(1.0, 0.0, 0.0);
iDynTree::Direction yAxis(0.0, 1.0, 0.0);
bool ok = projectionConstraint.buildConvexHull(xAxis,yAxis,iDynTree::Position::Zero(),polygons,transforms);
ASSERT_IS_TRUE(ok);
// Use a test position that is outside the convex hull with the normal projection
iDynTree::Position testPoint(1.0, 0.0, 1.0);
// Projected along the normal of the plane, this point is outside the convex hull
// Note: positive margin means inside, negative outside
ASSERT_IS_TRUE(projectionConstraint.computeMargin(projectionConstraint.project(testPoint)) < 0);
//----------------------------------------------------------------------------------------------
// If the direction along which we are projecting is the normal of the plane, the projection
// should match the one done without direction
iDynTree::Direction planeNormal(0.0, 0.0, -1.0);
projectionConstraint.setProjectionAlongDirection(planeNormal);
ASSERT_EQUAL_VECTOR(projectionConstraint.project(testPoint), projectionConstraint.projectAlongDirection(testPoint));
//----------------------------------------------------------------------------------------------
// If the direction of the projection is "skewed" to the left, the point should instead be inside the convex hull
iDynTree::Direction skewedDirection(-1.0, 0.0, -1.0);
projectionConstraint.setProjectionAlongDirection(skewedDirection);
std::cerr << projectionConstraint.computeMargin(projectionConstraint.projectAlongDirection(testPoint)) << std::endl;
ASSERT_IS_TRUE(projectionConstraint.computeMargin(projectionConstraint.projectAlongDirection(testPoint)) > 0);
//----------------------------------------------------------------------------------------------
// The code should work even if I try to project in the opposite direction wrt 'planeNormal'
iDynTree::Direction upwardDirection(0.0, 0.0, 1.0);
projectionConstraint.setProjectionAlongDirection(upwardDirection);
// I should obtain the same result as the orthogonal projection
ASSERT_EQUAL_VECTOR(projectionConstraint.project(testPoint), projectionConstraint.projectAlongDirection(testPoint));
// The projected point should be again outside the convex hull
ASSERT_IS_TRUE(projectionConstraint.computeMargin(projectionConstraint.projectAlongDirection(testPoint))< 0);
}
static void RecvMessage(IOTHUB_PROVISIONED_DEVICE* deviceToUse)
{
// arrange
IOTHUB_CLIENT_HANDLE iotHubClientHandle;
IOTHUB_SERVICE_CLIENT_AUTH_HANDLE iotHubServiceClientHandle;
IOTHUB_MESSAGING_CLIENT_HANDLE iotHubMessagingHandle;
IOTHUB_MESSAGING_RESULT iotHubMessagingResult;
IOTHUB_MESSAGE_RESULT iotHubMessageResult;
EXPECTED_RECEIVE_DATA* receiveUserContext;
IOTHUB_MESSAGE_HANDLE messageHandle;
// act
IoTHub_Init();
// Create Service Client
iotHubServiceClientHandle = IoTHubServiceClientAuth_CreateFromConnectionString(IoTHubAccount_GetIoTHubConnString(g_iothubAcctInfo1));
ASSERT_IS_NOT_NULL(iotHubServiceClientHandle, "Could not initialize IoTHubServiceClient to send C2D messages to the device");
iotHubMessagingHandle = IoTHubMessaging_Create(iotHubServiceClientHandle);
ASSERT_IS_NOT_NULL(iotHubMessagingHandle, "Could not initialize IoTHubMessaging to send C2D messages to the device");
iotHubMessagingResult = IoTHubMessaging_Open(iotHubMessagingHandle, openCompleteCallback, (void*)"Context string for open");
ASSERT_ARE_EQUAL(int, IOTHUB_MESSAGING_OK, iotHubMessagingResult);
// Create user context and message
receiveUserContext = ReceiveUserContext_Create();
ASSERT_IS_NOT_NULL(receiveUserContext, "Could not create receive user context");
messageHandle = IoTHubMessage_CreateFromString(MSG_CONTENT1);
ASSERT_IS_NOT_NULL(messageHandle, "Could not create IoTHubMessage to send C2D messages to the device");
iotHubMessageResult = IoTHubMessage_SetMessageId(messageHandle, MSG_ID1);
ASSERT_ARE_EQUAL(int, IOTHUB_MESSAGING_OK, iotHubMessageResult);
iotHubMessageResult = IoTHubMessage_SetCorrelationId(messageHandle, MSG_CORRELATION_ID1);
ASSERT_ARE_EQUAL(int, IOTHUB_MESSAGING_OK, iotHubMessageResult);
MAP_HANDLE mapHandle = IoTHubMessage_Properties(messageHandle);
for (size_t i = 0; i < MSG_PROP_COUNT; i++)
{
if (Map_AddOrUpdate(mapHandle, MSG_PROP_KEYS[i], MSG_PROP_VALS[i]) != MAP_OK)
{
(void)printf("ERROR: Map_AddOrUpdate failed for property %zu!\r\n", i);
}
}
iotHubMessagingResult = IoTHubMessaging_SendAsync(iotHubMessagingHandle, deviceToUse->deviceId, messageHandle, sendCompleteCallback, receiveUserContext);
ASSERT_ARE_EQUAL(int, IOTHUB_MESSAGING_OK, iotHubMessagingResult, "IoTHubMessaging_SendAsync failed, could not send C2D message to the device");
iotHubClientHandle = IoTHubClient_CreateFromConnectionString(deviceToUse->connectionString, HTTP_Protocol);
ASSERT_IS_NOT_NULL(iotHubClientHandle, "Failure creating Iothub Client");
if (deviceToUse->howToCreate == IOTHUB_ACCOUNT_AUTH_X509) {
IOTHUB_CLIENT_RESULT result;
result = IoTHubClient_SetOption(iotHubClientHandle, OPTION_X509_CERT, deviceToUse->certificate);
ASSERT_ARE_EQUAL(IOTHUB_CLIENT_RESULT, IOTHUB_CLIENT_OK, result, "Could not set the device x509 certificate");
result = IoTHubClient_SetOption(iotHubClientHandle, OPTION_X509_PRIVATE_KEY, deviceToUse->primaryAuthentication);
ASSERT_ARE_EQUAL(IOTHUB_CLIENT_RESULT, IOTHUB_CLIENT_OK, result, "Could not set the device x509 privateKey");
}
IOTHUB_CLIENT_RESULT result = IoTHubClient_SetMessageCallback(iotHubClientHandle, ReceiveMessageCallback, receiveUserContext);
ASSERT_ARE_EQUAL(IOTHUB_CLIENT_RESULT, IOTHUB_CLIENT_OK, result, "Failure setting message callback");
unsigned int minimumPollingTime = 1; /*because it should not wait*/
if (IoTHubClient_SetOption(iotHubClientHandle, OPTION_MIN_POLLING_TIME, &minimumPollingTime) != IOTHUB_CLIENT_OK)
{
printf("failure to set option \"MinimumPollingTime\"\r\n");
}
time_t beginOperation, nowTime;
beginOperation = time(NULL);
while (
(
(nowTime = time(NULL)),
(difftime(nowTime, beginOperation) < MAX_CLOUD_TRAVEL_TIME) //time box
)
)
{
if (Lock(receiveUserContext->lock) != LOCK_OK)
{
ASSERT_FAIL("unable ot lock");
}
else
{
if (receiveUserContext->wasFound)
{
(void)Unlock(receiveUserContext->lock);
break;
}
(void)Unlock(receiveUserContext->lock);
}
ThreadAPI_Sleep(100);
}
// assert
ASSERT_IS_TRUE(receiveUserContext->wasFound, "Failure retrieving message that was sent to IotHub."); // was found is written by the callback...
// cleanup
IoTHubMessage_Destroy(messageHandle);
IoTHubMessaging_Close(iotHubMessagingHandle);
IoTHubMessaging_Destroy(iotHubMessagingHandle);
IoTHubServiceClientAuth_Destroy(iotHubServiceClientHandle);
IoTHubClient_Destroy(iotHubClientHandle);
ReceiveUserContext_Destroy(receiveUserContext);
}
static void SendEvent(IOTHUB_PROVISIONED_DEVICE* deviceToUse)
{
// arrange
IOTHUB_CLIENT_HANDLE iotHubClientHandle;
IOTHUB_MESSAGE_HANDLE msgHandle;
EXPECTED_SEND_DATA* sendData = EventData_Create();
ASSERT_IS_NOT_NULL(sendData, "Failure creating data to be sent");
// Send the Event
{
IOTHUB_CLIENT_RESULT result;
// Create the IoT Hub Data
iotHubClientHandle = IoTHubClient_CreateFromConnectionString(deviceToUse->connectionString, HTTP_Protocol);
ASSERT_IS_NOT_NULL(iotHubClientHandle, "Failure creating IothubClient handle");
msgHandle = IoTHubMessage_CreateFromByteArray((const unsigned char*)sendData->expectedString, strlen(sendData->expectedString));
ASSERT_IS_NOT_NULL(msgHandle, "Failure to create message handle");
if (deviceToUse->howToCreate == IOTHUB_ACCOUNT_AUTH_X509) {
result = IoTHubClient_SetOption(iotHubClientHandle, OPTION_X509_CERT, deviceToUse->certificate);
ASSERT_ARE_EQUAL(IOTHUB_CLIENT_RESULT, IOTHUB_CLIENT_OK, result, "Could not set the device x509 certificate");
result = IoTHubClient_SetOption(iotHubClientHandle, OPTION_X509_PRIVATE_KEY, deviceToUse->primaryAuthentication);
ASSERT_ARE_EQUAL(IOTHUB_CLIENT_RESULT, IOTHUB_CLIENT_OK, result, "Could not set the device x509 privateKey");
}
// act
result = IoTHubClient_SendEventAsync(iotHubClientHandle, msgHandle, ReceiveConfirmationCallback, sendData);
ASSERT_ARE_EQUAL(IOTHUB_CLIENT_RESULT, IOTHUB_CLIENT_OK, result, "Failure calling IoTHubClient_SendEventAsync");
}
time_t beginOperation, nowTime;
beginOperation = time(NULL);
while (
(nowTime = time(NULL)),
(difftime(nowTime, beginOperation) < MAX_CLOUD_TRAVEL_TIME) // time box
)
{
if (Lock(sendData->lock) != LOCK_OK)
{
ASSERT_FAIL("unable to lock");
}
else
{
if (sendData->dataWasRecv)
{
Unlock(sendData->lock);
break;
}
Unlock(sendData->lock);
}
ThreadAPI_Sleep(100);
}
if (Lock(sendData->lock) != LOCK_OK)
{
ASSERT_FAIL("unable to lock");
}
else
{
ASSERT_IS_TRUE(sendData->dataWasRecv, "Failure sending data to IotHub"); // was found is written by the callback...
(void)Unlock(sendData->lock);
}
{
IOTHUB_TEST_HANDLE iotHubTestHandle = IoTHubTest_Initialize(IoTHubAccount_GetEventHubConnectionString(g_iothubAcctInfo1), IoTHubAccount_GetIoTHubConnString(g_iothubAcctInfo1), deviceToUse->deviceId, IoTHubAccount_GetEventhubListenName(g_iothubAcctInfo1), IoTHubAccount_GetEventhubAccessKey(g_iothubAcctInfo1), IoTHubAccount_GetSharedAccessSignature(g_iothubAcctInfo1), IoTHubAccount_GetEventhubConsumerGroup(g_iothubAcctInfo1));
ASSERT_IS_NOT_NULL(iotHubTestHandle);
IOTHUB_TEST_CLIENT_RESULT result = IoTHubTest_ListenForEventForMaxDrainTime(iotHubTestHandle, IoTHubCallback, IoTHubAccount_GetIoTHubPartitionCount(g_iothubAcctInfo1), sendData);
ASSERT_ARE_EQUAL(IOTHUB_TEST_CLIENT_RESULT, IOTHUB_TEST_CLIENT_OK, result);
IoTHubTest_Deinit(iotHubTestHandle);
}
// assert
ASSERT_IS_TRUE(sendData->wasFound, "Failure receiving data from eventhub"); // was found is written by the callback...*/
// cleanup
IoTHubMessage_Destroy(msgHandle);
IoTHubClient_Destroy(iotHubClientHandle);
EventData_Destroy(sendData);
}
ASSERT_IS_EQUAL(entity->getNumberOfComponents(), 1)
ASSERT_IS_EQUAL(component1->getOwner(), entity)
entity->addComponent("test2", component2);
ASSERT_IS_EQUAL(entity->getNumberOfComponents(), 2)
ASSERT_IS_EQUAL(component2->getOwner(), entity)
delete entity;
}
// Destroy entity and components and check state.
TEST(DeleteEntity)
{
/*
CaffEntity::SimpleEntity *entity = new CaffEntity::SimpleEntity();
TestComponentOne *component1 = new TestComponentOne();
TestComponentTwo *component2 = new TestComponentTwo();
*/
// We need a better way of checking to see if it has been deleted.
ASSERT_IS_TRUE(false)
}
int main()
{
Test::RunTests();
return 0;
}