-
Notifications
You must be signed in to change notification settings - Fork 2
/
ForceTorqueTest.cc
71 lines (56 loc) · 2.15 KB
/
ForceTorqueTest.cc
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
#include <chrono>
#include <cstddef>
#include <filesystem>
#include <string>
#include <thread>
#include <gtest/gtest.h>
#include <gz/common/Console.hh>
#include <gz/sim/TestFixture.hh>
#include <yarp/dev/MultipleAnalogSensorsInterfaces.h>
#include <yarp/dev/PolyDriver.h>
#include <yarp/os/Network.h>
#include <yarp/os/Property.h>
#include <yarp/sig/Vector.h>
TEST(ForceTorqueTest, PluginTest)
{
yarp::os::NetworkBase::setLocalMode(true);
// Maximum verbosity helps with debugging
gz::common::Console::SetVerbosity(4);
// Instantiate test fixture
auto modelPath = std::filesystem::path(CMAKE_CURRENT_SOURCE_DIR) / "model.sdf";
gz::sim::TestFixture fixture(modelPath.string());
int iterations = 1000;
fixture.Server()->Run(/*_blocking=*/true, iterations, /*_paused=*/false);
yarp::os::Property option;
option.put("device", "multipleanalogsensorsclient");
option.put("remote", "/forcetorque");
option.put("timeout", 1.0);
option.put("local", "/ForceTorqueTest");
yarp::dev::PolyDriver driver;
ASSERT_TRUE(driver.open(option));
yarp::dev::ISixAxisForceTorqueSensors* isixaxis = nullptr;
ASSERT_TRUE(driver.view(isixaxis));
fixture.Server()->Run(/*_blocking=*/true, iterations, /*_paused=*/false);
yarp::sig::Vector measure(6);
std::string sensorName;
double timestamp;
isixaxis->getSixAxisForceTorqueSensorName(0, sensorName);
size_t maxNrOfReadingAttempts = 20;
bool readSuccessful = false;
for (size_t i = 0; (i < maxNrOfReadingAttempts) && !readSuccessful; i++)
{
readSuccessful = isixaxis->getSixAxisForceTorqueSensorMeasure(0, measure, timestamp);
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
ASSERT_TRUE(readSuccessful);
yarp::dev::MAS_status status;
status = isixaxis->getSixAxisForceTorqueSensorStatus(0);
ASSERT_EQ(status, yarp::dev::MAS_OK);
EXPECT_NEAR(measure(0), 0.0, 1e-2);
EXPECT_NEAR(measure(1), 0.0, 1e-2);
EXPECT_NEAR(measure(2), -9.8 * 10, 1e-2);
EXPECT_NEAR(measure(3), 0.0, 1e-2);
EXPECT_NEAR(measure(4), 0.0, 1e-2);
EXPECT_NEAR(measure(5), 0.0, 1e-2);
EXPECT_GT(timestamp, 0.0);
}