Commit 4587834e authored by Yvan Bosshard's avatar Yvan Bosshard
Browse files

Added localization server and configured client to read only relayed data from the server

parent 213d8ff5
/*
*
* UWB Data Validator
*
* This node is subscribed to the ViconDataPublisher as well
* as to the UWBDataPublisher. It is used to compare the
* data and relay the desired localization source to the
* PPSClient.
*
*/
#ifndef LOCALIZATIONSERVER_H
#define LOCALIZATIONSERVER_H
#include "std_msgs/Int32.h"
#include "ros/ros.h"
#include "d_fall_pps/ViconData.h"
namespace d_fall_pps
{
// Loads the parameters of the node handle
bool loadParameters(ros::NodeHandle &nodeHandle);
// Loads the current context of the client
void loadCrazyflieContext();
// Callback function to handle incoming Vicon data
void viconDataCallback(const ViconData &viconData);
// Callback function to handle the change in database information
void dbChangedCallback(const std_msgs::Int32 &msg);
}
#endif // LOCALIZATIONSERVER_H included
\ No newline at end of file
<launch>
<node pkg="d_fall_pps" name="CrazyRadio" output="screen" type="CrazyRadio.py">
<rosparam command="load" file="$(find d_fall_pps)/param/ClientConfig.yaml" />
</node>
<node pkg="d_fall_pps" name="Client" output="screen" type="Client">
<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="LocalizationServer" output="screen" type="LocalizationServer">
<param name="clientID" value="$(optenv ROS_NAMESPACE)" />
</node>
<node pkg="d_fall_pps" name="SafeControllerService" output="screen" type="SafeControllerService">
<rosparam command="load" file="$(find d_fall_pps)/param/SafeController.yaml" />
</node>
<node pkg="d_fall_pps" name="CustomControllerService" output="screen" type="CustomControllerService">
<rosparam command="load" file="$(find d_fall_pps)/param/CustomController.yaml" />
</node>
<!-- <node pkg="d_fall_pps" name="UWBDataValidator" output="screen" type="UWBDataValidator">-->
<!--<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="student_GUI" output="screen" type="student_GUI">
<!-- <rosparam command="load" file="$(find d_fall_pps)/launch/ControlParams.yaml" /> -->
</node>
</launch>
This diff is collapsed.
/*
*
* Localization Server
*
* This node is subscribed to the ViconDataPublisher as well
* as to the UWBDataPublisher. It is used to relay the data
* of the desired localization source to the PPSClient and
* can compare the data provided by both.
*
*/
#include <stdlib.h>
#include <std_msgs/String.h>
#include <rosbag/bag.h>
#include <ros/package.h>
#include "LocalizationServer.h"
#include "d_fall_pps/CMQuery.h"
#include "d_fall_pps/CrazyflieData.h"
#include "d_fall_pps/CrazyflieContext.h"
#include "std_msgs/Float32.h"
using namespace d_fall_pps;
// the clientID defines namespace and identifier in the CentralManagerService
int clientID;
// values for safetyCheck
bool strictSafety;
// Self explanatory variables
ros::ServiceClient centralManager;
ros::Publisher localizationPublisher;
//std::string ros_namespace; // Needed?
CrazyflieContext context;
rosbag::Bag bag;
int main(int argc, char* argv[])
{
ros::init(argc, argv, "LocalizationServer");
ros::NodeHandle nodeHandle("~");
//ros_namespace = ros::this_node::getNamespace(); // Needed?
// load context parameters
if(!loadParameters(nodeHandle))
return -1;
centralManager = nodeHandle.serviceClient<CMQuery>("/CentralManagerService/Query", false);
loadCrazyflieContext();
ros::Subscriber viconSubscriber = nodeHandle.subscribe("/ViconDataPublisher/ViconData", 100, viconDataCallback);
ROS_INFO_STREAM("LocalizationServer connected to Vicon");
localizationPublisher = nodeHandle.advertise<CrazyflieData>("LocalizationData", 1);
ros::NodeHandle namespaceNodeHandle = ros::NodeHandle();
ros::Subscriber dbChangedSubscriber = namespaceNodeHandle.subscribe("/my_GUI/DBChanged", 1, dbChangedCallback);
ROS_INFO_STREAM("LocalizationServer started!");
std::string package_path;
package_path = ros::package::getPath("d_fall_pps") + "/";
std::string record_file = package_path + "log/LocalizationServerLog.bag";
bag.open(record_file, rosbag::bagmode::Write);
ros::spin();
bag.close();
return 0;
}
bool d_fall_pps::loadParameters(ros::NodeHandle &nodeHandle)
{
if(!nodeHandle.getParam("clientID", clientID))
{
ROS_ERROR("Failed to get clientID!");
return false;
}
return true;
}
void d_fall_pps::loadCrazyflieContext()
{
CMQuery contextCall;
contextCall.request.studentID = clientID;
centralManager.waitForExistence(ros::Duration(-1));
if(centralManager.call(contextCall))
{
context =contextCall.response.crazyflieContext;
ROS_INFO_STREAM("LocalizationServer context updated");
}
}
void d_fall_pps::viconDataCallback(const ViconData &viconData)
{
for(std::vector<CrazyflieData>::const_iterator it = viconData.crazyflies.begin(); it != viconData.crazyflies.end(); ++it)
{
CrazyflieData data = *it;
if(data.crazyflieName == context.crazyflieName)
{
localizationPublisher.publish(data);
bag.write("ViconData", ros::Time::now(), data);
// leave the for-loop so not every vector element has to be compared, once the matching context is found
break;
}
}
}
void d_fall_pps::dbChangedCallback(const std_msgs::Int32 &msg)
{
d_fall_pps::loadCrazyflieContext();
}
\ No newline at end of file
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