Commit 5eaa27f8 authored by tiagos's avatar tiagos
Browse files

Loading Anchor file and GUI Update works now like a charm

parent 494c8176
...@@ -989,29 +989,44 @@ void MainGUIWindow::on_checkBox_enable_UWB_toggled(bool checked) ...@@ -989,29 +989,44 @@ void MainGUIWindow::on_checkBox_enable_UWB_toggled(bool checked)
UWBServiceClientUpdate_publisher.publish(server_msg); UWBServiceClientUpdate_publisher.publish(server_msg);
// Receive the updated information
Anchors a; Anchors a;
if(UWBServiceClient.call(a)) if(UWBServiceClient.call(a))
{ {
ui->checkBox_enable_UWB->setEnabled(a.enableUWB); ui->checkBox_enable_UWB->setChecked(a.response.enableUWB);
// Update Anchor positions
// print anchor positions in list_anchors
ui->list_anchors->clear();
std::ostringstream strm;
strm << "id\tx\ty\tz\n";
for(int i = 0; i < a.response.anchorArray.length; ++i)
{
strm << a.response.anchorArray.data[i].id << "\t" << a.response.anchorArray.data[i].x << "\t"
<< a.response.anchorArray.data[i].y << "\t" << a.response.anchorArray.data[i].z << "\n";
//ROS_ERROR("Anchor %i, %f, %f, %f", , , , );
}
ui->list_anchors->addItem(QString::fromStdString(strm.str()));
} }
else else
{ {
ui->checkBox_enable_UWB->setEnabled(!checked); ui->checkBox_enable_UWB->setChecked(!checked);
} }
// Publish message that clients have to reload data from UWBServiceClient // Publish message that clients have to reload data from UWBServiceClient
//std_msgs::Int32 msg; std_msgs::Int32 msg;
//msg.data = 1; msg.data = 1;
// UWBSettings_publisher.publish(msg); UWBSettings_publisher.publish(msg);
} }
void MainGUIWindow::on_reloadAnchors_button_pressed() void MainGUIWindow::on_reloadAnchors_button_pressed()
{ {
// Load Data from UWBServiceClient // Load Data from UWBServiceClient
ui->list_anchors->addItem(QString::fromStdString("Hello Michael"));//clear();
// Publish message that clients have to reload data from UWBServiceClient // Publish message that clients have to reload data from UWBServiceClient
std_msgs::Int32 msg; std_msgs::Int32 msg;
......
...@@ -2,7 +2,7 @@ anchorLength: 3 # Update this number after adding/removing anchors!!! ...@@ -2,7 +2,7 @@ anchorLength: 3 # Update this number after adding/removing anchors!!!
# Structure: anchorID [x, y, z] (ID is always from 0 to length-1) # Structure: anchorID [x, y, z] (ID is always from 0 to length-1)
anchor0: [0.0, 0.0, 0.0] anchor0: [0.0, 0.0, -1.25]
anchor1: [0.0, 1.0, 0.0] anchor1: [0.0, 1.0, 0.0]
anchor2: [1.0, 0.0, 0.0] anchor2: [1.0, 0.0, 0.0]
...@@ -68,6 +68,7 @@ void d_fall_pps::readYaml() ...@@ -68,6 +68,7 @@ void d_fall_pps::readYaml()
} }
anchors.length = length; anchors.length = length;
anchors.data.clear();
std::vector<float> anchorArr; std::vector<float> anchorArr;
UWBAnchor tempAnchor; UWBAnchor tempAnchor;
...@@ -111,5 +112,4 @@ void d_fall_pps::updateCallback(const std_msgs::Int32 &message) ...@@ -111,5 +112,4 @@ void d_fall_pps::updateCallback(const std_msgs::Int32 &message)
default: default:
ROS_WARN("[UWBServiceClient] unknown update command"); ROS_WARN("[UWBServiceClient] unknown update command");
} }
ROS_ERROR("it works!!!");
} }
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