demo_config.cpp 3.12 KB
Newer Older
Paul Rouanet's avatar
Paul Rouanet committed
1
2
3
4
5
/* Utile pour la lecture de capteur ---> read() de system_bolt.cpp*/
/* Inspiré de demo_create_bolt_robot.cpp*/



6
#include "system_bolt.hpp"
Paul Rouanet's avatar
Paul Rouanet committed
7
8
9
10
11
12

#include <odri_control_interface/utils.hpp>
#include <odri_control_interface/imu.hpp>


using namespace odri_control_interface;
13
using namespace ros2_control_bolt;
Paul Rouanet's avatar
Paul Rouanet committed
14
15
16
17

#include <iostream>
#include <stdexcept>

18
19
const hardware_interface::HardwareInfo info_test;
std::shared_ptr<odri_control_interface::Robot> robot;
Paul Rouanet's avatar
Paul Rouanet committed
20
21
22
23
24
25
26
27
28
29
30
31

int main(int argc, char **argv)
{
    if (argc != 2)
    {
        throw std::runtime_error(
            "Please provide the interface name "
            "(i.e. using 'ifconfig' on linux");
    }

    nice(-20);  // Give the process a high priority.

32
33
34
35
    

    SystemBoltHardware::init_robot(info_test, robot);
    SystemBoltHardware::configure(info_test);
Paul Rouanet's avatar
Paul Rouanet committed
36
37

    // Start the robot.
38
    SystemBoltHardware::start();
Paul Rouanet's avatar
Paul Rouanet committed
39
40
41
42
43
44
45
46
47
48
49
50


    int c = 0;
    std::chrono::time_point<std::chrono::system_clock> last =
        std::chrono::system_clock::now();
    while (!robot->IsTimeout())
    {
        if (((std::chrono::duration<double>)(std::chrono::system_clock::now()-last)).count() > 0.001)
        {
            last = std::chrono::system_clock::now();  // last+dt would be better
            robot->ParseSensorData();

51
            /*c++;
Paul Rouanet's avatar
Paul Rouanet committed
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
            if (c % 1000 == 0)
            {
                std::cout << "Count :                        " << c << "\n";
                std::cout << "\n";

                std::cout << "Joints : \n";
                std::cout << "Position:                      ";
                joints->PrintVector(joints->GetPositions());
                std::cout << "\n";
                std::cout << "Velocities:                    ";
                joints->PrintVector(joints->GetVelocities());
                std::cout << "\n";
                std::cout << "Measured Torques:              ";
                joints->PrintVector(joints->GetMeasuredTorques());
                std::cout << "\n";
                std::cout << "\n";

                std::cout << "IMU : \n";
                std::cout << "Gyroscope                      ";
                joints->PrintVector(imu->GetGyroscope());
                std::cout << "\n";
                std::cout << "Accelerometer                  ";
                joints->PrintVector(imu->GetAccelerometer());
                std::cout << "\n";
                std::cout << "Linear Acceleration            ";
                joints->PrintVector(imu->GetLinearAcceleration());
                std::cout << "\n";
                std::cout << "Attitude Euler                 ";
                joints->PrintVector(imu->GetAttitudeEuler());
                std::cout << "\n";
                std::cout << "Attitude Quaternion            ";
                joints->PrintVector(imu->GetAttitudeQuaternion());
                std::cout << "\n";
                std::cout << "\n";
                std::cout << "\n";
                std::cout << "\n";
                std::cout << std::endl;
                
90
            }*/
Paul Rouanet's avatar
Paul Rouanet committed
91
92
93
94
95
96
97
98
99
100
        }
        else
        {
            std::this_thread::yield();
        }
    }
    std::cout << "Normal program shutdown." << std::endl;
    return 0;
}