From 81677f34395a8da953291ba05aa39d626ecbea74 Mon Sep 17 00:00:00 2001
From: andreeatulbure <andreea_tulbure@yahoo.de>
Date: Wed, 1 Mar 2017 16:09:47 +0100
Subject: [PATCH] sensor calibration loading and rotationRate
---
.../OrientedTactileSensor.scx | 2 +-
.../config/OrientedTactileSensorUnitApp.cfg | 2 +-
.../OrientedTactileSensorUnit/main.cpp | 1 +
.../OrientedTactileSensorUnitObserver.cpp | 3 +-
.../units/OrientedTactileSensorUnitObserver.h | 2 +-
.../OrientedTactileSensorUnit.cpp | 132 ++++++++++++++++--
.../OrientedTactileSensorUnit.h | 19 ++-
.../units/OrientedTactileSensorUnit.ice | 6 +-
8 files changed, 149 insertions(+), 18 deletions(-)
diff --git a/scenarios/OrientedTactileSensor/OrientedTactileSensor.scx b/scenarios/OrientedTactileSensor/OrientedTactileSensor.scx
index ec0c5693b..f72fe0a6c 100644
--- a/scenarios/OrientedTactileSensor/OrientedTactileSensor.scx
+++ b/scenarios/OrientedTactileSensor/OrientedTactileSensor.scx
@@ -1,5 +1,5 @@
<?xml version="1.0" encoding="utf-8"?>
-<scenario name="OrientedTactileSensor" lastChange="2017-02-27.01:54:44 PM" creation="2017-02-27.01:48:55 PM" globalConfigName="./config/global.cfg" package="RobotAPI">
+<scenario name="OrientedTactileSensor" lastChange="2017-03-01.15:11:23" creation="2017-02-27.01:48:55 PM" globalConfigName="./config/global.cfg" package="RobotAPI">
<application name="OrientedTactileSensorUnitApp" instance="" package="RobotAPI"/>
<application name="OrientedTactileSensorUnitObserverApp" instance="" package="RobotAPI"/>
</scenario>
diff --git a/scenarios/OrientedTactileSensor/config/OrientedTactileSensorUnitApp.cfg b/scenarios/OrientedTactileSensor/config/OrientedTactileSensorUnitApp.cfg
index fe53aea1b..9f81e34fd 100644
--- a/scenarios/OrientedTactileSensor/config/OrientedTactileSensorUnitApp.cfg
+++ b/scenarios/OrientedTactileSensor/config/OrientedTactileSensorUnitApp.cfg
@@ -103,7 +103,7 @@
# - Default: /dev/ttyACM0
# - Case sensitivity: no
# - Required: no
-ArmarX.OrientedTactileSensorUnit.SerialInterfaceDevice = /dev/ttyACM1
+ArmarX.OrientedTactileSensorUnit.SerialInterfaceDevice = /dev/ttyACM0
# ArmarX.OrientedTactileSensorUnit.TopicName: Name of the topic on which the sensor values are provided
diff --git a/source/RobotAPI/applications/OrientedTactileSensorUnit/main.cpp b/source/RobotAPI/applications/OrientedTactileSensorUnit/main.cpp
index ec06c4a12..f4ab124f7 100644
--- a/source/RobotAPI/applications/OrientedTactileSensorUnit/main.cpp
+++ b/source/RobotAPI/applications/OrientedTactileSensorUnit/main.cpp
@@ -1,6 +1,7 @@
#include <ArmarXCore/core/application/Application.h>
#include <RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.h>
#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/Component.h>
int main(int argc, char* argv[])
{
diff --git a/source/RobotAPI/components/units/OrientedTactileSensorUnitObserver.cpp b/source/RobotAPI/components/units/OrientedTactileSensorUnitObserver.cpp
index 98f60e6c9..a7d74099a 100644
--- a/source/RobotAPI/components/units/OrientedTactileSensorUnitObserver.cpp
+++ b/source/RobotAPI/components/units/OrientedTactileSensorUnitObserver.cpp
@@ -58,7 +58,7 @@ void OrientedTactileSensorUnitObserver::onExitObserver()
//debugDrawerPrx->removeLineVisu("IMU", "acceleration");
}
-void OrientedTactileSensorUnitObserver::reportSensorValues(int id, float pressure, float qw, float qx, float qy, float qz, const TimestampBasePtr& timestamp, const Ice::Current&)
+void OrientedTactileSensorUnitObserver::reportSensorValues(int id, float pressure, float qw, float qx, float qy, float qz, float rotationRate, const TimestampBasePtr& timestamp, const Ice::Current&)
{
ScopedLock lock(dataMutex);
TimestampVariantPtr timestampPtr = TimestampVariantPtr::dynamicCast(timestamp);
@@ -75,6 +75,7 @@ void OrientedTactileSensorUnitObserver::reportSensorValues(int id, float pressur
offerOrUpdateDataField(channelName, "pressure", Variant(pressure), "current pressure");
QuaternionPtr orientationQuaternion = new Quaternion(qw, qx, qy, qz);
offerOrUpdateDataField(channelName, "orientation", orientationQuaternion, "current oriantation");
+ offerOrUpdateDataField(channelName, "rotationRate", Variant(rotationRate), "current rotationRate");
}
/* TODO: for integration with debug drawer
updateChannel(device);
diff --git a/source/RobotAPI/components/units/OrientedTactileSensorUnitObserver.h b/source/RobotAPI/components/units/OrientedTactileSensorUnitObserver.h
index 2b95757d8..cacb30f90 100644
--- a/source/RobotAPI/components/units/OrientedTactileSensorUnitObserver.h
+++ b/source/RobotAPI/components/units/OrientedTactileSensorUnitObserver.h
@@ -72,7 +72,7 @@ namespace armarx
virtual void onConnectObserver();
virtual void onExitObserver();
- void reportSensorValues(int id, float pressure, float qw, float qx, float qy, float qz, const TimestampBasePtr& timestamp, const Ice::Current&);
+ void reportSensorValues(int id, float pressure, float qw, float qx, float qy, float qz, float rotationRate, const TimestampBasePtr& timestamp, const Ice::Current&);
/**
* @see PropertyUser::createPropertyDefinitions()
diff --git a/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.cpp b/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.cpp
index 747d27439..5da836167 100644
--- a/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.cpp
+++ b/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.cpp
@@ -9,6 +9,8 @@ using namespace armarx;
OrientedTactileSensorUnit::OrientedTactileSensorUnit()
{
+ maxSamples = 10;
+ sampleIndex = 0;
}
void OrientedTactileSensorUnit::onInitComponent()
@@ -56,17 +58,41 @@ void OrientedTactileSensorUnit::onInitComponent()
//wait for the Arduino to reboot
usleep(4000000);
-
- //wait for the IMU to be calibrated
std::string arduinoLine;
- while (arduinoLine.find("Calibratrion Sucessfull") == std::string::npos)
+ //wait for the IMU to be calibrated or load calibration
+ ARMARX_INFO << "waiting for IMU calibration - this can take some time";
+ if (CALIBRATE_ON)
{
- getline(arduino, arduinoLine, '\n');
- ARMARX_INFO << "waiting for IMU calibration - this can take some time";
- }
+ //calibrate
+ while (arduinoLine.find("mode") == std::string::npos)
+ {
+ getline(arduino, arduinoLine, '\n');
+ ARMARX_INFO << arduinoLine;
+ }
+ arduino.close();
- ARMARX_INFO << "IMU calibration finished";
+ arduino.open(getProperty<std::string>("SerialInterfaceDevice").getValue(), std::ios::out);
+ arduino << "calibrate";
+ arduino.flush();
+ arduino.close();
+ arduino.open(getProperty<std::string>("SerialInterfaceDevice").getValue(), std::ios::in);
+ while (arduinoLine.find("Calibration Sucessfull") == std::string::npos)
+ {
+ getline(arduino, arduinoLine, '\n');
+ ARMARX_INFO << arduinoLine;
+ }
+ ARMARX_IMPORTANT << "calibrated sensor";
+ }
+ else
+ {
+ //load calibration
+ ARMARX_INFO << "load";
+ if (loadCalibration())
+ {
+ ARMARX_IMPORTANT << "loaded calibration";
+ }
+ }
readTask = new RunningTask<OrientedTactileSensorUnit>(this, &OrientedTactileSensorUnit::run);
readTask->start();
@@ -90,13 +116,31 @@ void OrientedTactileSensorUnit::run()
{
std::string line;
getline(arduino, line, '\n');
+ ARMARX_INFO << line;
SensorData data = getValues(line.c_str());
IceUtil::Time now = IceUtil::Time::now();
- TimestampVariantPtr nowTimestamp = new TimestampVariant(now);
+ TimestampVariantPtr nowTimestamp = new TimestapressurempVariant(now);
+
+ float rotationRate = 0;
+ std::pair<IceUtil::Time, Eigen::Quaternionf> sample = std::make_pair(now, Eigen::Quaternionf(data.qw, data.qx, data.qy, data.qz));
+ if (samples.size() < maxSamples)
+ {
+ samples.push_back(sample);
+ }
+ else
+ {
+ samples[sampleIndex] = sample;
+ sampleIndex = (sampleIndex + 1) % maxSamples;
+ std::pair<IceUtil::Time, Eigen::Quaternionf> oldsample = samples.at(sampleIndex);
+ Eigen::AngleAxisf aa(oldsample.second.inverse() * sample.second);
+ rotationRate = aa.angle() / (sample.first - oldsample.first).toSecondsDouble();
+ }
+
if (topicPrx)
{
- topicPrx->reportSensorValues(data.id, data.pressure, data.qw, data.qx, data.qy, data.qz, nowTimestamp);
+ topicPrx->reportSensorValues(data.id, data.pressure, data.qw, data.qx, data.qy, data.qz, rotationRate, nowTimestamp);
}
+
}
}
@@ -115,8 +159,78 @@ OrientedTactileSensorUnit::SensorData OrientedTactileSensorUnit::getValues(std::
return data;
}
+/*void OrientedTactileSensorUnit::getCalibrationValues(std::string line)
+{
+ std::vector<std::string> splitValues;
+ boost::split(splitValues, line, boost::is_any_of(" "));
+ calibration.accel_offset_x = stoi(splitValues.at(0));
+ calibration.accel_offset_y = stoi(splitValues.at(1));
+ calibration.accel_offset_z = stoi(splitValues.at(2));
+ calibration.gyro_offset_x = stoi(splitValues.at(3));
+ calibration.gyro_offset_y = stoi(splitValues.at(4));
+ calibration.gyro_offset_z = stoi(splitValues.at(5));
+ calibration.mag_offset_x = stoi(splitValues.at(6));
+ calibration.mag_offset_y = stoi(splitValues.at(7));
+ calibration.mag_offset_z = stoi(splitValues.at(8));
+ calibration.accel_radius = stoi(splitValues.at(9));
+ calibration.mag_radius = stoi(splitValues.at(10));
+}
+*/
+std::string space = " ";
+bool OrientedTactileSensorUnit::loadCalibration()
+{
+ std::string calibrationStream = getCalibrationValues();
+ std::string arduinoLine;
+ while (arduinoLine.find("mode") == std::string::npos)
+ {
+ getline(arduino, arduinoLine, '\n');
+ }
+ arduino.close();
+
+ arduino.open(getProperty<std::string>("SerialInterfaceDevice").getValue(), std::ios::out);
+ arduino << "load";
+ arduino.flush();
+ arduino.close();
+ arduino.open(getProperty<std::string>("SerialInterfaceDevice").getValue(), std::ios::in);
+ while (arduinoLine.find("calibration data") == std::string::npos)
+ {
+ getline(arduino, arduinoLine, '\n');
+ }
+ arduino.close();
+
+ arduino.open(getProperty<std::string>("SerialInterfaceDevice").getValue(), std::ios::out);
+ arduino << calibrationStream;
+ arduino.flush();
+ arduino.close();
+ arduino.open(getProperty<std::string>("SerialInterfaceDevice").getValue(), std::ios::in);
+
+ while (arduinoLine.find("Calibration Sucessfull") == std::string::npos)
+ {
+ getline(arduino, arduinoLine, '\n');
+ }
+ return true;
+}
+std::string OrientedTactileSensorUnit::getCalibrationValues()
+{
+ calibration.accel_offset_x = 65524;
+ calibration.accel_offset_y = 3;
+ calibration.accel_offset_z = 12;
+ calibration.gyro_offset_x = 65534;
+ calibration.gyro_offset_y = 65534;
+ calibration.gyro_offset_z = 1;
+ calibration.mag_offset_x = 1208;
+ calibration.mag_offset_y = 119;
+ calibration.mag_offset_z = 58726;
+ calibration.accel_radius = 1000;
+ calibration.mag_radius = 943;
+ std::string space = " ";
+ std::string calibrationStream = "";
+ calibrationStream = calibrationStream + std::to_string(calibration.accel_offset_x) + space + std::to_string(calibration.accel_offset_y) + space + std::to_string(calibration.accel_offset_z) + space + std::to_string(calibration.gyro_offset_x) + space + std::to_string(calibration.gyro_offset_y) + space +
+ std::to_string(calibration.gyro_offset_z) + space + std::to_string(calibration.mag_offset_x) + space + std::to_string(calibration.mag_offset_y) + space + std::to_string(calibration.mag_offset_z) + space + std::to_string(calibration.accel_radius) + space + std::to_string(calibration.mag_radius) + space;
+ return calibrationStream;
+}
diff --git a/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.h b/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.h
index e5739e7ca..1561d2412 100644
--- a/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.h
+++ b/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.h
@@ -3,14 +3,17 @@
#include <ArmarXCore/core/Component.h>
#include <RobotAPI/interface/units/UnitInterface.h>
-#include <RobotAPI/interface/units/OrientedTactileSensorUnit.h> //.ice for datastructure
+#include <RobotAPI/interface/units/OrientedTactileSensorUnit.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/core/services/tasks/RunningTask.h>
#include <netinet/in.h>
#include <fstream>
#include <stdio.h>
#include <boost/date_time/posix_time/posix_time.hpp>
+//#include "RobotAPI/components/units/SensorActorUnit.h"
+#include <Eigen/Dense>
+#define CALIBRATE_ON 0
namespace armarx
{
@@ -58,6 +61,11 @@ namespace armarx
float qw, qx, qy, qz;
};
+ struct CalibrationData
+ {
+ int accel_offset_x, accel_offset_y, accel_offset_z, gyro_offset_x, gyro_offset_y, gyro_offset_z, mag_offset_x, mag_offset_y, mag_offset_z, accel_radius, mag_radius;
+ };
+
protected:
virtual void onInitComponent();
virtual void onConnectComponent();
@@ -68,13 +76,18 @@ namespace armarx
std::fstream arduino;
RunningTask<OrientedTactileSensorUnit>::pointer_type readTask;
OrientedTactileSensorUnitListenerPrx topicPrx;
+ OrientedTactileSensorUnitInterfacePrx interfacePrx;
void run();
SensorData getValues(std::string s);
+ std::string getCalibrationValues();
+ bool loadCalibration();
int fd;
+ CalibrationData calibration;
+ std::vector<std::pair<IceUtil::Time, Eigen::Quaternionf>> samples;
+ int maxSamples;
+ int sampleIndex;
};
}
-
-
#endif // SENSORPACKAGEUNIT_H
diff --git a/source/RobotAPI/interface/units/OrientedTactileSensorUnit.ice b/source/RobotAPI/interface/units/OrientedTactileSensorUnit.ice
index d41e55367..d36263d95 100644
--- a/source/RobotAPI/interface/units/OrientedTactileSensorUnit.ice
+++ b/source/RobotAPI/interface/units/OrientedTactileSensorUnit.ice
@@ -39,13 +39,15 @@ module armarx
};
- interface OrientedTactileSensorUnitInterface extends SensorActorUnitInterface
+ interface OrientedTactileSensorUnitInterface extends armarx::SensorActorUnitInterface
{
+ //needs uint16_t
+ //bool loadCalibration(int accel_offset_x, int accel_offset_y, int accel_offset_z, int gyro_offset_x, int gyro_offset_y, int gyro_offset_z, int mag_offset_x, int mag_offset_y, int mag_offset_z, int accel_radius, int mag_radius );
};
interface OrientedTactileSensorUnitListener
{
- void reportSensorValues(int id, float pressure, float qw, float qx, float qy, float qz, TimestampBase timestamp);
+ void reportSensorValues(int id, float pressure, float qw, float qx, float qy, float qz, float rotationRate, TimestampBase timestamp);
};
--
GitLab