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 c0390c6a authored by tiagos's avatar tiagos
Browse files

final enhancements

parent 273709ca
......@@ -1089,24 +1089,6 @@ void MainGUIWindow::on_reloadAnchors_button_pressed()
UWBServiceClientUpdate_publisher.publish(server_msg);
updateUWBSettings(!ui->checkBox_enable_UWB->isChecked());
/*// Receive the updated information
Anchors a;
if(UWBServiceClient.call(a))
{
updateAnchors(&a);
}
else
{
ROS_ERROR("[Teacher GUI] Could not update UWB Settings!");
return;
}
// Publish message that clients have to reload data from UWBServiceClient
std_msgs::Int32 msg;
msg.data = 1;
UWBSettings_publisher.publish(msg);*/
}
void MainGUIWindow::on_calibrateAnchors_button_pressed()
......@@ -1119,6 +1101,8 @@ void MainGUIWindow::on_calibrateAnchors_button_pressed()
ui->checkBox_enable_UWB->setEnabled(true);
ui->reloadAnchors_button->setEnabled(true);
ros::Duration(10).sleep();
updateUWBSettings(!ui->checkBox_enable_UWB->isChecked());
}
......
......@@ -22,7 +22,7 @@ void IndoorNewton(const double anc[18], const double d[6], double result[3], uns
// initial value
for (i = 0; i < 3; i++) {
result[i] = 0.0;
result[i] = 100.0;
}
for (count = 0; count < it; count++) {
......
......@@ -40,7 +40,7 @@ void DataListener::_LOC_Debugger(std::string value)
anchors_raw.push_back(anchor_raw_data(source, dest, distance));
printf("--- LOC: \t%u \t%u \t%u\n", source, dest, distance);
//printf("--- LOC: \t%u \t%u \t%u\n", source, dest, distance);
//Reset Timer
locTimer = std::chrono::high_resolution_clock::now();
......@@ -103,8 +103,6 @@ bool DataListener::_calculateAnchorPosition()
if (!_check_data_validity())
return false;
printf("Finding anchor`s position...\n");
anchors_raw.sort(compare_anchor_data);
double d01, d02, d12;
......@@ -130,7 +128,6 @@ bool DataListener::_calculateAnchorPosition()
//For anchor n we only need d0n, d1n, d2n
for (unsigned int n = 3; n < 6; n++)
{
int flag = 1;
double anchor_distances[6] = { _get_distance_average(0, n), _get_distance_average(1, n), _get_distance_average(2, n), \
_get_distance_average(0, n), _get_distance_average(1, n), _get_distance_average(2, n)
};
......@@ -152,6 +149,7 @@ bool DataListener::_calculateAnchorPosition()
for (unsigned int i = 0; i < anchor_position.size(); i++)
{
//printf("anchor%i (%f, %f, %f)\n", i, anchor_position[i].x, anchor_position[i].y, anchor_position[i].z);
anchorArr = {anchor_position[i].x - offset.x, anchor_position[i].y - offset.y, anchor_position[i].z - offset.z};
nodeHandle.setParam("anchor" + std::to_string(i), anchorArr);
}
......@@ -199,16 +197,18 @@ bool DataListener::_startSerialCommunication()
bool DataListener::startAnchorLocalistaion()
{
_resetLocalisation();
for (unsigned int i = 0; i < 5 && !finishedLoc; i++)
{
if (i != 0)
printf("Looks like something went wrong... %u tries left\n", 5 - i);
ROS_WARN("[Selflocalisation] Looks like something went wrong... %u tries left\n", 5 - i);
_resetLocalisation();
if (!_startSerialCommunication())
continue;
if (!_calculateAnchorPosition()) {
printf("One or more distances are missing\n");
ROS_ERROR("[Selflocalisation] One or more distances are missing");
continue;
}
}
......
......@@ -104,7 +104,6 @@ bool d_fall_pps::getAnchorPositions(Anchors::Request& request, Anchors::Response
void d_fall_pps::updateCallback(const std_msgs::Int32 &message)
{
ROS_WARN("received Data");
switch(message.data)
{
case UWB_UPDATE_DISABLE:
......@@ -128,40 +127,9 @@ void d_fall_pps::updateCallback(const std_msgs::Int32 &message)
void d_fall_pps::calibrateAnchors()
{
ROS_WARN("[UWBManagerService] Calibrating Anchors...");
selfLoc.startAnchorLocalistaion();
ROS_WARN("[UWBManagerService] Calibration done!");
/*Serial serial;
DataListener interpreter;
bool useLocalisation = false;
serial.init();
bool running = true;
interpreter.setOffset(6.75, -24.5, -6.0);
useLocalisation = true;
serial.writeSerial("loc100\r");
while (running)
{
serial.manageSerial();
std::string value = serial.readSerial();
while (value != "") {
//printf("\tRECEIVED: %s", value.c_str());
interpreter.checkAndReadData(value);
value = serial.readSerial();
}
if(useLocalisation)
interpreter.calulateAnchorPosition();
}
serial.closeConnection();*/
//readYaml();
if(selfLoc.startAnchorLocalistaion())
ROS_WARN("[UWBManagerService] Calibration successfull!");
else
ROS_ERROR("[UWBManagerService] Calibration failed!");
}
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