Commit 416d27e7 authored by Yvan Bosshard's avatar Yvan Bosshard
Browse files

Added Tiagos Solver

parent 494c8176
......@@ -224,7 +224,7 @@ void MainGUIWindow::_init()
ros::NodeHandle nodeHandle("~");
UWBServiceClient = nodeHandle.serviceClient<Anchors>("/UWBServiceClient/UWBData", false);
UWBServiceClient = nodeHandle.serviceClient<Anchors>("/UWBServiceManager/UWBData", false);
DBChangedPublisher = nodeHandle.advertise<std_msgs::Int32>("DBChanged", 1);
emergencyStopPublisher = nodeHandle.advertise<std_msgs::Int32>("emergencyStop", 1);
......@@ -999,6 +999,7 @@ void MainGUIWindow::on_checkBox_enable_UWB_toggled(bool checked)
else
{
ui->checkBox_enable_UWB->setEnabled(!checked);
ROS_ERROR("[Teacher GUI] Could not update UWB Settings!");
}
......
#pragma once
#include <cmath>
#include <cstring>
void IndoorNewton(const double anc[18], const double d[6], double result[3], unsigned int it)
{
int i;
unsigned int count;
int p1;
double x[9];
double y[9];
double J[18];
double absx11;
double absx21;
int p3;
double absx31;
double r[6];
int itmp;
double b_y[9];
double c_y[18];
// initial value
for (i = 0; i < 3; i++) {
result[i] = 0.0;
}
for (count = 0; count < it; count++) {
// populate Jacobian
// poopulate r-function
for (i = 0; i < 6; i++) {
for (p1 = 0; p1 < 3; p1++) {
J[i + 6 * p1] = 2.0 * (anc[i * 3 + p1] - result[p1]);
}
p1 = i * 3;
absx11 = anc[p1] - result[0];
absx21 = anc[p1 + 1] - result[1];
absx31 = anc[p1 + 2] - result[2];
r[i] = ((d[i] * d[i] - absx11 * absx11) - absx21 * absx21) - absx31 *
absx31;
}
for (p1 = 0; p1 < 3; p1++) {
for (i = 0; i < 3; i++) {
y[p1 + 3 * i] = 0.0;
for (p3 = 0; p3 < 6; p3++) {
y[p1 + 3 * i] += J[p3 + 6 * p1] * J[p3 + 6 * i];
}
}
}
memcpy(&x[0], &y[0], 9U * sizeof(double));
p1 = 0;
i = 3;
p3 = 6;
absx11 = std::abs(y[0]);
absx21 = std::abs(y[1]);
absx31 = std::abs(y[2]);
if ((absx21 > absx11) && (absx21 > absx31)) {
p1 = 3;
i = 0;
x[0] = y[1];
x[1] = y[0];
x[3] = y[4];
x[4] = y[3];
x[6] = y[7];
x[7] = y[6];
}
else {
if (absx31 > absx11) {
p1 = 6;
p3 = 0;
x[0] = y[2];
x[2] = y[0];
x[3] = y[5];
x[5] = y[3];
x[6] = y[8];
x[8] = y[6];
}
}
absx11 = x[1] / x[0];
x[1] /= x[0];
absx21 = x[2] / x[0];
x[2] /= x[0];
x[4] -= absx11 * x[3];
x[5] -= absx21 * x[3];
x[7] -= absx11 * x[6];
x[8] -= absx21 * x[6];
if (std::abs(x[5]) > std::abs(x[4])) {
itmp = i;
i = p3;
p3 = itmp;
x[1] = absx21;
x[2] = absx11;
absx11 = x[4];
x[4] = x[5];
x[5] = absx11;
absx11 = x[7];
x[7] = x[8];
x[8] = absx11;
}
absx11 = x[5] / x[4];
x[5] /= x[4];
x[8] -= absx11 * x[7];
absx11 = (x[5] * x[1] - x[2]) / x[8];
absx21 = -(x[1] + x[7] * absx11) / x[4];
b_y[p1] = ((1.0 - x[3] * absx21) - x[6] * absx11) / x[0];
b_y[p1 + 1] = absx21;
b_y[p1 + 2] = absx11;
absx11 = -x[5] / x[8];
absx21 = (1.0 - x[7] * absx11) / x[4];
b_y[i] = -(x[3] * absx21 + x[6] * absx11) / x[0];
b_y[i + 1] = absx21;
b_y[i + 2] = absx11;
absx11 = 1.0 / x[8];
absx21 = -x[7] * absx11 / x[4];
b_y[p3] = -(x[3] * absx21 + x[6] * absx11) / x[0];
b_y[p3 + 1] = absx21;
b_y[p3 + 2] = absx11;
for (p1 = 0; p1 < 3; p1++) {
absx11 = 0.0;
for (i = 0; i < 6; i++) {
c_y[p1 + 3 * i] = 0.0;
for (p3 = 0; p3 < 3; p3++) {
c_y[p1 + 3 * i] += b_y[p1 + 3 * p3] * J[i + 6 * p3];
}
absx11 += c_y[p1 + 3 * i] * r[i];
}
result[p1] -= absx11;
}
}
}
\ No newline at end of file
......@@ -14,26 +14,27 @@
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#include "AnchorPositions.h"
#include "d_fall_pps/Anchors.h"
#include <ros/package.h>
#include <string>
#include <vector>
#include <ros/package.h>
#include "d_fall_pps/Anchors.h"
#include "AnchorPositions.h"
#define UWB_UPDATE_DISABLE 0
#define UWB_UPDATE_ENABLE 1
#define UWB_UPDATE_ANCHORS 5
using namespace d_fall_pps;
bool enableUWB;
UWBAnchorArray anchors;
//ros::NodeHandle nodeHandle;//("~");
int main(int argc, char* argv[])
{
ros::init(argc, argv, "UWBServiceClient");
ros::init(argc, argv, "UWBServiceManager");
ros::NodeHandle nodeHandle("~");
readYaml();
......@@ -109,7 +110,8 @@ void d_fall_pps::updateCallback(const std_msgs::Int32 &message)
readYaml();
break;
default:
ROS_WARN("[UWBServiceClient] unknown update command");
ROS_WARN("[UWBServiceManager] unknown update command");
}
ROS_ERROR("it works!!!");
ROS_WARN("[UWBServiceManager] Updated UWB Settings!");
}
......@@ -36,6 +36,7 @@ rosbag::Bag bag;
bool enableUWB = true;
bool useUWB = true;
//bool useUWB = false;
CrazyflieData CFData_vicon;
......@@ -137,9 +138,9 @@ void d_fall_pps::UWBDataCallback(const CrazyflieData &data)
// override UWB Data
CrazyflieData CFData_uwb = data;
CFData_uwb.x = CFData_vicon.x;
/* CFData_uwb.x = CFData_vicon.x;
CFData_uwb.y = CFData_vicon.y;
CFData_uwb.z = CFData_vicon.z;
CFData_uwb.z = CFData_vicon.z;*/
CFData_uwb.roll = CFData_vicon.roll;
CFData_uwb.pitch = CFData_vicon.pitch;
CFData_uwb.yaw = CFData_vicon.yaw;
......
......@@ -26,6 +26,8 @@
#include "d_fall_pps/CrazyflieData.h"
#include "d_fall_pps/CrazyflieContext.h"
#include "IndoorNewton.h"
#include "UWBDataPublisher.h"
......@@ -136,9 +138,33 @@ void d_fall_pps::cfDistancesCallback(const std_msgs::UInt32MultiArray &distances
//calculates position from anchor locations and distances -> forces algorithm?
void d_fall_pps::calculateXYZ(const std_msgs::UInt32MultiArray& distances, double(&xyz)[3])
{
//add algorithm from forces - paul
//double positions[3*UWB_NUM_ANCHORS] = { 12,0,0, 28,0,10, 28,20,0, 15,30,10, 0,22,0, 0,6,0 };
double positions[3*UWB_NUM_ANCHORS] = {
-1.2, -14.0, 5.8,
14.9, -14.0, 15.9,
14.7, 5.8, 5.8,
1.1, 14.6, 15.9,
-13.5, 7.6, 5.8,
-13.4, -8.4, 5.8
};
double dist[UWB_NUM_ANCHORS];
double result[3];
for(unsigned int i = 0; i < UWB_NUM_ANCHORS; i++)
{
dist[i] = distances.data[i] / 100.0; //in dm
if(distances.data[i] == 0)
ROS_WARN("[UWBDataPublisher] bad reading from Anchor %i", i);
}
IndoorNewton(positions, dist, result, 50);
xyz[0] = result[0] / 10.0;
xyz[1] = result[1] / 10.0;
xyz[2] = result[2] / 10.0;
double r1, r2, r3, x, y, z;
/*double r1, r2, r3, x, y, z;
const double tri_d = 1.8;
const double tri_i = 0.0;
const double tri_j = 2.7;
......@@ -163,7 +189,7 @@ void d_fall_pps::calculateXYZ(const std_msgs::UInt32MultiArray& distances, doubl
xyz[1] = y;
xyz[2] = z;
return;
return;*/
}
// Loads the current context of the client
......
Supports Markdown
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