From 66921fc210e29691cb43a3dabaa4730ae6f2d8fb Mon Sep 17 00:00:00 2001
From: bucyril <bucyril@ethz.ch>
Date: Mon, 22 May 2017 10:18:30 +0200
Subject: [PATCH] PPSClient can establish connection to CentralManager
---
pps_ws/src/d_fall_pps/launch/Student.launch | 1 +
pps_ws/src/d_fall_pps/src/PPSClient.cpp | 24 +++++++++++++------
.../src/d_fall_pps/src/ViconDataPublisher.cpp | 1 -
3 files changed, 18 insertions(+), 8 deletions(-)
diff --git a/pps_ws/src/d_fall_pps/launch/Student.launch b/pps_ws/src/d_fall_pps/launch/Student.launch
index fbfde6ea..555914a1 100755
--- a/pps_ws/src/d_fall_pps/launch/Student.launch
+++ b/pps_ws/src/d_fall_pps/launch/Student.launch
@@ -6,6 +6,7 @@
<node pkg="d_fall_pps" name="PPSClient" output="screen" type="PPSClient">
<rosparam command="load" file="$(find d_fall_pps)/param/ClientConfig.yaml" />
+ <param name="studentID" value="$(optenv ROS_NAMESPACE)" />
</node>
<node pkg="d_fall_pps" name="SafeControllerService" output="screen" type="SafeControllerService">
diff --git a/pps_ws/src/d_fall_pps/src/PPSClient.cpp b/pps_ws/src/d_fall_pps/src/PPSClient.cpp
index 725f81a7..d15ab046 100755
--- a/pps_ws/src/d_fall_pps/src/PPSClient.cpp
+++ b/pps_ws/src/d_fall_pps/src/PPSClient.cpp
@@ -21,7 +21,7 @@
#include <ros/package.h>
#include "d_fall_pps/Controller.h"
-#include "d_fall_pps/CentralManager.h"
+#include "d_fall_pps/CMQuery.h"
#include "d_fall_pps/ViconData.h"
#include "d_fall_pps/CrazyflieData.h"
@@ -43,6 +43,8 @@ using namespace d_fall_pps;
std::string teamName;
//name of the crazyflie, as specified in Vicon
std::string crazyflieName;
+//studentID, gives namespace and identifier in CentralManagerService
+int studentID;
//the safe controller specified in the ClientConfig.yaml, is considered trusted
ros::ServiceClient safeController;
@@ -140,15 +142,23 @@ void loadParameters(ros::NodeHandle& nodeHandle) {
if(!nodeHandle.getParam("crazyFlieName", crazyflieName)) {
ROS_ERROR("Failed to get crazyFlieName");
}
+
+ if(!nodeHandle.getParam("studentID", studentID)) {
+ ROS_ERROR("Failed to get studentID");
+ }
}
void loadCrazyflieContext() {
- CentralManager centralManagerCall;
- if(centralManager.call(centralManagerCall)) {
- context = centralManagerCall.response.context;
- ROS_INFO("CrazyflieContext obtained");
+ CMQuery contextCall;
+ contextCall.request.studentID = studentID;
+
+ centralManager.waitForExistence(ros::Duration(-1));
+
+ if(centralManager.call(contextCall)) {
+ context = contextCall.response.crazyflieContext;
+ ROS_INFO_STREAM("CrazyflieContext:" << context);
} else {
- ROS_ERROR("Failed to call CentralManagerService");
+ ROS_ERROR("Failed to load context");
}
}
@@ -213,7 +223,7 @@ int main(int argc, char* argv[]){
//ros::service::waitForService("/CentralManagerService/CentralManager");
- centralManager = nodeHandle.serviceClient<CentralManager>("/CentralManagerService/CentralManager");
+ centralManager = nodeHandle.serviceClient<CMQuery>("/CentralManagerService/Query", false);
loadCrazyflieContext();
//keeps 100 messages because otherwise ViconDataPublisher would override the data immediately
diff --git a/pps_ws/src/d_fall_pps/src/ViconDataPublisher.cpp b/pps_ws/src/d_fall_pps/src/ViconDataPublisher.cpp
index 7d4e4026..d8c07a89 100755
--- a/pps_ws/src/d_fall_pps/src/ViconDataPublisher.cpp
+++ b/pps_ws/src/d_fall_pps/src/ViconDataPublisher.cpp
@@ -20,7 +20,6 @@
#include "d_fall_pps/ViconData.h"
#include "d_fall_pps/UnlabeledMarker.h"
-#define TESTING_FAKE_DATA
// notice that unit here are in milimeters
using namespace ViconDataStreamSDK::CPP;
--
GitLab