Commit 494c8176 authored by tiagos's avatar tiagos
Browse files

UWBServiceClient delivers UWB Status and Teacher GUI triggers update

parent 9069a45a
......@@ -175,11 +175,14 @@ private:
std::vector<crazyFly*> cf_positionsVector;
CFLinker* cf_linker;
ros::ServiceClient UWBServiceClient;
ros::Publisher DBChangedPublisher;
ros::Publisher emergencyStopPublisher;
ros::Publisher refreshStudents_publisher;
ros::Publisher UWBSettings_publisher;
ros::Publisher UWBServiceClientUpdate_publisher;
#endif
......
......@@ -36,6 +36,7 @@
#include "d_fall_pps/CMUpdate.h"
#include "d_fall_pps/CMCommand.h"
#include "CentralManagerService.h"
#include "d_fall_pps/Anchors.h"
#include <ros/ros.h>
#include <ros/network.h>
......@@ -222,6 +223,9 @@ void MainGUIWindow::_init()
QObject::connect(cf_linker, SIGNAL(updateComboBoxes()), this, SLOT(updateComboBoxes()));
ros::NodeHandle nodeHandle("~");
UWBServiceClient = nodeHandle.serviceClient<Anchors>("/UWBServiceClient/UWBData", false);
DBChangedPublisher = nodeHandle.advertise<std_msgs::Int32>("DBChanged", 1);
emergencyStopPublisher = nodeHandle.advertise<std_msgs::Int32>("emergencyStop", 1);
......@@ -977,9 +981,26 @@ void MainGUIWindow::on_checkBox_enable_UWB_toggled(bool checked)
{
// Send Command to enable UWB to UWBServiceClient
std_msgs::Int32 server_msg;
server_msg.data = UWB_UPDATE_ENABLE;
if(checked)
server_msg.data = UWB_UPDATE_ENABLE;
else
server_msg.data = UWB_UPDATE_DISABLE;
UWBServiceClientUpdate_publisher.publish(server_msg);
Anchors a;
if(UWBServiceClient.call(a))
{
ui->checkBox_enable_UWB->setEnabled(a.enableUWB);
// Update Anchor positions
}
else
{
ui->checkBox_enable_UWB->setEnabled(!checked);
}
// Publish message that clients have to reload data from UWBServiceClient
......
......@@ -2,6 +2,10 @@
<node pkg="d_fall_pps" name="CentralManagerService" output="screen" type="CentralManagerService">
</node>
<node pkg="d_fall_pps" name="UWBServiceClient" output="screen" type="UWBServiceClient">
<rosparam command="load" file="$(find d_fall_pps)/param/AnchorPos.yaml" />
</node>
<node pkg="d_fall_pps" name="ViconDataPublisher" output="screen" type="ViconDataPublisher">
<rosparam command="load" file="$(find d_fall_pps)/param/ViconConfig.yaml" />
</node>
......
......@@ -29,16 +29,16 @@ using namespace d_fall_pps;
bool enableUWB;
UWBAnchorArray anchors;
ros::NodeHandle nodeHandle("~");
//ros::NodeHandle nodeHandle;//("~");
int main(int argc, char* argv[])
{
ros::init(argc, argv, "AnchorPositions");
//ros::NodeHandle nodeHandle("~");
ros::init(argc, argv, "UWBServiceClient");
ros::NodeHandle nodeHandle("~");
readYaml();
ros::ServiceServer uwbService = nodeHandle.advertiseService("UWBServiceClient", getAnchorPositions);
ros::ServiceServer uwbService = nodeHandle.advertiseService("UWBData", getAnchorPositions);
ros::Subscriber uwbUpdate = nodeHandle.subscribe("/my_GUI/UWBUpdate", 1, updateCallback);
......@@ -59,6 +59,7 @@ void d_fall_pps::loadParameterFloatVector(ros::NodeHandle& nodeHandle, std::stri
void d_fall_pps::readYaml()
{
ros::NodeHandle nodeHandle("~");
int length;
if(!nodeHandle.getParam("anchorLength", length))
{
......
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