Commit 60311a71 authored by tiagos's avatar tiagos
Browse files

Teacher GUI receives position data from every nodes localization server

parent 4b63ea8e
......@@ -125,6 +125,7 @@ add_message_files(
FILES
UnlabeledMarker.msg
CrazyflieData.msg
CrazyfliePositionData.msg
ViconData.msg
ControlCommand.msg
CrazyflieContext.msg
......@@ -234,6 +235,7 @@ include_directories(
## The recommended prefix ensures that target names across packages don't collide
# add_executable(${PROJECT_NAME}_node src/d_fall_pps_node.cpp)
add_executable(ViconDataPublisher src/ViconDataPublisher.cpp)
add_executable(TeacherService src/TeacherService.cpp)
add_executable(LocalizationServer src/LocalizationServer.cpp)
add_executable(PPSClient src/PPSClient.cpp)
add_executable(Client src/Client.cpp)
......@@ -285,6 +287,7 @@ qt5_use_modules(student_GUI Widgets)
add_dependencies(ViconDataPublisher d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(LocalizationServer d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(TeacherService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(PPSClient d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(Client d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(SafeControllerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
......@@ -321,6 +324,7 @@ target_link_libraries(ViconDataPublisher ${catkin_LIBRARIES})
target_link_libraries(ViconDataPublisher ${VICON_LIBRARY})
target_link_libraries(LocalizationServer ${catkin_LIBRARIES})
target_link_libraries(TeacherService ${catkin_LIBRARIES})
target_link_libraries(PPSClient ${catkin_LIBRARIES})
......
#ifndef TEACHERSERVICE_H
#define TEACHERSERVICE_H
#include "std_msgs/Int32.h"
#include "d_fall_pps/CrazyflieData.h"
#include "StudentSubscriber.h"
//#include "ros/ros.h"
namespace d_fall_pps
{
// Stores the connected student IDs
std::vector<int> studentIDs;
// and subscriptions
std::vector<StudentSubscriber> studentSubscribers;
// Callback once the refresh Student ID button is clicked
void refreshStudents_callback(const std_msgs::Int32 &msg);
// Updates the subscription to the localization data of the students
void subscribeLocalization();
ros::Publisher studentIDs_publisher;
ros::Publisher CFData_publisher;
}
#endif // TEACHERSERVICE_H included
\ No newline at end of file
......@@ -34,6 +34,7 @@
#include "d_fall_pps/CrazyflieEntry.h"
#include <std_msgs/Int32.h>
#include <std_msgs/Int32MultiArray.h>
#define CMD_CRAZYFLY_MOTORS_OFF 5
......@@ -131,6 +132,10 @@ private slots:
#ifdef CATKIN_MAKE
void updateNewViconData(const ptrToMessage& p_msg);
void updateStudentIDs(const std_msgs::Int32MultiArray& ids);
void updateCFPositions(const CrazyfliePositionData& data);
#endif
void on_checkBox_vicon_crazyflies_toggled(bool checked);
......@@ -163,10 +168,14 @@ private:
rosNodeThread* _rosNodeThread;
std::vector<Marker*> markers_vector;
std::vector<crazyFly*> crazyflies_vector;
std::vector<crazyFly*> cf_positionsVector;
CFLinker* cf_linker;
ros::Publisher DBChangedPublisher;
ros::Publisher emergencyStopPublisher;
ros::Publisher refreshStudents_publisher;
#endif
void updateComboBoxesCFs();
......
......@@ -30,8 +30,11 @@
#include <ros/network.h>
#include "d_fall_pps/UnlabeledMarker.h"
#include "d_fall_pps/CrazyflieData.h"
#include "d_fall_pps/CrazyfliePositionData.h"
#include "d_fall_pps/ViconData.h"
#include <std_msgs/Int32MultiArray.h>
using namespace d_fall_pps;
typedef ViconData::ConstPtr ptrToMessage;
......@@ -49,6 +52,8 @@ public:
// void messageCallback(const ViconData& data);
void messageCallback(const ptrToMessage& p_msg);
void studentIDCallback(const std_msgs::Int32MultiArray &ids);
void positionDataCallback(const CrazyfliePositionData& data);
ros::ServiceClient m_read_db_client;
ros::ServiceClient m_update_db_client;
......@@ -57,6 +62,8 @@ public:
signals:
void newViconData(const ptrToMessage& p_msg);
void newStudentIDs(const std_msgs::Int32MultiArray &ids);
void newPositionData(const CrazyfliePositionData& data);
public slots:
void run();
......@@ -71,6 +78,8 @@ private:
ViconData m_vicon_data;
ros::Subscriber m_vicon_subscriber;
ros::Subscriber studentID_subscriber;
ros::Subscriber positionData_subscriber;
// ros::Publisher sim_velocity;
};
#endif
......
......@@ -37,4 +37,7 @@ std::map<std::string, std::string> channel_LUT
{"PPS_CF08", "0/56/2M/E7E7E7E708"},
{"PPS_CF09", "0/56/2M/E7E7E7E709"},
{"PPS_CF10", "0/56/2M/E7E7E7E70A"},
{"uwbtest", "0/80/2M/E7E7E7E7E7"},
//{"UWB_CF01", "0/56/2M/E7E7E7E7E7"},
{"UWB_CF02", "0/70/2M/E7E7E7E7E7"},
};
......@@ -126,6 +126,11 @@ void MainGUIWindow::doNumCrazyFlyZonesChanged(int n)
updateComboBoxesCFZones();
}
void demo(const std_msgs::Int32MultiArray &ids)
{
ROS_ERROR("Callback");
}
void MainGUIWindow::_init()
{
// initialize checkboxes, spinboxes,....
......@@ -210,12 +215,19 @@ void MainGUIWindow::_init()
#ifdef CATKIN_MAKE
_rosNodeThread->init();
qRegisterMetaType<ptrToMessage>("ptrToMessage");
qRegisterMetaType<std_msgs::Int32MultiArray>("std_msgs::Int32MultiArray");
qRegisterMetaType<CrazyfliePositionData>("CrazyfliePositionData");
QObject::connect(_rosNodeThread, SIGNAL(newViconData(const ptrToMessage&)), this, SLOT(updateNewViconData(const ptrToMessage&)));
QObject::connect(_rosNodeThread, SIGNAL(newStudentIDs(const std_msgs::Int32MultiArray&)), this, SLOT(updateStudentIDs(const std_msgs::Int32MultiArray&)));
QObject::connect(_rosNodeThread, SIGNAL(newPositionData(const CrazyfliePositionData&)), this, SLOT(updateCFPositions(const CrazyfliePositionData&)));
QObject::connect(cf_linker, SIGNAL(updateComboBoxes()), this, SLOT(updateComboBoxes()));
ros::NodeHandle nodeHandle("~");
DBChangedPublisher = nodeHandle.advertise<std_msgs::Int32>("DBChanged", 1);
emergencyStopPublisher = nodeHandle.advertise<std_msgs::Int32>("emergencyStop", 1);
refreshStudents_publisher = nodeHandle.advertise<std_msgs::Int32>("refreshStudents", 1);
#endif
}
......@@ -274,9 +286,62 @@ void MainGUIWindow::updateComboBoxesCFZones()
#ifdef CATKIN_MAKE
void MainGUIWindow::updateNewViconData(const ptrToMessage& p_msg) //connected to newViconData, from node
void MainGUIWindow::updateCFPositions(const CrazyfliePositionData& data)
{
bool inScene = false;
int index;
// Reconvert CFPD to CFD
CrazyflieData cfdata;
cfdata.crazyflieName = std::to_string(data.id);
cfdata.x = data.x;
cfdata.y = data.y;
cfdata.z = data.z;
cfdata.roll = data.roll;
cfdata.pitch = data.pitch;
cfdata.yaw = data.yaw;
cfdata.acquiringTime = data.acquiringTime;
cfdata.occluded = false;
for(int i = 0; i < cf_positionsVector.size(); ++i)
{
if(cf_positionsVector[i]->getName() == cfdata.crazyflieName)
{
cf_positionsVector[i]->updateCF(&cfdata);
inScene = true;
index = i;
break;
}
}
if(!inScene)
{
QString filename(":/images/center_rect.svg");
crazyFly* tmp_p_crazyfly = new crazyFly(&cfdata, filename);
cf_positionsVector.push_back(tmp_p_crazyfly);
index = cf_positionsVector.size() - 1;
}
if(ui->checkBox_vicon_crazyflies->checkState() == Qt::Checked)
{
if(!cf_positionsVector[index]->isAddedToScene())
{
scene->addItem(cf_positionsVector[index]);
cf_positionsVector[index]->setAddedToScene(true);
}
}
}
void MainGUIWindow::updateStudentIDs(const std_msgs::Int32MultiArray& ids)
{
ui->list_discovered_student_ids->clear();
for(int i = 0; i < ids.data.size(); ++i)
ui->list_discovered_student_ids->addItem(std::to_string(ids.data[i]).c_str());
}
void MainGUIWindow::updateNewViconData(const ptrToMessage& p_msg) //connected to newViconData, from node
{
// update Markers
if(p_msg->markers.size() < markers_vector.size()) // some markers have dissapeared, received stuff is smaller than what we have
......@@ -656,36 +721,12 @@ void MainGUIWindow::on_refresh_cfs_button_clicked()
void MainGUIWindow::on_refresh_student_ids_button_clicked()
{
#ifdef CATKIN_MAKE
ui->list_discovered_student_ids->clear();
// \/(\d)\/PPSClient
ros::V_string v_str;
ros::master::getNodes(v_str);
for(int i = 0; i < v_str.size(); i++)
{
std::string s = v_str[i];
std::smatch m;
std::regex e ("\\/(\\d)\\/PPSClient");
// std::regex e("\\/PPSClien(.)");
// while(std::regex_search(s, m, e))
// {
// for (int i = 0; i < m.size(); i++)
// {
// ROS_INFO("FOUND: %s", m[i].str().c_str());
// // std::cout << "FOUND" << m[i] << "\n";
// }
// s = m.suffix().str();
// }
// Publish the button being pressed
std_msgs::Int32 msg;
msg.data = 1;
this->refreshStudents_publisher.publish(msg);
if(std::regex_search(s, m, e))
{
// ROS_INFO("===============================================FOUND: %s", m[1].str().c_str()); // one because we are interested ONLY in the first match
std::string found_string = m[1].str();
ui->list_discovered_student_ids->addItem(found_string.c_str());
}
}
#endif
}
......
......@@ -61,6 +61,8 @@ bool rosNodeThread::init()
ros::NodeHandle nh("~");
m_vicon_subscriber = nh.subscribe("/ViconDataPublisher/ViconData", 100, &rosNodeThread::messageCallback, this);
studentID_subscriber = nh.subscribe("/TeacherService/studentIDs", 1, &rosNodeThread::studentIDCallback, this);
positionData_subscriber = nh.subscribe("/TeacherService/positionData", 1, &rosNodeThread::positionDataCallback, this);
// clients for db services:
m_read_db_client = nh.serviceClient<CMRead>("/CentralManagerService/Read", false);
......@@ -71,11 +73,21 @@ bool rosNodeThread::init()
return true;
} // set up the thread
void rosNodeThread::studentIDCallback(const std_msgs::Int32MultiArray &ids)
{
emit newStudentIDs(ids);
}
void rosNodeThread::messageCallback(const ptrToMessage& p_msg) // When a message arrives to the topic, this callback is executed
{
emit newViconData(p_msg); //pass the message to other places
}
void rosNodeThread::positionDataCallback(const CrazyfliePositionData& data)
{
emit newPositionData(data);
}
// void rosNodeThread::messageCallback(const ViconData& data) // When a message arrives to the topic, this callback is executed
// {
// QMutex * pMutex = new QMutex();
......
#ifndef STUDENTSUBSCRIBER_H
#define STUDENTSUBSCRIBER_H
namespace d_fall_pps
{
class StudentSubscriber
{
public:
// Constructor
StudentSubscriber(int id, std::string address);
// Somehow the subscription didn't work inside the constructor
void subscribe(ros::NodeHandle& handle);
// Just to cleanly unsubscribe from the topics
void unsubscribe();
// The callback once data arrives from this node
void callback(const CrazyflieData& data);
private:
int _studentID;
std::string _address;
ros::Subscriber _subscriber;
};
}
#endif
\ No newline at end of file
<launch>
<node pkg="d_fall_pps" name="CentralManagerService" output="screen" type="CentralManagerService">
</node>
<node pkg="d_fall_pps" name="ViconDataPublisher" output="screen" type="ViconDataPublisher">
<rosparam command="load" file="$(find d_fall_pps)/param/ViconConfig.yaml" />
</node>
<!-- When we have a GUI, this has to be adapted and included -->
<node pkg="d_fall_pps" name="my_GUI" output="screen" type="my_GUI">
<!-- <rosparam command="load" file="$(find d_fall_pps)/launch/ControlParams.yaml" /> -->
</node>
<node pkg="d_fall_pps" name="UWB_BaseStation" output="screen" type="UWB_BaseStation">
</node>
<node pkg="d_fall_pps" name="TeacherService" output="screen" type="TeacherService">
</node>
</launch>
int32 id
float64 x
float64 y
float64 z
float64 roll
float64 pitch
float64 yaw
float64 acquiringTime #delta t
\ No newline at end of file
5,PPS_CF05,0/32/2M/E7E7E7E705,0,-0.868587,-0.673046,-0.2,0.686782,0.686782,2
8,UWB_CF02,0/70/2M/E7E7E7E7E7,0,-0.7,-0.7,-0.2,1.07,0.58,2
9,uwbtest,0/80/2M,0,0,-0,-0.2,0,-0,2
#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[])
{
ros::init(argc, argv, "TeacherService");
ros::NodeHandle nodeHandle("~");
ros::NodeHandle namespaceNodehandle = ros::NodeHandle();
ros::Subscriber refreshStudents_subscriber = namespaceNodehandle.subscribe("my_GUI/refreshStudents", 1, refreshStudents_callback);
studentIDs_publisher = nodeHandle.advertise<std_msgs::Int32MultiArray>("studentIDs", 1);
CFData_publisher = nodeHandle.advertise<CrazyfliePositionData>("positionData", 1);
ros::spin();
return 0;
}
void d_fall_pps::refreshStudents_callback(const std_msgs::Int32 &msg)
{
// Clear existing buffer
studentIDs.clear();
// Poll for all connected nodes
ros::V_string v_str;
ros::master::getNodes(v_str);
// iterate through all nodes to find PPSClient's
for(int i = 0; i < v_str.size(); i++)
{
std::string s = v_str[i];
std::smatch m;
std::regex e ("\\/(\\d)\\/PPSClient");
if(std::regex_search(s, m, e))
{
std::string found_string = m[1].str();
int studentID = std::stoi(found_string); // one because we are interested ONLY in the first match
studentIDs.push_back(studentID);
}
}
std::sort(studentIDs.begin(), studentIDs.end());
// Prepare the data:
std_msgs::Int32MultiArray publish_msg;
for(std::vector<int>::const_iterator it = studentIDs.begin(); it != studentIDs.end(); ++it)
publish_msg.data.push_back(*it);
// Publish the data for the GUI
studentIDs_publisher.publish(publish_msg);
// Update subscriptions to the clients
subscribeLocalization();
ROS_INFO("[TeacherService] updated Student IDs");
}
void d_fall_pps::subscribeLocalization()
{
ros::NodeHandle nodeHandle("~");
for(std::vector<StudentSubscriber>::iterator it = studentSubscribers.begin(); it != studentSubscribers.end(); ++it)
(*it).unsubscribe();
studentSubscribers.clear();
for(std::vector<int>::const_iterator it = studentIDs.begin(); it != studentIDs.end(); ++it)
{
ROS_ERROR("Adding %i", (*it));
std::string address = "/";
address.append(std::to_string(*it));
address.append("/LocalizationServer/LocalizationData");
StudentSubscriber sub = StudentSubscriber(*it, address);
studentSubscribers.push_back(sub);
}
for(std::vector<StudentSubscriber>::iterator it = studentSubscribers.begin(); it != studentSubscribers.end(); ++it)
(*it).subscribe(nodeHandle);
}
d_fall_pps::StudentSubscriber::StudentSubscriber(int id, std::string address)
{
this->_studentID = id;
this->_address = address;
}
void d_fall_pps::StudentSubscriber::subscribe(ros::NodeHandle& handle)
{
this->_subscriber = handle.subscribe(this->_address, 1, &StudentSubscriber::callback, this);
}
void d_fall_pps::StudentSubscriber::unsubscribe()
{
this->_subscriber.shutdown();
}
void d_fall_pps::StudentSubscriber::callback(const CrazyflieData& data)
{
CrazyfliePositionData pos;
pos.id = this->_studentID;
pos.x = data.x;
pos.y = data.y;
pos.z = data.z;
pos.roll = data.roll;
pos.pitch = data.pitch;
pos.yaw = data.yaw;
pos.acquiringTime = data.acquiringTime;
CFData_publisher.publish(pos);
}
\ No newline at end of file
#include "ros/ros.h"
#include <stdlib.h>
#include <std_msgs/String.h>
#include <rosbag/bag.h>
#include <ros/package.h>
//using namespace d_fall_pps
int main(int argc, char* argv[])
{
ros::init(argc, argv, "UWB_BaseStation");
ROS_INFO("STARTED UWB BASE STATION NODE");
return 0;
}
\ No newline at end of file
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