Commit 8c992851 authored by Paul Rouanet's avatar Paul Rouanet
Browse files

Try to import imu.hpp from odri_control_interface

parent 735dbb01
...@@ -18,6 +18,7 @@ ...@@ -18,6 +18,7 @@
/*Connection to ODRI for read sensors and write commands*/ /*Connection to ODRI for read sensors and write commands*/
#include "odri_control_interface/calibration.hpp" #include "odri_control_interface/calibration.hpp"
#include "odri_control_interface/robot.hpp" #include "odri_control_interface/robot.hpp"
#include "odri_control_interface/imu.hpp"
......
/* Utile pour la lecture de capteur ---> read() de system_bolt.cpp*/
/* Inspiré de demo_create_bolt_robot.cpp*/
#include "system_bolt.hpp"
#include <odri_control_interface/utils.hpp>
#include <odri_control_interface/imu.hpp>
using namespace odri_control_interface;
using namespace ros2_control_bolt;
#include <iostream>
#include <stdexcept>
const hardware_interface::HardwareInfo info_test;
std::shared_ptr<odri_control_interface::Robot> robot;
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.
SystemBoltHardware::init_robot(info_test, robot);
SystemBoltHardware::configure(info_test);
// Start the robot.
SystemBoltHardware::start();
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();
/*c++;
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;
}*/
}
else
{
std::this_thread::yield();
}
}
std::cout << "Normal program shutdown." << std::endl;
return 0;
}
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment