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

Added Loading and serving of CF Hangar

parent 07b26147
#ifndef TEACHERSERVICE_H
#define TEACHERSERVICE_H
#include "ros/ros.h"
#include "std_msgs/Int32.h"
#include "d_fall_pps/CrazyflieData.h"
#include "d_fall_pps/CFIndex.h"
#include "StudentSubscriber.h"
//#include "ros/ros.h"
namespace d_fall_pps
{
......@@ -14,13 +17,24 @@ namespace d_fall_pps
std::vector<StudentSubscriber> studentSubscribers;
// Callback once the refresh Student ID button is clicked
void refreshStudents_callback(const std_msgs::Int32 &msg);
void refresh_callback(const std_msgs::Int32 &msg);
// Updates the subscription to the localization data of the students
void subscribeLocalization();
// Loads the CF names and addresses from the Hangar and
// stores them in the LUT
bool loadCFsFromHangar(ros::NodeHandle& nh);
bool serveCFList(CFIndex::Request& request, CFIndex::Response& response);
//void initialiseCFIndex();
ros::Publisher studentIDs_publisher;
ros::Publisher CFData_publisher;
std::map<std::string, std::string> channel_LUT;
}
#endif // TEACHERSERVICE_H included
#include <regex>
#include "ros/ros.h"
#include "std_msgs/Int32MultiArray.h"
#include "d_fall_pps/CrazyfliePositionData.h"
#include "TeacherService.h"
using namespace d_fall_pps;
int main(int argc, char* argv[])
......@@ -16,16 +13,86 @@ int main(int argc, char* argv[])
ros::NodeHandle nodeHandle("~");
ros::NodeHandle namespaceNodehandle = ros::NodeHandle();
ros::Subscriber refreshStudents_subscriber = namespaceNodehandle.subscribe("my_GUI/refreshStudents", 1, refreshStudents_callback);
ros::Subscriber refreshStudents_subscriber = namespaceNodehandle.subscribe("my_GUI/refreshStudents", 1, refresh_callback);
studentIDs_publisher = nodeHandle.advertise<std_msgs::Int32MultiArray>("studentIDs", 1);
CFData_publisher = nodeHandle.advertise<CrazyfliePositionData>("positionData", 1);
ros::ServiceServer CFHangar = nodeHandle.advertiseService("Hangar", serveCFList);
if(!loadCFsFromHangar(nodeHandle))
return -1;
/*for(int i = 0; i < 1000; ++i)
initialiseCFIndex();*/
ros::spin();
return 0;
}
void d_fall_pps::refreshStudents_callback(const std_msgs::Int32 &msg)
bool d_fall_pps::loadCFsFromHangar(ros::NodeHandle& nh)
{
channel_LUT.clear();
XmlRpc::XmlRpcValue cf_list;
bool success = nh.getParam("Hangar", cf_list);
if(!success || cf_list.size() == 0)
{
ROS_ERROR("[TeacherService] No CrazyFlies found in hangar!");
return false;
}
for(unsigned int i = 0; i < cf_list.size(); ++i)
{
std::string address = cf_list[i][1];
if(address == "")
{
ROS_ERROR("[TeacherService] Bad Hangar entry with CF %i", i);
channel_LUT.clear();
return false;
}
channel_LUT.insert(std::pair<std::string, std::string>(cf_list[i][0], cf_list[i][1]));
}
std::stringstream strm;
strm << "\n[TeacherService] " << channel_LUT.size() << " CrazyFlies in hangar:\n";
for(auto it = channel_LUT.begin(); it != channel_LUT.end(); ++it)
strm << "\t" << it->first.c_str() << "\n";
ROS_INFO_STREAM(strm.str());
return true;
}
/*void d_fall_pps::initialiseCFIndex()
{*/
/*ROS_ERROR("init index");
CrazyflieData data;
data.crazyflieName = "HELLO";
data.x = 0;
data.y = 0;
data.z = 0;
data.roll = 0;
data.pitch = 0;
data.yaw = 0;
data.acquiringTime = 0;
data.occluded = false;*/
//CFData_publisher.publish(data);
/*for(auto it = channel_LUT.begin(); it != channel_LUT.end(); ++it)
{
}*/
//}
void d_fall_pps::refresh_callback(const std_msgs::Int32 &msg)
{
// Clear existing buffer
studentIDs.clear();
......@@ -37,7 +104,7 @@ void d_fall_pps::refreshStudents_callback(const std_msgs::Int32 &msg)
// iterate through all nodes to find PPSClient's
for(int i = 0; i < v_str.size(); i++)
{
std::string s = v_str[i];
std::string s = v_str[i];
std::smatch m;
std::regex e ("\\/(\\d)\\/PPSClient");
......@@ -67,6 +134,31 @@ void d_fall_pps::refreshStudents_callback(const std_msgs::Int32 &msg)
ROS_INFO("[TeacherService] updated Student IDs");
}
bool d_fall_pps::serveCFList(CFIndex::Request& request, CFIndex::Response& response)
{
if(request.cfName != "")
{
auto it = channel_LUT.find(request.cfName);
if(it == channel_LUT.end())
return false;
response.length = 1;
response.data.push_back(it->second);
return true;
}
else
{
response.length = channel_LUT.size();
for(auto it = channel_LUT.begin(); it != channel_LUT.end(); ++it)
response.data.push_back(it->first);
return true;
}
}
void d_fall_pps::subscribeLocalization()
{
ros::NodeHandle nodeHandle("~");
......@@ -78,7 +170,7 @@ void d_fall_pps::subscribeLocalization()
for(std::vector<int>::const_iterator it = studentIDs.begin(); it != studentIDs.end(); ++it)
{
ROS_ERROR("Adding %i", (*it));
ROS_WARN("Adding Student %i.", (*it));
std::string address = "/";
address.append(std::to_string(*it));
......@@ -91,7 +183,6 @@ void d_fall_pps::subscribeLocalization()
StudentSubscriber sub = StudentSubscriber(8, "/8/UWBDataPublisher/UWBData");
studentSubscribers.push_back(sub);
for(std::vector<StudentSubscriber>::iterator it = studentSubscribers.begin(); it != studentSubscribers.end(); ++it)
(*it).subscribe(nodeHandle);
}
......
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