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);
}
示例#8
0
	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;
}