Commit 74908be7 authored by roangel's avatar roangel
Browse files

First try DBChanged topic. Need to test it

parent 7a14460f
...@@ -17,6 +17,8 @@ ...@@ -17,6 +17,8 @@
#include "d_fall_pps/CrazyflieDB.h" #include "d_fall_pps/CrazyflieDB.h"
#include "d_fall_pps/CrazyflieEntry.h" #include "d_fall_pps/CrazyflieEntry.h"
#include <std_msgs/Int32.h>
using namespace d_fall_pps; using namespace d_fall_pps;
#endif #endif
...@@ -142,6 +144,8 @@ private: ...@@ -142,6 +144,8 @@ private:
std::vector<Marker*> markers_vector; std::vector<Marker*> markers_vector;
std::vector<crazyFly*> crazyflies_vector; std::vector<crazyFly*> crazyflies_vector;
CFLinker* cf_linker; CFLinker* cf_linker;
ros::Publisher DBChangedPublisher;
#endif #endif
void updateComboBoxesCFs(); void updateComboBoxesCFs();
......
...@@ -20,6 +20,10 @@ ...@@ -20,6 +20,10 @@
#include "d_fall_pps/CMUpdate.h" #include "d_fall_pps/CMUpdate.h"
#include "d_fall_pps/CMCommand.h" #include "d_fall_pps/CMCommand.h"
#include "CentralManagerService.h" #include "CentralManagerService.h"
#include <ros/ros.h>
#include <ros/network.h>
#endif #endif
#include <string> #include <string>
...@@ -191,6 +195,9 @@ void MainGUIWindow::_init() ...@@ -191,6 +195,9 @@ void MainGUIWindow::_init()
qRegisterMetaType<ptrToMessage>("ptrToMessage"); qRegisterMetaType<ptrToMessage>("ptrToMessage");
QObject::connect(_rosNodeThread, SIGNAL(newViconData(const ptrToMessage&)), this, SLOT(updateNewViconData(const ptrToMessage&))); QObject::connect(_rosNodeThread, SIGNAL(newViconData(const ptrToMessage&)), this, SLOT(updateNewViconData(const ptrToMessage&)));
QObject::connect(cf_linker, SIGNAL(updateComboBoxes()), this, SLOT(updateComboBoxes())); QObject::connect(cf_linker, SIGNAL(updateComboBoxes()), this, SLOT(updateComboBoxes()));
ros::NodeHandle nodeHandle("~");
DBChangedPublisher = nodeHandle.advertise<std_msgs::Int32>("DBChanged", 1);
#endif #endif
} }
...@@ -756,6 +763,11 @@ void MainGUIWindow::on_save_in_DB_button_clicked() ...@@ -756,6 +763,11 @@ void MainGUIWindow::on_save_in_DB_button_clicked()
// save the database in the file // save the database in the file
fill_database_file(); fill_database_file();
// Now also publish a ROS message stating that we changed the DB, so the nodes can update it
std_msgs::Int32 msg;
msg.data = 1;
this->DBChangedPublisher.publish(msg);
} }
void MainGUIWindow::clear_database_file() void MainGUIWindow::clear_database_file()
......
...@@ -81,6 +81,8 @@ private: ...@@ -81,6 +81,8 @@ private:
ros::Publisher setpointPublisher; ros::Publisher setpointPublisher;
ros::Subscriber setpointSubscriber; ros::Subscriber setpointSubscriber;
ros::Subscriber DBChangedSubscriber;
ros::ServiceClient centralManager; ros::ServiceClient centralManager;
// callbacks // callbacks
...@@ -88,6 +90,7 @@ private: ...@@ -88,6 +90,7 @@ private:
void CFBatteryCallback(const std_msgs::Float32& msg); void CFBatteryCallback(const std_msgs::Float32& msg);
void flyingStateChangedCallback(const std_msgs::Int32& msg); void flyingStateChangedCallback(const std_msgs::Int32& msg);
void setpointCallback(const Setpoint& newSetpoint); void setpointCallback(const Setpoint& newSetpoint);
void DBChangedCallback(const std_msgs::Int32& msg);
float fromVoltageToPercent(float voltage); float fromVoltageToPercent(float voltage);
void updateBatteryVoltage(float battery_voltage); void updateBatteryVoltage(float battery_voltage);
......
...@@ -39,6 +39,7 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) : ...@@ -39,6 +39,7 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
setpointPublisher = nodeHandle.advertise<Setpoint>("SafeControllerService/Setpoint", 1); setpointPublisher = nodeHandle.advertise<Setpoint>("SafeControllerService/Setpoint", 1);
setpointSubscriber = nodeHandle.subscribe("SafeControllerService/Setpoint", 1, &MainWindow::setpointCallback, this); setpointSubscriber = nodeHandle.subscribe("SafeControllerService/Setpoint", 1, &MainWindow::setpointCallback, this);
DBChangedSubscriber = nodeHandle.subscribe("my_GUI/DBChanged", 1, &MainWindow::DBChangedCallback, this);
// communication with PPS Client, just to make it possible to communicate through terminal also we use PPSClient's name // communication with PPS Client, just to make it possible to communicate through terminal also we use PPSClient's name
...@@ -84,6 +85,11 @@ void MainWindow::enableGUI() ...@@ -84,6 +85,11 @@ void MainWindow::enableGUI()
ui->groupBox->setEnabled(true); ui->groupBox->setEnabled(true);
} }
void MainWindow::DBChangedCallback(const std_msgs::Int32& msg)
{
loadCrazyflieContext();
}
void MainWindow::setpointCallback(const Setpoint& newSetpoint) void MainWindow::setpointCallback(const Setpoint& newSetpoint)
{ {
m_setpoint = newSetpoint; m_setpoint = newSetpoint;
......
<?xml version="1.0" encoding="UTF-8"?> <?xml version="1.0" encoding="UTF-8"?>
<!DOCTYPE QtCreatorProject> <!DOCTYPE QtCreatorProject>
<!-- Written by QtCreator 4.0.2, 2017-08-28T12:32:50. --> <!-- Written by QtCreator 4.0.2, 2017-08-28T18:07:37. -->
<qtcreator> <qtcreator>
<data> <data>
<variable>EnvironmentId</variable> <variable>EnvironmentId</variable>
......
...@@ -423,6 +423,11 @@ void commandCallback(const std_msgs::Int32& commandMsg) { ...@@ -423,6 +423,11 @@ void commandCallback(const std_msgs::Int32& commandMsg) {
} }
} }
void DBChangedCallback(const std_msgs::Int32& msg)
{
loadCrazyflieContext();
}
int main(int argc, char* argv[]) int main(int argc, char* argv[])
{ {
ros::init(argc, argv, "PPSClient"); ros::init(argc, argv, "PPSClient");
...@@ -449,6 +454,9 @@ int main(int argc, char* argv[]) ...@@ -449,6 +454,9 @@ int main(int argc, char* argv[])
// this topic will publish flying state whenever it changes. // this topic will publish flying state whenever it changes.
flyingStatePublisher = nodeHandle.advertise<std_msgs::Int32>("flyingState", 1); flyingStatePublisher = nodeHandle.advertise<std_msgs::Int32>("flyingState", 1);
// publish first flying state data // publish first flying state data
std_msgs::Int32 flying_state_msg; std_msgs::Int32 flying_state_msg;
flying_state_msg.data = flying_state; flying_state_msg.data = flying_state;
...@@ -458,6 +466,9 @@ int main(int argc, char* argv[]) ...@@ -458,6 +466,9 @@ int main(int argc, char* argv[])
ros::NodeHandle namespaceNodeHandle = ros::NodeHandle(); ros::NodeHandle namespaceNodeHandle = ros::NodeHandle();
safeControllerServiceSetpointPublisher = namespaceNodeHandle.advertise<d_fall_pps::Setpoint>("SafeControllerService/Setpoint", 1); safeControllerServiceSetpointPublisher = namespaceNodeHandle.advertise<d_fall_pps::Setpoint>("SafeControllerService/Setpoint", 1);
// subscriber for DBChanged
ros::Subscriber DBChangedSubscriber = namespaceNodeHandle.subscribe("my_GUI/DBChanged", 1, DBChangedCallback);
//start with safe controller //start with safe controller
flying_state = STATE_MOTORS_OFF; flying_state = STATE_MOTORS_OFF;
usingSafeController = true; usingSafeController = true;
......
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