To receive notifications about scheduled maintenance, please subscribe to the mailing-list gitlab-operations@sympa.ethz.ch. You can subscribe to the mailing-list at https://sympa.ethz.ch

Commit ddea22ec authored by tiagos's avatar tiagos
Browse files

stuff added

parent 7662abd2
......@@ -2,9 +2,9 @@ anchorLength: 6 # Update this number after adding/removing anchors!!!
# Structure: anchorID [x, y, z] (ID is always from 0 to length-1)
anchor0: [-22.3, 6.6, 6.1]
anchor1: [-10.8, -18.5, 2.5]
anchor2: [1.1, -10.5, 6.1]
anchor3: [24.6, -10.1, 6.2]
anchor4: [24.8, 7.6, 6.1]
anchor5: [3.3, 13.8, 2.2]
anchor0: [-18.8, 24.3, 0]
anchor1: [2.5, -13.82, 0]
anchor2: [12.89, 24.3, 0]
anchor3: [18.04, 0.85, -6.83]
anchor4: [-0.56, 3.95, 17.91]
anchor5: [-21.15, 2.5, 0]
8,UWB_CF02,0/70/2M/E7E7E7E7E7,0,-1.35,-1.36,-0.2,1.47,1.3,2
9,UWB_CF01,0/80/250K/E7E7E7E7E7,1,0.03,-1.15,-0.2,1.65,0.87,2
8,UWB_CF02,0/70/2M/E7E7E7E7E7,0,-0.552586,-2.72294,-0.2,2.42231,-0.386166,2
......@@ -115,6 +115,49 @@ bool DataListener::_calculateAnchorPosition()
double x = (d01 * d01 + d02 * d02 - d12 * d12) / (2 * d02);
double y = d01 * sin(acos((d01 * d01 + d02 * d02 - d12 * d12) / (2 * d02 * d01)));
/*double d03, d23;
d03 = _get_distance_average(0,3);
d23 = _get_distance_average(2,3);*/
/*double a01, a21;
a01 = asin(-12.8/d01);
a21 = asin(-12.8/d12);
d01 *= cos(a01);
d12 *= cos(a21);*/
/*
d01 *= cos(a01);
d12 *= cos(a21);*/
/*double a03, a23;
a03 = asin(-6/d03);
a23 = asin(-6/d23);
d03 *= cos(a03);
d23 *= cos(a23);
double x3 = (d03 * d03 + d02 * d02 - d23 * d23) / (2 * d02);
double y3 = d03 * sin(acos((d03 * d03 + d02 * d02 - d23 * d23) / (2 * d02 * d03)));
double x = (d01 * d01 + d02 * d02 - d12 * d12) / (2 * d02);
double y = d01 * sin(acos((d01 * d01 + d02 * d02 - d12 * d12) / (2 * d02 * d01)));
double anchor_positions[18] = {0, 0, 0, \
x, -y, 0, \
d02, 0, 0, \
0, 0, 0, \
x3, -y3, -6, \
d02, 0, 0
};
ROS_WARN("x: %f, y: %f", x, -y);*/
double anchor_positions[18] = {0, 0, 0, \
x, -y, 0, \
d02, 0, 0, \
......@@ -126,6 +169,7 @@ bool DataListener::_calculateAnchorPosition()
anchor_position.push_back(anchor_position_data(0, anchor_positions[0], anchor_positions[1], anchor_positions[2]));
anchor_position.push_back(anchor_position_data(1, anchor_positions[3], anchor_positions[4], anchor_positions[5]));
anchor_position.push_back(anchor_position_data(2, anchor_positions[6], anchor_positions[7], anchor_positions[8]));
//anchor_position.push_back(anchor_position_data(3, anchor_positions[12], anchor_positions[13], anchor_positions[14]));
//For anchor n we only need d0n, d1n, d2n
for (unsigned int n = 3; n < 6; n++)
......@@ -137,9 +181,9 @@ bool DataListener::_calculateAnchorPosition()
//Calculate position of anchor n using IndoorNewton
double result[3];
IndoorNewton(anchor_positions, anchor_distances, result, 200);
IndoorNewton(anchor_positions, anchor_distances, result, 100);
anchor_position.push_back(anchor_position_data(n, result[0], result[1], result[2]));
anchor_position.push_back(anchor_position_data(n, result[0], result[1], 0.0));//result[2])); //result[2]
}
//Save anchor positions to yaml file
......@@ -156,9 +200,9 @@ void DataListener::_resetLocalisation()
anchors_raw.clear();
anchor_position.clear();
//offset.x = 0;
//offset.y = 0;
//offset.z = 0;
offset.x = 0;
offset.y = 0;
offset.z = 0;
finishedLoc = false;
}
......
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